I think this offset is working

src/main/java/frc4388/robot/RobotContainer.java

@@ -107,9 +107,9 @@ public class RobotContainer {
     private Command autoCommand;
 
     private Command waitFeedStation = new waitSupplier(m_robotElevator::readyToMove);
-    private Command waitDebuger = new waitSupplier(m_driverXbox::getXButtonPressed);
+    // private Command waitDebuger = new waitSupplier(m_driverXbox::getYButtonPressed);
     // private Command waitDebugerManual = new waitSupplier(m_driverXbox::getYButtonPressed);
-    // private Command waitDebuger = new waitSupplier(() -> true);
+    private Command waitDebuger = new waitSupplier(() -> true);
 
     // private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName);
     // private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive, 
@@ -289,58 +289,42 @@ public class RobotContainer {
 
     private Command AprilLidarAlignL4LeftFullAuto = new SequentialCommandGroup(
         new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
-
+        
-        // new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
-
         // new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
         new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
-        // new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true),
+        new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true),
         // ), () -> m_robotElevator.isL4Primed()),
-        
-        waitDebuger.asProxy(),
 
         // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
-        // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
+        // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT),
 
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
 
-        waitDebuger.asProxy(),
-
-
         new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true),
         waitDebuger.asProxy(),
         // new LidarAlign(m_robotSwerveDrive, reefLidar),
         // waitDebuger.asProxy(),
-        // new ParallelRaceGroup(
+        new ParallelRaceGroup(
-        //     new WaitCommand(1),
+            new WaitCommand(1),
-        //     new MoveUntilSuply(
+            new MoveUntilSuply(
-        //         m_robotSwerveDrive, 
+                m_robotSwerveDrive, 
-        //         new Translation2d(0,-0.5), 
+                new Translation2d(0,-0.5), 
-        //         new Translation2d(), m_robotElevator::getEndeffectorLimit, true)
+                new Translation2d(), m_robotElevator::getEndeffectorLimit, true)
-        // ),
+        ),
-        // new InstantCommand(m_robotSwerveDrive::softStop, m_robotSwerveDrive),
+        new InstantCommand(m_robotSwerveDrive::softStop, m_robotSwerveDrive),
-        
+
         new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
 
-        waitDebuger.asProxy(),
-
-
         new waitEndefectorRefrence(m_robotElevator),
 
-        waitDebuger.asProxy(),
+        new MoveForTimeCommand(m_robotSwerveDrive, 
-
+        new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
-
-        
-        // new MoveForTimeCommand(m_robotSwerveDrive, 
-        //     new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
-
-        // // // new ConditionalCommand(
-        // // //     new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
-        //     new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
-                // () -> m_robotElevator.hasCoral()),
-
 
+        // new ConditionalCommand(
+        //     new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
+            new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
+            //  () -> m_robotElevator.hasCoral())
 
         new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
     );
@@ -692,8 +676,8 @@ public class RobotContainer {
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
 
         
-        new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
+        // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
-            .onTrue(AprilLidarAlignL4LeftFullAuto);
+        //     .onTrue(AprilLidarAlignL4LeftFullAuto);
             
 
         // ! /* Speed */
@@ -826,7 +810,7 @@ public class RobotContainer {
 
         // Auto Scoring 
         new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.8 && !operatorManualMode)
-            .onTrue(AprilLidarAlignL4LeftSemiAuto);
+            .onTrue(AprilLidarAlignL4LeftFullAuto);
         
         new Trigger(() -> getDeadbandedOperatorController().getRightTriggerAxis() >= 0.8 && !operatorManualMode)
             .onTrue(AprilLidarAlignL4RightSemiAuto);

src/main/java/frc4388/robot/constants/BuildConstants.java

@@ -7,12 +7,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "2025RidgeScape";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 175;
+  public static final int GIT_REVISION = 176;
-  public static final String GIT_SHA = "93ba351a198d2f83e0592eed3444465fc5b408f6";
+  public static final String GIT_SHA = "17239b8920efd45bc5a3b6a098baf4e5509fb558";
-  public static final String GIT_DATE = "2025-09-11 17:20:49 MDT";
+  public static final String GIT_DATE = "2025-09-13 11:20:40 MDT";
   public static final String GIT_BRANCH = "advantagekit";
-  public static final String BUILD_DATE = "2025-09-13 11:19:55 MDT";
+  public static final String BUILD_DATE = "2025-09-13 14:19:44 MDT";
-  public static final long BUILD_UNIX_TIME = 1757783995171L;
+  public static final long BUILD_UNIX_TIME = 1757794784689L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

src/main/java/frc4388/robot/constants/Constants.java

@@ -50,7 +50,7 @@ public final class Constants {
     public static final class AutoConstants {
         // public static final Gains XY_GAINS = new Gains(5,0.6,0.0);
         // public static final Gains XY_GAINS = new Gains(8,0,0.0);
-        public static final Gains XY_GAINS = new Gains(0.8,0,0.0);
+        public static final Gains XY_GAINS = new Gains(3,0,1.0);
         // public static final ConfigurableDouble P_XY_GAINS = new ConfigurableDouble("P_XY_GAINS", XY_GAINS.kP);
         // public static final ConfigurableDouble I_XY_GAINS = new ConfigurableDouble("I_XY_GAINS", XY_GAINS.kI);
         // public static final ConfigurableDouble D_XY_GAINS = new ConfigurableDouble("D_XY_GAINS", XY_GAINS.kD);
@@ -79,8 +79,8 @@ public final class Constants {
         public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16+1);
         public static final double HALF_ROBOT_SIZE = Units.inchesToMeters(18);
 
-        public static final double L4_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15 - 3)  ;
+        public static final double L4_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15 - 6);
-        public static final double L4_DISTANCE_SCORE = L4_DISTANCE_PREP - Units.inchesToMeters(3);
+        public static final double L4_DISTANCE_SCORE = L4_DISTANCE_PREP;
         // public static final double L4_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(4.5);
         
         public static final double L3_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15);
@@ -114,7 +114,7 @@ public final class Constants {
         public static final String LEFT_CAMERA_NAME = "CAMERA_LEFT";
         public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT";
 
-        public static final Transform3d LEFT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(5.3), Units.inchesToMeters(8.75), Units.inchesToMeters(9)), new Rotation3d(0,0.0,-15.0 * Math.PI / 180));
+        public static final Transform3d LEFT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(5.3), Units.inchesToMeters(8.75+3), Units.inchesToMeters(9)), new Rotation3d(0,0.0,-15.0 * Math.PI / 180));
         public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(0), -Units.inchesToMeters(0), Units.inchesToMeters(0)), new Rotation3d(0,0.0,0));
         
         public static final double MIN_ESTIMATION_DISTANCE = 2; // Meters

src/main/java/frc4388/utility/compute/ReefPositionHelper.java

@@ -55,7 +55,7 @@ public class ReefPositionHelper {
             }
         }
         
-        System.out.println(minPos.getRotation().getDegrees());
+        // System.out.println(minPos.getRotation().getDegrees());
 
         return minPos;
     }