fix indexing piston code

[3501/2017steamworks] / src / org / usfirst / frc / team3501 / robot / commands / shooter / RunIndexWheelContinuous.java

diff --git a/src/org/usfirst/frc/team3501/robot/commands/shooter/RunIndexWheelContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/shooter/RunIndexWheelContinuous.java
index 815577e24d63cfde14158f23d4d66400b75a636e..1bf1309583fd06a0e5553ce95a2f1ac800d34094 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/commands/shooter/RunIndexWheelContinuous.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/shooter/RunIndexWheelContinuous.java
@@ -1,5 +1,6 @@
 package org.usfirst.frc.team3501.robot.commands.shooter;
 
+import org.usfirst.frc.team3501.robot.Constants;
 import org.usfirst.frc.team3501.robot.Robot;
 import org.usfirst.frc.team3501.robot.subsystems.Shooter;
 
@@ -33,6 +34,14 @@ public class RunIndexWheelContinuous extends Command {
 
   @Override
   protected void execute() {
+    if (timeSinceInitialized() % 1 == 0) {
+      if (Shooter.getShooter().getPistonValue() == Constants.Shooter.LOW_GEAR) {
+        Shooter.getShooter().setHighGear();
+      } else {
+        Shooter.getShooter().setLowGear();
+      }
+    }
+
     double shooterSpeed = shooter.getShooterRPM();
     double targetShooterSpeed = shooter.getTargetShootingSpeed();
     double threshold = shooter.getRPMThreshold();