From 0d9a8105299c44a471b72db9dd3d81a4e32275db Mon Sep 17 00:00:00 2001
From: suryatho <suryathoppae@gmail.com>
Date: Wed, 6 Mar 2024 21:21:05 -0500
Subject: [PATCH] Add untrapping to climbNTrap sequence

---
 .../frc2024/commands/ClimbingCommands.java                | 5 ++++-
 .../frc2024/subsystems/superstructure/Superstructure.java | 8 ++++++++
 .../frc2024/subsystems/superstructure/arm/Arm.java        | 1 +
 3 files changed, 13 insertions(+), 1 deletion(-)

diff --git a/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java b/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java
index db59e2bf..e578d8f0 100644
--- 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
index d0859260..61114c9a 100644
--- 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
index 955f27b7..6f92954e 100644
--- 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;