public double getRPM() {
if (Timer.getFPGATimestamp() > RPMUpdatePeriod + encoderMarkTime) {
double encoderCPS =
(enc.get() - encoderMarkCounts) / (Timer.getFPGATimestamp() - encoderMarkTime);
encoderRPM = (encoderCPS * 60) / 16;
encoderMarkCounts = enc.get();
encoderMarkTime = Timer.getFPGATimestamp();
}
return encoderRPM;
}
/**
* Sets the speed higher or lower (by 0.05) depending on V Bus.
*
* @param speed - The speed of the shooter from the V Bus.
*/
public void setSpeedReliablyVBus(double speed) {
double currentSpeed = speed;
double startTime = Timer.getFPGATimestamp();
setSpeed(currentSpeed);
while (Math.abs(getCurrentMotorSpeedInRPM() - speed * SHOOTER_ENCODER_MAXSPEED) < 150.0) {
if (getCurrentMotorSpeedInRPM() - speed * SHOOTER_ENCODER_MAXSPEED > 0.0) {
currentSpeed -= 0.05;
} else {
currentSpeed += 0.05;
}
setSpeed(currentSpeed);
if (Timer.getFPGATimestamp() - startTime > 2000.0) {
return;
}
}
}
public class I2CGyro {
private I2C gyro = new I2C(I2C.Port.kOnboard, 0x68);
private double degrees = 0;
private double timerStart = Timer.getFPGATimestamp();
public I2CGyro() {
gyro.write(0x6B, 0x03); // Power
gyro.write(0x1A, 0x18); // Basic Config
gyro.write(0x1B, 0x00); // Gyro Config
}
public void Init() {
timerStart = Timer.getFPGATimestamp();
degrees = 0.0;
}
public double getDegrees() {
double timerDelta = Timer.getFPGATimestamp() - timerStart;
byte[] angle = new byte[1];
gyro.read(0x47, 1, angle);
double rotation = (angle[0] * timerDelta) * 2;
degrees += rotation;
return -degrees;
}
}
public TargetFinder() {
System.out.println("TargetFinder() begin " + Timer.getFPGATimestamp());
cam = AxisCamera.getInstance();
cam.writeResolution(AxisCamera.ResolutionT.k320x240);
cam.writeBrightness(camBrightness);
cam.writeColorLevel(camColor);
cam.writeWhiteBalance(camWhiteBalance);
cam.writeExposureControl(camExposure);
cam.writeMaxFPS(15);
cam.writeExposurePriority(AxisCamera.ExposurePriorityT.none);
cam.writeCompression(50);
// System.out.println("TargetFinder() * " + cam.toString() + "[" + Timer.getFPGATimestamp());
boxCriteria = new CriteriaCollection();
inertiaCriteria = new CriteriaCollection();
boxCriteria.addCriteria(
NIVision.MeasurementType.IMAQ_MT_BOUNDING_RECT_WIDTH, 30, bboxWidthMin, false);
boxCriteria.addCriteria(
NIVision.MeasurementType.IMAQ_MT_BOUNDING_RECT_HEIGHT, 40, bboxHeightMin, false);
inertiaCriteria.addCriteria(
NIVision.MeasurementType.IMAQ_MT_NORM_MOMENT_OF_INERTIA_XX, 0, inertiaXMin, true);
inertiaCriteria.addCriteria(
NIVision.MeasurementType.IMAQ_MT_NORM_MOMENT_OF_INERTIA_YY, 0, inertiaYMin, true);
Timer.delay(7);
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
if (timeFinal <= Timer.getFPGATimestamp()) {
return true;
} else {
return false;
}
}
@Override
public void update(Object param) {
int reading;
try {
reading = mGyroInterface.getReading();
} catch (GyroInterface.GyroException e) {
System.out.println("Gyro read failed: " + e.getMessage());
return;
}
GyroInterface.StatusFlag status = GyroInterface.extractStatus(reading);
List<GyroInterface.ErrorFlag> errors = GyroInterface.extractErrors(reading);
if (GyroInterface.StatusFlag.VALID_DATA != status || !errors.isEmpty()) {
System.out.println(
"Gyro read failed. Status: " + status + ". Errors: " + Util.joinStrings(", ", errors));
return;
}
if (mRemainingStartupCycles > 0) {
mRemainingStartupCycles--;
return;
}
if (mVolatileShouldReZero) {
mVolatileShouldReZero = false;
mVolatileHasData = false;
mIsZerod = false;
}
double unbiasedAngleRate = GyroInterface.extractAngleRate(reading);
mZeroRateSamples[mZeroRateSampleIndex] = unbiasedAngleRate;
mZeroRateSampleIndex++;
if (mZeroRateSampleIndex >= K_ZEROING_SAMPLES) {
mZeroRateSampleIndex = 0;
mHasEnoughZeroingSamples = true;
}
if (!mIsZerod) {
if (!mHasEnoughZeroingSamples) {
return;
}
mZeroBias = 0;
for (Double sample : mZeroRateSamples) {
mZeroBias += sample / K_ZEROING_SAMPLES;
}
mAngle = 0;
mVolatileAngle = 0;
GyroThread.this.reset();
mIsZerod = true;
return;
}
double now = edu.wpi.first.wpilibj.Timer.getFPGATimestamp();
double timeElapsed = mLastTime == 0 ? 1.0 / K_READING_RATE : now - mLastTime;
mLastTime = now;
mVolatileRate = unbiasedAngleRate - mZeroBias;
mAngle += mVolatileRate * timeElapsed;
mVolatileAngle = mAngle;
mVolatileHasData = true;
}
protected void updateYawHistory(float curr_yaw) {
if (next_yaw_history_index >= YAW_HISTORY_LENGTH) {
next_yaw_history_index = 0;
}
yaw_history[next_yaw_history_index] = curr_yaw;
last_update_time = Timer.getFPGATimestamp();
next_yaw_history_index++;
}
public void updateAcel() {
double timestamp = Timer.getFPGATimestamp();
double deltaTime = timestamp - lastTimestamp;
acceleration = new Vector2(xFilter.pidGet(), yFilter.pidGet());
velocity =
Vector2.add(
velocity, Vector2.rotate(Vector2.scale(acceleration, deltaTime), -getHeading()));
position = Vector2.add(position, Vector2.scale(velocity, deltaTime));
lastTimestamp = timestamp;
}
public double getDegrees() {
double timerDelta = Timer.getFPGATimestamp() - timerStart;
byte[] angle = new byte[1];
gyro.read(0x47, 1, angle);
double rotation = (angle[0] * timerDelta) * 2;
degrees += rotation;
return -degrees;
}
public void tankDrive(Joystick left, Joystick right) {
if (left.getRawButton(1)) {
if (turbo == false) {
startTime = Timer.getFPGATimestamp();
turbo = true;
} else if (Timer.getFPGATimestamp() - startTime < 0.25) {
drive.tankDrive(
left.getY() / (2 - 4 * (Timer.getFPGATimestamp() - startTime)),
right.getY() / (2 - 4 * (Timer.getFPGATimestamp() - startTime)));
// System.out.println("start: " + startTime + " time: " + Timer.getFPGATimestamp() + "
// scale: " + (2 - 4*(startTime - Timer.getFPGATimestamp())));
} else {
drive.tankDrive(left, right);
}
} else {
turbo = false;
drive.tankDrive(left.getY() / 2, right.getY() / 2);
}
}
public void run() {
SmartDashboard.putString("Status", "Started");
double lastTime = Timer.getFPGATimestamp();
double time = lastTime;
double deltaTime;
while (!Thread.interrupted()) {
time = Timer.getFPGATimestamp();
deltaTime = time - lastTime;
acceleration = new Vector2(xFilter.pidGet(), yFilter.pidGet());
velocity = Vector2.add(velocity, Vector2.scale(acceleration, deltaTime));
position =
Vector2.add(position, Vector2.rotate(Vector2.scale(velocity, deltaTime), -getHeading()));
lastTime = time;
try {
Thread.sleep(20);
} catch (InterruptedException e) {
break;
}
}
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
/*
if(Robot.globalVariables.STACKOUT==false){
return true;
}
else{
return false;
}*/
if ((startTime + 3) <= Timer.getFPGATimestamp()) {
return true;
} else {
return false;
}
}
public void calibrateAcel() {
double avgX = 0;
double avgY = 0;
for (int i = 0; i < calibrationSamples; i++) {
avgX += acel.getX();
avgY += acel.getY();
Timer.delay(0.005); // calibrationSampleRate / 1000.0);
}
avgX /= calibrationSamples;
avgY /= calibrationSamples;
acelBias = new Vector2(avgX, avgY);
lastTimestamp = Timer.getFPGATimestamp();
// SmartDashboard.putNumber("X Bias", acelBias.x);
// SmartDashboard.putNumber("Y Bias", acelBias.y);
}
/**
* Constructs a PID Thread. Each PID thread is an implementation of the Runnable interface. PID
* Threads can be created or stopped anytime, and can also be enabled/disabled ("Paused") when not
* in use. To use a PID Thread, you'll need 3 PID-specific objects - 2 PIDSubsystems and a
* PIDCommand. PIDSubsystem and PIDCommand are interfaces that your subsystems and commands should
* implement if they are to use PID. If you're looking for more information about how PID works,
* please refer to this introductory piece: http://www.societyofrobots.com/programming_PID.shtml
*
* @param kP - the Proportional parameter
* @param kI - the integral parameter
* @param kD - the derivative parameter
* @param tolerance - error considered 0 if error within +- this value
* @param dt - thread refresh rate (5ms recommended)
* @param marker - VERY IMPORTANT - each new PID thread must have its own PID marker (0 through 3
* reserved for PID Drive)
*/
public PIDThread(double kP, double kI, double kD, double tolerance, long dt, int marker) {
this.kP = kP;
this.kI = kI;
this.kD = kP;
this.tolerance = tolerance;
this.setpoint = 0;
this.currentMeasurement = 0;
this.PIDinput = null;
this.PIDOutput = null;
this.PIDSetpoint = null;
this.dt = dt;
this.marker = marker;
enabled = true;
previous = (long) Timer.getFPGATimestamp() * Math.pow(10, -3);
current = 0;
dtMeasured = 0;
}
public void run() {
while (m_run) {
if (m_raspberryPi.isEnabled()) { // Checks for Thread to run
if (m_raspberryPi.isConnected()) {
report = true;
try {
String[] data =
m_raspberryPi.tokenizeData(m_raspberryPi.getRawData()); // Get and examine Data
time = Timer.getFPGATimestamp();
if (data.length < 2) { // Error Check
report = false;
} else {
try {
distance = Integer.parseInt(data[3]); // Get data
offset = Integer.parseInt(data[0]);
report = true;
} catch (NumberFormatException ex) {
report = false;
}
}
} catch (IOException ex) {
report = false;
}
DataKeeper.setReport(m_connected);
if (report) { // Store Data in DataKeeper
DataKeeper.setDistance(distance);
DataKeeper.setOffset(offset);
DataKeeper.setTime(time);
}
} else {
try {
m_raspberryPi.connect();
} catch (IOException ex) {
m_connected = false;
DataKeeper.setReport(m_connected);
}
}
}
try {
Thread.sleep(375); // Wait half second before getting Data again
} catch (InterruptedException ex) {
}
}
}
/** Called when the thread the PIDThread is passed into is started (myThread.start()) */
public void run() {
// TODO add sensor failure detection to abort loop and return to manual/variable voltage control
while (true) {
// skip PID if the thread is disabled
if (enabled) {
current = (long) Timer.getFPGATimestamp() * Math.pow(10, -3);
dtMeasured = (long) (current - previous);
if (dtMeasured == 0) {
dtMeasured = 5;
}
// Core PID Code follows:
// setpoint get the setpoint from the PIDSetpoint passed in (use this thread's marker)
// currentMeasurement gets the current reading from the PIDinput (use this thread's marker)
setpoint = PIDSetpoint.getPIDSetpoint(marker);
currentMeasurement = PIDinput.getPIDSource(marker);
// PID Calculation (includes tolerance adjustment)
double error = (setpoint) - (currentMeasurement);
error = toleranceAdjustment(error);
integral = integral + (error * dtMeasured);
double derivative = (error - previousError) / dtMeasured;
output = (kP * error) + (kI * integral) + (kD * derivative);
previousError = error;
// set the PIDoutput to the generated output (again, use the specific marker to prevent
// cross-thread data transmission, ex. left front encoder reading used in right front PID)
PIDOutput.setPIDOutput(output, marker);
PIDSetpoint.updateError(error, marker);
previous = current;
}
// if PID disabled, just set output to 0 and 0 integral.
// IMPORTANT - if switching to manual control, implement a "disregard all data" catch in the
// setPIDOutput() method of your PIDOutput object to prevent 0 movement from actuator.
else {
PIDOutput.setPIDOutput(0.0, marker);
integral = 0;
}
// thread frequency adjustment. catch statement can also be used to trigger events
try {
Thread.sleep(dt);
} catch (InterruptedException e) {
System.out.println("PIDThread #" + marker + "interupted");
}
}
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
// if auto drive has been overridden, this command completes
if (Robot.drive.stopAuto()) {
System.out.println("Auto Turn STOPPED");
return true;
}
if ((Timer.getFPGATimestamp() - startTime) > COMMAND_TIMEOUT) {
System.out.println("ArmShoulderMoveToAngle.isFinished(); - COMMAND_TIMEOUT");
return true;
}
return true;
//
// if(Math.abs((Robot.navigation.getHeading() - targetHeading)%360) < HEADING_TOLERANCE)
// {
// return true;
// }
// else
// {
// return false;
// }
}
public boolean processImage() {
boolean debugWriteImages = true;
boolean success = cam.freshImage();
if (success) {
try {
System.out.println("In Try loop");
ColorImage im = cam.getImage();
System.out.println("Got image");
if (debugWriteImages) {
im.write("image1.jpg");
System.out.println("Wrote color image");
}
BinaryImage thresholdIm =
im.thresholdRGB(redLow, redHigh, greenLow, greenHigh, blueLow, blueHigh);
if (debugWriteImages) {
thresholdIm.write("image2.jpg");
System.err.println("Wrote Threshold Image");
}
BinaryImage filteredBoxIm = thresholdIm.particleFilter(boxCriteria);
ParticleAnalysisReport[] xparticles = filteredBoxIm.getOrderedParticleAnalysisReports();
System.out.println(xparticles.length + " particles at " + Timer.getFPGATimestamp());
BinaryImage filteredInertiaIm = filteredBoxIm.particleFilter(inertiaCriteria);
ParticleAnalysisReport[] particles = filteredInertiaIm.getOrderedParticleAnalysisReports();
System.out.println(particles.length + " particles at " + Timer.getFPGATimestamp());
// Loop through targets, find highest one.
// Targets aren't found yet.
highTarget = Target.NullTarget;
target1 = Target.NullTarget;
target2 = Target.NullTarget;
target3 = Target.NullTarget;
target4 = Target.NullTarget;
System.out.println("Targets created");
double minY = IMAGE_HEIGHT; // Minimum y <-> higher in image.
for (int i = 0; i < particles.length; i++) {
Target t = new Target(i, particles[i]);
if (t.ratio > ratioMin && t.ratio < ratioMax) {
addTarget(t);
if (t.centerY <= minY) {
highTarget = t;
}
}
System.out.println(
"Target "
+ i
+ ": ("
+ t.centerX
+ ","
+ t.centerY
+ ") Distance: "
+ getDistance(t));
}
System.out.println("Best target: " + highTarget.index);
System.out.println("Distance to the target: " + getDistance(highTarget));
if (debugWriteImages) {
filteredBoxIm.write("image3.jpg");
filteredInertiaIm.write("image4.jpg");
System.out.println("Wrote Images");
}
// Free memory from images.
im.free();
thresholdIm.free();
filteredBoxIm.free();
filteredInertiaIm.free();
} catch (AxisCameraException ex) {
System.out.println("Axis Camera Exception Gotten" + ex.getMessage());
ex.printStackTrace();
} catch (NIVisionException ex) {
System.out.println("NIVision Exception Gotten - " + ex.getMessage());
ex.printStackTrace();
}
}
return success;
}
public void Init() {
timerStart = Timer.getFPGATimestamp();
degrees = 0.0;
}
// Called just before this Command runs the first time
protected void initialize() {
Robot.drive.setBrakeModeOn(true);
Robot.drive.stopAuto(false);
System.out.println("AutoDriveTurnToHeading.initialize()");
startTime = Timer.getFPGATimestamp();
}
@Override
protected boolean isFinished() {
return startTime + time < Timer.getFPGATimestamp();
}
@Override
protected void initialize() {
startTime = Timer.getFPGATimestamp();
}
// Called just before this Command runs the first time
protected void initialize() {
timeFinal = Timer.getFPGATimestamp() + seconds;
}
public void teleopPeriodic() {
/*SmartDashboard.getNumber("Accel x: ", accel.getX());
SmartDashboard.getNumber("Accel Y: ", accel.getY());
SmartDashboard.getNumber("Accel Z: ", accel.getZ());*/
if (controller.isEnabled()) {
if (((Timer.getFPGATimestamp() - timerStart) > timeToCancel) || controller.onTarget()) {
controller.disable();
timerStart = -1;
timeToCancel = -1;
}
}
if (j1.getRawButton(2)) {
enc.reset();
controller.setSetpoint(120); // destination 24 inches -> NO!! Trying to figure out this value
timerStart = Timer.getFPGATimestamp();
timeToCancel = 10; // timeout after 10 seconds
controller.enable();
} else if (j1.getRawButton(
1)) { // this button stops the robot if the button 2 command goes crazy
controller.disable();
timerStart = -1;
timeToCancel = -1;
} else { // if time out or distance, end
controller.disable();
timerStart = -1;
timeToCancel = -1;
}
if (!controller.isEnabled()) {
rd.arcadeDrive(-j1.getY(), -j1.getX()); // normal arcade drive
}
if (j2.getRawAxis(2) != 0) { // set shooter values to the left trigger value
shooter.set(-j2.getRawAxis(2));
shooter2.set(-j2.getRawAxis(2));
SmartDashboard.putNumber("Shooter: ", shooter.get());
} else { // stop shooter
shooter.set(0);
shooter2.set(0);
}
/*if(j2.getRawButton(8)){ //runs intake, waits, runs shooter
shooter.set(-1);
intake.set(.5);
Timer.delay(.50);
intake.set(0);
Timer.delay(1.5);
intake.set(-1);
Timer.delay(1);
shooter.set(0);
intake.set(0);
}*/
// Need to mess around with sensitivity of light sensor
if (j2.getRawAxis(3) != 0 && !light.get()) { // run intake at speed of right trigger
intake.set(-1 * j2.getRawAxis(3));
} else if (j2.getRawButton(5) && !light.get()) { // run intake into robot
intake.set(-1);
} else if (j2.getRawButton(6)) { // run intake out of robot
intake.set(1);
} else { // stop intake
intake.set(0);
}
if (j2.getRawButton(2)) { // lift intake
inup.set(DoubleSolenoid.Value.kForward);
inup2.set(DoubleSolenoid.Value.kForward);
} else if (j2.getRawButton(3)) { // drop intake
inup.set(DoubleSolenoid.Value.kReverse);
inup2.set(DoubleSolenoid.Value.kReverse);
} else { // solenoids off
inup.set(DoubleSolenoid.Value.kOff);
inup2.set(DoubleSolenoid.Value.kOff);
}
// reading values
SmartDashboard.putNumber("Encoder Dist: ", enc.getDistance()); // Distance is in inches
SmartDashboard.putNumber("Encoder: ", enc.get());
SmartDashboard.putNumber("Encoder Rate: ", enc.getRate());
SmartDashboard.putNumber("Gyro: ", gyro.getAngle());
SmartDashboard.putNumber("Encoder Raw", enc.getRaw());
SmartDashboard.putNumber("Controller: ", controller.getSetpoint());
SmartDashboard.putBoolean("Light Sensor: ", light.get());
}
public void run() {
stop = false;
boolean stream_response_received = false;
double last_valid_packet_time = 0.0;
int partial_binary_packet_count = 0;
int stream_response_receive_count = 0;
int timeout_count = 0;
int discarded_bytes_count = 0;
int port_reset_count = 0;
double last_stream_command_sent_timestamp = 0.0;
int updates_in_last_second = 0;
double last_second_start_time = 0;
try {
serial_port.setReadBufferSize(512);
serial_port.setTimeout(1.0);
serial_port.enableTermination('\n');
serial_port.flush();
serial_port.reset();
} catch (RuntimeException ex) {
ex.printStackTrace();
}
IMUProtocol.StreamResponse response = new IMUProtocol.StreamResponse();
byte[] stream_command = new byte[256];
int cmd_packet_length =
IMUProtocol.encodeStreamCommand(stream_command, update_type, update_rate_hz);
try {
serial_port.reset();
serial_port.write(stream_command, cmd_packet_length);
serial_port.flush();
port_reset_count++;
// SmartDashboard.putNumber("nav6_PortResets", (double)port_reset_count);
last_stream_command_sent_timestamp = Timer.getFPGATimestamp();
} catch (RuntimeException ex) {
ex.printStackTrace();
}
while (!stop) {
try {
// Wait, with delays to conserve CPU resources, until
// bytes have arrived.
while (!stop && (serial_port.getBytesReceived() < 1)) {
Timer.delay(1.0 / update_rate_hz);
}
int packets_received = 0;
byte[] received_data = serial_port.read(256);
int bytes_read = received_data.length;
if (bytes_read > 0) {
byte_count += bytes_read;
int i = 0;
// Scan the buffer looking for valid packets
while (i < bytes_read) {
// Attempt to decode a packet
int bytes_remaining = bytes_read - i;
if (received_data[i] != IMUProtocol.PACKET_START_CHAR) {
/* Skip over received bytes until a packet start is detected. */
i++;
discarded_bytes_count++;
// SmartDashboard.putNumber("nav6 Discarded Bytes", (double)discarded_bytes_count);
continue;
} else {
if ((bytes_remaining > 2) && (received_data[i + 1] == (byte) '#')) {
/* Binary packet received; next byte is packet length-2 */
byte total_expected_binary_data_bytes = received_data[i + 2];
total_expected_binary_data_bytes += 2;
while (bytes_remaining < total_expected_binary_data_bytes) {
/* This binary packet contains an embedded */
/* end-of-line character. Continue to receive */
/* more data until entire packet is received. */
byte[] additional_received_data = serial_port.read(256);
byte_count += additional_received_data.length;
bytes_remaining += additional_received_data.length;
/* Resize array to hold existing and new data */
byte[] c = new byte[received_data.length + additional_received_data.length];
if (c.length > 0) {
System.arraycopy(received_data, 0, c, 0, received_data.length);
System.arraycopy(
additional_received_data,
0,
c,
received_data.length,
additional_received_data.length);
received_data = c;
} else {
/* Timeout waiting for remainder of binary packet */
i++;
bytes_remaining--;
partial_binary_packet_count++;
// SmartDashboard.putNumber("nav6 Partial Binary Packets",
// (double)partial_binary_packet_count);
continue;
}
}
}
}
int packet_length = decodePacketHandler(received_data, i, bytes_remaining);
if (packet_length > 0) {
packets_received++;
update_count++;
last_valid_packet_time = Timer.getFPGATimestamp();
updates_in_last_second++;
if ((last_valid_packet_time - last_second_start_time) > 1.0) {
// SmartDashboard.putNumber("nav6 UpdatesPerSec", (double)updates_in_last_second);
updates_in_last_second = 0;
last_second_start_time = last_valid_packet_time;
}
i += packet_length;
} else {
packet_length =
IMUProtocol.decodeStreamResponse(received_data, i, bytes_remaining, response);
if (packet_length > 0) {
packets_received++;
setStreamResponse(response);
stream_response_received = true;
i += packet_length;
stream_response_receive_count++;
// SmartDashboard.putNumber("nav6 Stream Responses",
// (double)stream_response_receive_count);
} else {
// current index is not the start of a valid packet; increment
i++;
}
}
}
if ((packets_received == 0) && (bytes_read == 256)) {
// Workaround for issue found in SerialPort implementation:
// No packets received and 256 bytes received; this
// condition occurs in the SerialPort. In this case,
// reset the serial port.
serial_port.reset();
port_reset_count++;
// SmartDashboard.putNumber("nav6_PortResets", (double)port_reset_count);
}
// If a stream configuration response has not been received within three seconds
// of operation, (re)send a stream configuration request
if (!stream_response_received
&& ((Timer.getFPGATimestamp() - last_stream_command_sent_timestamp) > 3.0)) {
cmd_packet_length =
IMUProtocol.encodeStreamCommand(stream_command, update_type, update_rate_hz);
try {
last_stream_command_sent_timestamp = Timer.getFPGATimestamp();
serial_port.write(stream_command, cmd_packet_length);
serial_port.flush();
} catch (RuntimeException ex2) {
ex2.printStackTrace();
}
} else {
// If no bytes remain in the buffer, and not awaiting a response, sleep a bit
if (stream_response_received && (serial_port.getBytesReceived() == 0)) {
Timer.delay(1.0 / update_rate_hz);
}
}
/* If receiving data, but no valid packets have been received in the last second */
/* the navX MXP may have been reset, but no exception has been detected. */
/* In this case , trigger transmission of a new stream_command, to ensure the */
/* streaming packet type is configured correctly. */
if ((Timer.getFPGATimestamp() - last_valid_packet_time) > 1.0) {
stream_response_received = false;
}
}
} catch (RuntimeException ex) {
// This exception typically indicates a Timeout
stream_response_received = false;
timeout_count++;
// SmartDashboard.putNumber("nav6 Serial Port Timeouts", (double)timeout_count);
// SmartDashboard.putString("LastNavExceptionBacktrace",ex.getStackTrace().toString());
// SmartDashboard.putString("LastNavException", ex.getMessage() + "; " + ex.toString());
}
}
}
/**
* Indicates whether the nav6 IMU is currently connected to the host computer. A connection is
* considered established whenever a value update packet has been received from the nav6 IMU
* within the last second.
*
* @return Returns true if a valid update has been received within the last second.
*/
public boolean isConnected() {
double time_since_last_update = Timer.getFPGATimestamp() - this.last_update_time;
return time_since_last_update <= 1.0;
}