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();
}
}