// Called repeatedly when this Command is scheduled to run
@Override
protected void execute() {
Joystick joystickDrive = Robot.oi.getJoystickDrive();
this.joystickX = joystickDrive.getAxis(Joystick.AxisType.kX) * Robot.driveTrain.turnMultiplier;
this.joystickY = joystickDrive.getAxis(Joystick.AxisType.kY) * -1;
VisionState vs = VisionState.getInstance();
if (vs == null || !vs.wantsControl()) {
// endAutoTurn is harmless when not needed but required
// if the driver changes her mind after initiating auto-targeting..
Robot.driveTrain.endAutoTurn();
this.scaledThrottle = scaleThrottle(joystickDrive.getAxis(Joystick.AxisType.kThrottle));
if ((Math.abs(this.joystickX) < 0.075) && (Math.abs(this.joystickY) < 0.075)) {
Robot.driveTrain.stop();
} else {
Robot.driveTrain.arcadeDrive(joystickY * scaledThrottle, joystickX * scaledThrottle);
}
} else {
if (vs.DriveLockedOnTarget) {
// wait for launcher to shoot and exit auto mode or toggle AutoAim
Robot.driveTrain.stop(); // needed to keep driveTrain alive
} else {
if (!Robot.driveTrain.isAutoTurning()) {
double h = Robot.driveTrain.getCurrentHeading();
double target = vs.getTargetHeading(h);
Robot.driveTrain.startAutoTurn(target);
} else if (Robot.driveTrain.isAutoTurnFinished()) {
Robot.driveTrain.endAutoTurn();
vs.DriveLockedOnTarget = true;
} // else allow auto-turn to continue
}
}
}
public void takeJoystickInputs(Joystick leftJoystick, Joystick rightJoystick) {
// robotDrive.tankDrive(leftJoystick, rightJoystick);
SmartDashboard.putNumber("Sensitivity", 1 - leftJoystick.getAxis(AxisType.kZ));
sensitivity = 1 - leftJoystick.getAxis(AxisType.kZ);
robotDrive.tankDrive(
leftJoystick.getAxis(AxisType.kY) * sensitivity,
rightJoystick.getAxis(AxisType.kY) * sensitivity);
}
/** @param joy The ps3 style joystick to use to drive tank style. */
public void drive(Joystick joy) {
drive(-joy.getY(), -joy.getAxis(AxisType.kThrottle));
}
