Make climb use encoders

src/main/java/frc/robot/Constants.java

@@ -18,14 +18,12 @@ public class Constants {
     public static final double REV_TIME = 1.0;
 
     /*CLIMB CONFIG */
-    /**the time we give the arms to extend in seconds */
-    public static final double ARM_EXTENSION_TIME = 3;
-    /**the time we give the arms to retract in seconds */
-    public static final double ARM_RETRACTION_TIME = 3;
+    public static final double CLIMB_EXTENDED_ENCODER_POSITION = 5;
+    public static final double CLIMB_RETRACTED_ENCODER_POSITION = 0.5;
     /**the % of max power we give to the arm motors while extending */
-    public static final double ARM_EXTENSION_SPEED = .25;
+    public static final double CLIMB_EXTENSION_SPEED = .25;
     /**the % of max power we give to the arm motors while retracting */
-    public static final double ARM_RETRACTION_SPEED = .25;
+    public static final double CLIMB_RETRACTION_SPEED = -.25;
     /*DRIVE TRAIN CONFIG */
     public static final double CONTROLLER_DEADZONE = 0.06;
 
@@ -58,7 +56,7 @@ public class Constants {
     /** amount of time in ms to wait for confirmation */
     public static final int TIMEOUT_MS = 30;
     public static final double ENCODER_ROTATION = 4096.0;
-
     /* ROTATION PID CONSTANTS */
+
     public static final double ROTATION_ERROR_DEGREES = 5.;
 }

src/main/java/frc/robot/InputtedCoDriverControls.java

@@ -14,6 +14,8 @@ public static void setCoDriverController(XboxController xboxController) {
     }
 
     public static void onEveryFrame() {
+        Robot.motors.getLeftClimb().stopMotor();
+        Robot.motors.getRightClimb().stopMotor();
         if (controller.getAButton()) {
             Robot.motors.getFeeder().set(30);
         } else {
@@ -24,19 +26,25 @@ public static void onEveryFrame() {
         }
         doShooterIntake();
         if (controller.getStartButton()) {
-            extendArms();
+            //extendArms();
+            Robot.motors.getLeftClimb().set(Constants.CLIMB_EXTENSION_SPEED);
+            Robot.motors.getRightClimb().set(Constants.CLIMB_EXTENSION_SPEED);
         }
         if (controller.getBackButton()) {
-            retractArms();
+            //retractArms();
+            Robot.motors.getLeftClimb().set(Constants.CLIMB_RETRACTION_SPEED);
+            Robot.motors.getRightClimb().set(Constants.CLIMB_RETRACTION_SPEED);
         }
     }
 
     private static void extendArms() {
         Robot.getTeleopActionRunner().addActionToRun(new ExtendArms());
+        Robot.getTeleopActionRunner().removeActionsOfType(RetractArms.class);
     }
 
     private static void retractArms() {
         Robot.getTeleopActionRunner().addActionToRun(new RetractArms());
+        Robot.getTeleopActionRunner().removeActionsOfType(ExtendArms.class);
     }
 
     private static void doShooterIntake() {

src/main/java/frc/robot/auton/ExtendArms.java

@@ -6,11 +6,9 @@
 
 public class ExtendArms extends AutonAction {
 
-    private double extensionTime;
-
     @Override
     public boolean isDone() {
-        if (Timer.getFPGATimestamp() >= extensionTime) {
+        if (Robot.motors.getLeftClimb().getEncoderPosition() > Constants.CLIMB_EXTENDED_ENCODER_POSITION) {
             Robot.motors.getLeftClimb().set(0);
             Robot.motors.getRightClimb().set(0);
             return true;
@@ -20,8 +18,7 @@ public boolean isDone() {
 
     @Override
     public void initiate() {
-        Robot.motors.getLeftClimb().set(Constants.ARM_EXTENSION_SPEED);
-        Robot.motors.getRightClimb().set(Constants.ARM_EXTENSION_SPEED);
-        extensionTime = Timer.getFPGATimestamp() + Constants.ARM_EXTENSION_TIME;
+        Robot.motors.getLeftClimb().set(Constants.CLIMB_EXTENSION_SPEED);
+        Robot.motors.getRightClimb().set(Constants.CLIMB_EXTENSION_SPEED);
     }
 }

src/main/java/frc/robot/auton/ParallelActionRunner.java

@@ -1,6 +1,7 @@
 package frc.robot.auton;
 
 import java.util.ArrayList;
+import java.util.Iterator;
 import java.util.List;
 
 public class ParallelActionRunner {
@@ -13,12 +14,28 @@ public void addActionToRun(AutonAction action) {
     }
 
     public void onEveryFrame() {
-        for (int i = 0; i < actions.size(); i++) {
-            AutonAction action = actions.get(i);
+        Iterator<AutonAction> it = actions.iterator();
+        while (it.hasNext()) {
+            AutonAction action = it.next();
             if (action.isDone()) {
-                actions.remove(i);
-                i--;
+                actions.remove(action);
             }
         }
     }
+
+    // This method is important if you have code that could add two of the same action with opposite goals.
+    // That would mean they would fight each other and never finish, which is bad news bears if you ask me.
+    public void removeActionsOfType(Class<?> clazz) {
+        Iterator<AutonAction> it = actions.iterator();
+        while (it.hasNext()) {
+            AutonAction auction = it.next();
+            if (auction.getClass().equals(clazz)) {
+                actions.remove(auction);
+            }
+        }
+    }
+
+    public void removeActionsOfType(AutonAction action) {
+        removeActionsOfType(action.getClass());
+    }
 }

src/main/java/frc/robot/auton/RetractArms.java

@@ -6,11 +6,9 @@
 
 public class RetractArms extends AutonAction {
 
-    private double extensionTime;
-
     @Override
     public boolean isDone() {
-        if (Timer.getFPGATimestamp() >= extensionTime) {
+        if (Robot.motors.getLeftClimb().getEncoderPosition() < Constants.CLIMB_RETRACTED_ENCODER_POSITION) {
             Robot.motors.getLeftClimb().set(0);
             Robot.motors.getRightClimb().set(0);
             return true;
@@ -20,8 +18,7 @@ public boolean isDone() {
 
     @Override
     public void initiate() {
-        Robot.motors.getLeftClimb().set(Constants.ARM_RETRACTION_SPEED);
-        Robot.motors.getRightClimb().set(Constants.ARM_RETRACTION_SPEED);
-        extensionTime = Timer.getFPGATimestamp() + Constants.ARM_RETRACTION_TIME;
+        Robot.motors.getLeftClimb().set(Constants.CLIMB_RETRACTION_SPEED);
+        Robot.motors.getRightClimb().set(Constants.CLIMB_RETRACTION_SPEED);
     }
 }