import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.command.Subsystem;
public static final int ENCODER_PULSES_PER_REVOLUTION = 256;
public static final double INCHES_PER_PULSE = WHEEL_DIAMETER * Math.PI
/ ENCODER_PULSES_PER_REVOLUTION;
-
+ public static final int MAINTAIN_CLIMBED_POSITION = 1;
private static DriveTrain driveTrain;
+
private final CANTalon frontLeft, frontRight, rearLeft, rearRight;
private final RobotDrive robotDrive;
private final Encoder leftEncoder, rightEncoder;
- private final DoubleSolenoid shifter;
+ private final DoubleSolenoid leftGearPiston, rightGearPiston;
private ADXRS450_Gyro imu;
robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
this.imu = new ADXRS450_Gyro(Constants.DriveTrain.GYRO_PORT);
- shifter = DoubleSolenoid(10, Constants.DriveTrain.SHIFTER_FORWARD,
- Constants.DriveTrain.SHIFTER_REVERSE);
+
+ // TODO: Not sure if MODULE_NUMBER should be the same for both
+ leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.MODULE_NUMBER,
+ Constants.DriveTrain.LEFT_GEAR_PISTON_FORWARD,
+ Constants.DriveTrain.LEFT_GEAR_PISTON_REVERSE);
+ rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.MODULE_NUMBER,
+ Constants.DriveTrain.RIGHT_GEAR_PISTON_FORWARD,
+ Constants.DriveTrain.RIGHT_GEAR_PISTON_REVERSE);
}
public static DriveTrain getDriveTrain() {
// System.out.println("left: " + getLeftEncoderDistance());
// System.out.println("right: " + getRightEncoderDistance());
System.out.println(getAvgEncoderDistance());
+ System.out.println("left: " + getLeftEncoderDistance());
+ System.out.println("right: " + getRightEncoderDistance());
}
public double getAvgEncoderDistance() {
return this.gyroZero;
}
+ /*
+ * @return a value that is the current setpoint for the piston kReverse or
+ * KForward
+ */
+ public Value getLeftGearPistonValue() {
+ return leftGearPiston.get();
+ }
+
+ /*
+ * @return a value that is the current setpoint for the piston kReverse or
+ * KForward
+ */
+ public Value getRightGearPistonValue() {
+ return rightGearPiston.get();
+ }
+
+ /*
+ * Changes the ball shift gear assembly to high
+ */
+ public void setHighGear() {
+ changeGear(Constants.DriveTrain.HIGH_GEAR);
+ }
+
+ /*
+ * Changes the ball shift gear assembly to low
+ */
+ public void setLowGear() {
+ changeGear(Constants.DriveTrain.LOW_GEAR);
+ }
+
+ /*
+ * Changes the gear to a DoubleSolenoid.Value
+ */
+ private void changeGear(DoubleSolenoid.Value gear) {
+ leftGearPiston.set(gear);
+ rightGearPiston.set(gear);
+ }
+
@Override
protected void initDefaultCommand() {
setDefaultCommand(new JoystickDrive());