@Override
public void loop() {
/* reset the navX-Model device yaw angle so that whatever direction */
/* it is currently pointing will be zero degrees. */
if (first_iteration) {
first_iteration = false;
navx_device.zeroYaw();
yawPIDResult = new navXPIDController.PIDResult();
}
/* Wait for new Yaw PID output values, then update the motors
with the new PID value with each new output value.
*/
/* Drive straight forward at 1/2 of full drive speed */
double drive_speed = 0.5;
if (yawPIDController.isNewUpdateAvailable(yawPIDResult)) {
if (yawPIDResult.isOnTarget()) {
leftMotor.setPower(drive_speed);
rightMotor.setPower(drive_speed);
} else {
double output = yawPIDResult.getOutput();
leftMotor.setPower(drive_speed + output);
rightMotor.setPower(drive_speed - 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. */
}
}
@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()));
}
}