@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. */
}
}
public void DriveOnHeadingReverse(
navXPIDController.PIDResult yawPIDResult, float heading, float distanceInches) {
// calculate encoder counts for distance
float wheelDiameter = 4; // inches
float wheelCirc = wheelDiameter * (float) 3.14159;
float encoderTicksPerRotation = 1120;
float ticksPerInch = encoderTicksPerRotation / wheelCirc;
int ticksToTravel = (int) (distanceInches * ticksPerInch);
// check motor position
int startEncCount = robot.leftfrontMotor.getCurrentPosition();
int DEVICE_TIMEOUT_MS = 500;
/* Drive straight forward at 1/2 of full drive speed */
double drive_speed = 0.5;
yawPIDController.setPID(0.05, YAW_PID_I, YAW_PID_D);
yawPIDController.setSetpoint(heading);
DecimalFormat df = new DecimalFormat("#.##");
try {
while ((robot.leftfrontMotor.getCurrentPosition() - startEncCount) > -ticksToTravel
&& !Thread.currentThread().isInterrupted()) {
if (yawPIDController.waitForNewUpdate(yawPIDResult, DEVICE_TIMEOUT_MS)) {
if (yawPIDResult.isOnTarget()) {
robot.Drive(-drive_speed, -drive_speed);
telemetry.addData("PIDOutput", df.format(drive_speed) + ", " + df.format(drive_speed));
} else {
double output = yawPIDResult.getOutput();
if (output < -0.5) {
output = -0.5;
}
if (output > 0.5) {
output = 0.5;
}
robot.Drive(-drive_speed + output, -drive_speed - output);
telemetry.addData(
"PIDOutput",
df.format(limit(-drive_speed - output))
+ ", "
+ df.format(limit(-drive_speed + output)));
}
telemetry.addData("Yaw", df.format(navx_device.getYaw()));
telemetry.addData("Output", df.format(yawPIDResult.getOutput()));
telemetry.addData("RevEnc:", robot.leftfrontMotor.getCurrentPosition());
telemetry.addData("EncStart:", startEncCount);
} else {
/* A timeout occurred */
Log.w("navXDriveStraightOp", "Yaw PID waitForNewUpdate() TIMEOUT.");
}
}
} catch (InterruptedException ex) {
Thread.currentThread().interrupt();
}
}
public void TurnToHeading(
navXPIDController.PIDResult yawPIDResult, double heading, double maxTimeSeconds) {
try {
long starttime = System.currentTimeMillis();
TARGET_ANGLE_DEGREES = heading;
int DEVICE_TIMEOUT_MS = 500;
yawPIDController.setPID(0.02, YAW_PID_I, YAW_PID_D);
yawPIDController.setSetpoint(TARGET_ANGLE_DEGREES);
DecimalFormat df = new DecimalFormat("#.##");
while (!Thread.currentThread().isInterrupted()
&& (System.currentTimeMillis() - starttime) < (long) (maxTimeSeconds * 1000)) {
if (yawPIDController.waitForNewUpdate(yawPIDResult, DEVICE_TIMEOUT_MS)) {
if (yawPIDResult.isOnTarget()) {
float zero = 0;
robot.FloatMotors();
telemetry.addData("PIDOutput", df.format(0.00));
telemetry.update();
} else {
double output = yawPIDResult.getOutput();
if (output > 0 && output < 0.25) {
output = 0.25;
}
if (output < 0 && output > -0.25) {
output = -0.25;
}
robot.Drive(output, -output);
telemetry.addData("PIDOutput", df.format(output) + ", " + df.format(-output));
}
} else {
/* A timeout occurred */
Log.w("navXRotateOp", "Yaw PID waitForNewUpdate() TIMEOUT.");
telemetry.addData("timeout", "timeout occured");
}
telemetry.addData("Yaw aw", df.format(navx_device.getYaw()));
telemetry.update();
}
} catch (InterruptedException ex) {
Thread.currentThread().interrupt();
}
}
@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()));
}
}