/** Update normal driver control */
private void updateDriveControl() {
// Set speed to twist because the robot should turn in place w/ button 2
if (driverstation.JoystickDriverButton3) {
speed = driverstation.JoystickDriverTwist;
} else {
speed =
(driverstation.getStickDistance(
driverstation.JoystickDriverX, driverstation.JoystickDriverY));
}
// sets the drive speed so it will not drive below a minimum speed
if (Math.abs(speed) < MINUMUM_DRIVE_SPEED) {
speed = 0.0;
}
// Decide drive mode
if (driverstation.JoystickDriverButton3) {
if (driverstation.JoystickDriverTrigger) {
speed = speed / 2;
}
// Rotate in place if button 3 is pressed
drive.turnDrive(-speed);
} else {
// Normally use crab drive
drive.crabDrive(
driverstation.getStickAngle(driverstation.JoystickDriverX, driverstation.JoystickDriverY),
speed);
}
}
/** 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);
}
}
