From 9ba7c6341096edab5fa4cdf2a7614745d3380b46 Mon Sep 17 00:00:00 2001
From: Jonah <47046556+jwbonner@users.noreply.github.com>
Date: Thu, 14 Mar 2024 18:11:43 -0400
Subject: [PATCH] Finish climb sequence

---
 .../org/littletonrobotics/frc2024/Robot.java  |  5 ++--
 .../frc2024/commands/ClimbingCommands.java    | 23 ++++++++++++++-----
 2 files changed, 20 insertions(+), 8 deletions(-)

diff --git a/src/main/java/org/littletonrobotics/frc2024/Robot.java b/src/main/java/org/littletonrobotics/frc2024/Robot.java
index 0122bc74..15359dc3 100644
--- a/src/main/java/org/littletonrobotics/frc2024/Robot.java
+++ b/src/main/java/org/littletonrobotics/frc2024/Robot.java
@@ -28,6 +28,7 @@
 import java.util.HashMap;
 import java.util.Map;
 import java.util.function.BiConsumer;
+import java.util.function.DoubleSupplier;
 import org.littletonrobotics.frc2024.Constants.Mode;
 import org.littletonrobotics.frc2024.subsystems.leds.Leds;
 import org.littletonrobotics.frc2024.util.Alert;
@@ -83,12 +84,12 @@ public class Robot extends LoggedRobot {
   private final Alert sameBatteryAlert =
       new Alert("The battery has not been changed since the last match.", AlertType.WARNING);
 
-  public static Trigger createTeleopTimeTrigger(double teleElapsedTime) {
+  public static Trigger createTeleopTimeTrigger(DoubleSupplier teleElapsedTime) {
     return new Trigger(
         () ->
             DriverStation.isFMSAttached()
                 && DriverStation.isTeleopEnabled()
-                && Robot.teleElapsedTime > teleElapsedTime);
+                && Robot.teleElapsedTime > teleElapsedTime.getAsDouble());
   }
 
   /**
diff --git a/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java b/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java
index 6ece0e03..db3da1d3 100644
--- a/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java
+++ b/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java
@@ -12,6 +12,7 @@
 import edu.wpi.first.wpilibj2.command.WrapperCommand;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import lombok.experimental.ExtensionMethod;
+import org.littletonrobotics.frc2024.Robot;
 import org.littletonrobotics.frc2024.RobotState;
 import org.littletonrobotics.frc2024.subsystems.drive.Drive;
 import org.littletonrobotics.frc2024.subsystems.leds.Leds;
@@ -26,6 +27,8 @@
 public class ClimbingCommands {
   private static final LoggedTunableNumber chainToBack =
       new LoggedTunableNumber("ClimbingCommands/ChainToBackOffset", 0.3);
+  private static final LoggedTunableNumber autoUntrapTime =
+      new LoggedTunableNumber("ClimbingCommands/AutoUntrapTime", 134.5);
 
   public static Command trapSequence(
       Drive drive,
@@ -33,9 +36,16 @@ public static Command trapSequence(
       Rollers rollers,
       Trigger forwardTrigger,
       Trigger reverseTrigger) {
+    Trigger endOfMatch = Robot.createTeleopTimeTrigger(autoUntrapTime);
     SteppableCommandGroup sequence =
         new SteppableCommandGroup(
-            forwardTrigger.and(superstructure::atGoal),
+            forwardTrigger
+                .and(superstructure::atArmGoal)
+                .or(
+                    endOfMatch.and(
+                        () ->
+                            superstructure.getDesiredGoal() == Superstructure.Goal.TRAP
+                                && rollers.getGoal() == Rollers.Goal.TRAP_SCORE)),
             reverseTrigger,
 
             // Move arm to prepare prepare climb setpoint while moving climbers up
@@ -57,15 +67,16 @@ public static Command trapSequence(
                 .alongWith(
                     superstructure.setGoalWithConstraintsCommand(
                         Superstructure.Goal.PREPARE_CLIMB,
-                        Arm.prepareClimbProfileConstraints.get())),
+                        Arm.prepareClimbProfileConstraints.get()),
+                    rollers.setGoalCommand(Rollers.Goal.SHUFFLE_BACKPACK)),
 
             // Allow driver to line up and climb
-            superstructure
-                .setGoalCommand(Superstructure.Goal.CLIMB)
-                .alongWith(rollers.setGoalCommand(Rollers.Goal.SHUFFLE_BACKPACK)),
+            superstructure.setGoalCommand(Superstructure.Goal.CLIMB),
 
             // Extend backpack
-            superstructure.setGoalCommand(Superstructure.Goal.TRAP),
+            superstructure
+                .setGoalCommand(Superstructure.Goal.TRAP)
+                .alongWith(rollers.setGoalCommand(Rollers.Goal.TRAP_PRESCORE)),
 
             // Trap.
             superstructure