Add untrapping to climbNTrap sequence

Mechanical-Advantage · Mar 7, 2024 · 0d9a810 · 0d9a810
1 parent f917e74
commit 0d9a810
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 1 deletion.
diff --git a/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java b/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java
@@ -215,7 +215,7 @@ public static Command climbNTrapSequence(
                         .andThen(Commands.waitUntil(trapScoreTrigger.negate()))),
 
             // Retract backpack
-            superstructure.setGoalCommand(Superstructure.Goal.CLIMB))
+            superstructure.setGoalCommand(Superstructure.Goal.UNTRAP))
 
         // If cancelled, go to safe state
         .finallyDo(
@@ -230,6 +230,9 @@ public static Command climbNTrapSequence(
                 case TRAP ->
                     superstructure.setDefaultCommand(
                         superstructure.setGoalCommand(Superstructure.Goal.CLIMB));
+                case UNTRAP ->
+                    superstructure.setDefaultCommand(
+                            superstructure.setGoalCommand(Superstructure.Goal.TRAP));
               }
             });
   }

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java
@@ -36,6 +36,7 @@ public enum Goal {
     CLIMB,
     CANCEL_CLIMB,
     TRAP,
+    UNTRAP,
     RESET,
     DIAGNOSTIC_ARM
   }
@@ -148,6 +149,13 @@ public void periodic() {
         climber.setGoal(Climber.Goal.RETRACT);
         backpackActuator.setGoal(BackpackActuator.Goal.EXTEND);
       }
+      case UNTRAP -> {
+        arm.setGoal(Arm.Goal.UNTRAP);
+        climber.setGoal(Climber.Goal.RETRACT);
+        if (atArmGoal()) {
+          backpackActuator.setGoal(BackpackActuator.Goal.RETRACT);
+        }
+      }
       case RESET -> {
         desiredGoal = Goal.STOW;
         setDefaultCommand(setGoalCommand(Goal.STOW));

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java
@@ -83,6 +83,7 @@ public enum Goal {
     PODIUM(new LoggedTunableNumber("Arm/PodiumDegrees", 30.0)),
     PREPARE_CLIMB(new LoggedTunableNumber("Arm/PrepareClimbDegrees", 105.0)),
     CLIMB(new LoggedTunableNumber("Arm/ClimbDegrees", 90.0)),
+    UNTRAP(new LoggedTunableNumber("Arm/UnTrapDegrees", 80.0)),
     CUSTOM(new LoggedTunableNumber("Arm/CustomSetpoint", 20.0));
 
     private final DoubleSupplier armSetpointSupplier;