@Override
public void loop() {
if (!calibration_complete) {
/* navX-Micro Calibration completes automatically ~15 seconds after it is
powered on, as long as the device is still. To handle the case where the
navX-Micro has not been able to calibrate successfully, hold off using
the navX-Micro Yaw value until calibration is complete.
*/
calibration_complete = !navx_device.isCalibrating();
if (calibration_complete) {
navx_device.zeroYaw();
} else {
telemetry.addData("navX-Micro", "Startup Calibration in Progress");
}
} else {
/* Wait for new Yaw PID output values, then update the motors
with the new PID value with each new output value.
*/
if (yawPIDController.isNewUpdateAvailable(yawPIDResult)) {
if (yawPIDResult.isOnTarget()) {
leftMotor.setPowerFloat();
rightMotor.setPowerFloat();
telemetry.addData("Motor Output", df.format(0.00));
} else {
double output = yawPIDResult.getOutput();
leftMotor.setPower(output);
rightMotor.setPower(-output);
telemetry.addData("Motor Output", df.format(output) + ", " + df.format(-output));
}
} else {
/* No sensor update has been received since the last time */
/* the loop() function was invoked. Therefore, there's no */
/* need to update the motors at this time. */
}
telemetry.addData("Yaw", df.format(navx_device.getYaw()));
}
}
@Override
public void setPowerFloat() {
dcMotor.setPowerFloat();
}