/** Update steering calibration control */
private void updateCalibrateControl() {
double stickAngle =
driverstation.getStickAngle(driverstation.JoystickDriverX, driverstation.JoystickDriverY);
// Branch into motor being calibrated
if (driverstation.getStickDistance(driverstation.JoystickDriverX, driverstation.JoystickDriverY)
> 0.5) {
if (stickAngle < 0) {
if (stickAngle < -0.5) {
motorId = RobotMap.BACK_LEFT;
} else {
motorId = RobotMap.FRONT_LEFT;
}
} else {
if (stickAngle > 0) {
if (stickAngle > 0.5) {
motorId = RobotMap.BACK_RIGHT;
} else {
motorId = RobotMap.FRONT_RIGHT;
}
}
}
}
// Prints selected motor to the driverstation
printSelectedMotor();
// Determine calibration mode
if (driverstation.JoystickDriverButton3) {
Calibration.stopWheelCalibrate();
steerMode = true;
}
if (driverstation.JoystickDriverButton4 && button4Debounce) {
Calibration.toggleWheelCalibrate();
steerMode = false;
button4Debounce = false;
}
if (!driverstation.JoystickDriverButton4) {
button4Debounce = true;
}
// Branch into type of calibration
if (steerMode) {
driverstation.println("Steering Calibrate", 3);
Calibration.updateSteeringCalibrate(motorId);
System.out.println(drive.getSteeringAngle(motorId));
} else {
driverstation.println("Wheel Calibrate", 3);
Calibration.updateWheelCalibrate(motorId);
}
}
