From 1f6a07751786a1c9c4160ef8b047510ce11b2687 Mon Sep 17 00:00:00 2001
From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com>
Date: Sat, 15 Mar 2025 19:03:59 -0600
Subject: [PATCH] spaget: end of utah

---
 src/main/java/frc4388/robot/RobotContainer.java | 14 ++++++++++----
 1 file changed, 10 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 1b9eb45..8fbc18b 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -116,6 +116,7 @@ public class RobotContainer {
     public void stop() {
         new InstantCommand(()->{}, m_robotSwerveDrive).schedule();
         m_robotSwerveDrive.stopModules();
+        Constants.AutoConstants.Y_OFFSET_TRIM.set(0);
     }
 
     // ! /*  Autos */
@@ -406,7 +407,9 @@ public class RobotContainer {
             new MoveForTimeCommand(m_robotSwerveDrive, 
                 new Translation2d(0, 1), 
                 new Translation2d(), 300, true
-        ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)));
+                
+        ), new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(0)),
+        new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)));
         
         NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left);
         NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4Right);
@@ -415,9 +418,12 @@ public class RobotContainer {
         NamedCommands.registerCommand("place-coral-left-l2", AprilLidarAlignL2Left);
         NamedCommands.registerCommand("place-coral-right-l2", AprilLidarAlignL2Right);
 
-        NamedCommands.registerCommand("prepare-l4", new InstantCommand(() -> {
-            m_robotElevator.transitionState(CoordinationState.PrimedFour);
-        }));
+        NamedCommands.registerCommand("prepare-l4", new SequentialCommandGroup(
+            // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Hovering)),
+            // new waitElevatorRefrence(m_robotElevator),
+            new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(1.5)),
+            new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour))
+        ));
 
         configureButtonBindings();        
         configureVirtualButtonBindings();