Trap repeatedly (#185)

Mechanical-Advantage · Mar 9, 2024 · f316d57 · f316d57
1 parent a889655
commit f316d57
Showing 1 changed file with 25 additions and 18 deletions.
diff --git a/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java b/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java
@@ -208,24 +208,31 @@ public static Command climbNTrapSequence(
                     Commands.waitUntil(trapScoreTrigger)
                         .andThen(Commands.waitUntil(trapScoreTrigger.negate()))),
 
-            // Extend backpack
-            superstructure
-                .setGoalCommand(Superstructure.Goal.TRAP)
-                .alongWith(rollers.setGoalCommand(Rollers.Goal.TRAP_PRESCORE))
-                .raceWith(
-                    Commands.waitUntil(trapScoreTrigger)
-                        .andThen(Commands.waitUntil(trapScoreTrigger.negate()))),
-
-            // Score in trap and wait
-            rollers
-                .setGoalCommand(Rollers.Goal.TRAP_SCORE)
-                .alongWith(superstructure.setGoalCommand(Superstructure.Goal.TRAP))
-                .raceWith(
-                    Commands.waitUntil(trapScoreTrigger)
-                        .andThen(Commands.waitUntil(trapScoreTrigger.negate()))),
-
-            // Retract backpack
-            superstructure.setGoalCommand(Superstructure.Goal.CLIMB))
+            // Repeat trap sequence forever and ever
+            Commands.sequence(
+                    // Extend backpack
+                    superstructure
+                        .setGoalCommand(Superstructure.Goal.TRAP)
+                        .alongWith(rollers.setGoalCommand(Rollers.Goal.TRAP_PRESCORE))
+                        .raceWith(
+                            Commands.waitUntil(trapScoreTrigger)
+                                .andThen(Commands.waitUntil(trapScoreTrigger.negate()))),
+
+                    // Score in trap and wait
+                    rollers
+                        .setGoalCommand(Rollers.Goal.TRAP_SCORE)
+                        .alongWith(superstructure.setGoalCommand(Superstructure.Goal.TRAP))
+                        .raceWith(
+                            Commands.waitUntil(trapScoreTrigger)
+                                .andThen(Commands.waitUntil(trapScoreTrigger.negate()))),
+
+                    // Retract backpack
+                    superstructure
+                        .setGoalCommand(Superstructure.Goal.CLIMB)
+                        .raceWith(
+                            Commands.waitUntil(trapScoreTrigger)
+                                .andThen(Commands.waitUntil(trapScoreTrigger.negate()))))
+                .repeatedly())
 
         // If cancelled, go to safe state
         .finallyDo(