Add ToggleIndexerPiston class

[3501/2017steamworks] / src / org / usfirst / frc / team3501 / robot / commands / driving / ToggleGear.java

diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleGear.java b/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleGear.java
index ce7489096ec57d9443c0e17f1cc5da1ec71ad9f3..3a17440b1e920cd182db1cbc87d20c003296346e 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleGear.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleGear.java
@@ -29,10 +29,17 @@ public class ToggleGear extends Command {
   protected void execute() {
     Value leftGearPistonValue = driveTrain.getLeftGearPistonValue();
     Value rightGearPistonValue = driveTrain.getRightGearPistonValue();
+
     if (leftGearPistonValue == Constants.DriveTrain.LOW_GEAR) {
-      driveTrain.setHighGear();
+      driveTrain.setHighGear(driveTrain.getLeftPiston());
+    } else {
+      driveTrain.setLowGear(driveTrain.getLeftPiston());
+    }
+
+    if (rightGearPistonValue == Constants.DriveTrain.LOW_GEAR) {
+      driveTrain.setHighGear(driveTrain.getRightPiston());
     } else {
-      driveTrain.setLowGear();
+      driveTrain.setLowGear(driveTrain.getRightPiston());
     }
   }