import edu.wpi.first.wpilibj.command.PIDSubsystem;
public class DriveTrain extends PIDSubsystem {
+ // Determines if the "front" of the robot has been reversed
private boolean outputFlipped = false;
+
private static double pidOutput = 0;
private Encoder leftEncoder, rightEncoder;
super(Constants.DriveTrain.kp, Constants.DriveTrain.ki,
Constants.DriveTrain.kd);
- frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT);
- frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT);
- rearLeft = new CANTalon(Constants.DriveTrain.REAR_LEFT);
- rearRight = new CANTalon(Constants.DriveTrain.REAR_RIGHT);
+ frontLeft = new CANTalon(Constants.DriveTrain.DRIVE_FRONT_LEFT);
+ frontRight = new CANTalon(Constants.DriveTrain.DRIVE_FRONT_RIGHT);
+ rearLeft = new CANTalon(Constants.DriveTrain.DRIVE_REAR_LEFT);
+ rearRight = new CANTalon(Constants.DriveTrain.DRIVE_REAR_RIGHT);
robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
this.disable();
- leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.LEFT_MODULE,
- Constants.DriveTrain.LEFT_FORWARD, Constants.DriveTrain.LEFT_REVERSE);
- rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.RIGHT_MODULE,
- Constants.DriveTrain.RIGHT_FORWARD, Constants.DriveTrain.RIGHT_REVERSE);
+ leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.LEFT_SHIFT_MODULE,
+ Constants.DriveTrain.LEFT_SHIFT_FORWARD,
+ Constants.DriveTrain.LEFT_SHIFT_REVERSE);
+ rightGearPiston = new DoubleSolenoid(
+ Constants.DriveTrain.RIGHT_SHIFT_MODULE,
+ Constants.DriveTrain.RIGHT_SHIFT_FORWARD,
+ Constants.DriveTrain.RIGHT_SHIFT_REVERSE);
}
@Override
}
public void stop() {
- drive(0, 0);
+ setMotorSpeeds(0, 0);
}
public void resetEncoders() {
output = Math.signum(output) * 0.3;
left = output;
right = output + drift * Constants.DriveTrain.kp / 10;
- drive(left, right);
+ setMotorSpeeds(left, right);
pidOutput = output;
}
return getAvgEncoderDistance();
}
- /*
- * @param left and right setpoints to set to the left and right side of tank
- * inverted is for Logan, wants the robot to invert all controls left = right
- * and right = left negative input is required for the regular rotation
- * because RobotDrive tankdrive method drives inverted
- */
public void drive(double left, double right) {
- robotDrive.tankDrive(-left, -right);
+ // Handle flipping of the "front" of the robot
+ double k = (isFlipped() ? -1 : 1);
+
+ // During teleop, leftY is throttle, rightX is twist.
+ robotDrive.arcadeDrive(-left * k, -right);
}
public void setMotorSpeeds(double left, double right) {
- // positive setpoint to left side makes it go backwards
- // positive setpoint to right side makes it go forwards.
- frontLeft.set(-left);
- rearLeft.set(-left);
- frontRight.set(right);
- rearRight.set(right);
+ double k = (isFlipped() ? -1 : 1);
+ robotDrive.tankDrive(-left * k, -right * k);
}
/**