From 712c81316d51a83e1042ccf4a40c343f5eb7f109 Mon Sep 17 00:00:00 2001
From: Nummun14 <142012009+Nummun14@users.noreply.github.com>
Date: Thu, 29 Jan 2026 23:32:03 +0200
Subject: [PATCH 01/12] did stuff

---
 .../java/frc/trigon/robot/RobotContainer.java |  11 +-
 .../commandfactories/AutonomousCommands.java  | 117 ++++++------------
 .../robot/constants/AutonomousConstants.java  |  44 ++++++-
 .../robot/constants/FieldConstants.java       |  15 ++-
 4 files changed, 96 insertions(+), 91 deletions(-)

diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java
index a0d3fe81..b2fdefaf 100644
--- a/src/main/java/frc/trigon/robot/RobotContainer.java
+++ b/src/main/java/frc/trigon/robot/RobotContainer.java
@@ -5,12 +5,12 @@
 
 package frc.trigon.robot;
 
-import com.pathplanner.lib.auto.AutoBuilder;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.trigon.lib.utilities.flippable.Flippable;
 import frc.trigon.robot.commands.CommandConstants;
+import frc.trigon.robot.commands.commandfactories.AutonomousCommands;
 import frc.trigon.robot.commands.commandfactories.FuelIntakeCommands;
 import frc.trigon.robot.commands.commandfactories.GeneralCommands;
 import frc.trigon.robot.constants.AutonomousConstants;
@@ -37,7 +37,6 @@
 import frc.trigon.robot.subsystems.swerve.Swerve;
 import frc.trigon.robot.subsystems.turret.Turret;
 import frc.trigon.robot.subsystems.turret.TurretCommands;
-import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
 public class RobotContainer {
     public static final RobotPoseEstimator ROBOT_POSE_ESTIMATOR = new RobotPoseEstimator();
@@ -53,11 +52,9 @@ public class RobotContainer {
     public static final Shooter SHOOTER = new Shooter();
     public static final Spindexer SPINDEXER = new Spindexer();
     public static final Turret TURRET = new Turret();
-    private LoggedDashboardChooser<Command> autoChooser;
 
     public RobotContainer() {
         initializeGeneralSystems();
-        buildAutoChooser();
         configureBindings();
     }
 
@@ -65,7 +62,7 @@ public RobotContainer() {
      * @return the command to run in autonomous mode
      */
     public Command getAutonomousCommand() {
-        return autoChooser.get();
+        AutonomousCommands.getAutonomousCommand();
     }
 
     private void configureBindings() {
@@ -112,8 +109,4 @@ private void initializeGeneralSystems() {
         AutonomousConstants.init();
         ShootingLookupTable3D.init();
     }
-
-    private void buildAutoChooser() {
-        autoChooser = new LoggedDashboardChooser<>("AutoChooser", AutoBuilder.buildAutoChooser());
-    }
 }
\ No newline at end of file
diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
index cbf6158a..52a0255c 100644
--- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
+++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
@@ -3,11 +3,14 @@
 import com.pathplanner.lib.commands.PathPlannerAuto;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj2.command.*;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.trigon.lib.utilities.flippable.FlippablePose2d;
 import frc.trigon.robot.RobotContainer;
-import frc.trigon.robot.commands.commandclasses.GamePieceAutoDriveCommand;
 import frc.trigon.robot.constants.AutonomousConstants;
+import frc.trigon.robot.constants.FieldConstants;
 import frc.trigon.robot.subsystems.swerve.SwerveCommands;
 import org.json.simple.parser.ParseException;
 
@@ -18,99 +21,59 @@
  * A class that contains command factories for preparation commands and commands used during the 15-second autonomous period at the start of each match.
  */
 public class AutonomousCommands {
-    private static FlippablePose2d TARGET_SCORING_POSE = null;
-
-    /**
-     * Creates a dynamic autonomous command intended for the 15-second autonomous period at the beginning of a match.
-     * Dynamic means that the command isn't pre-programmed and instead autonomously decides what game pieces to collect and where to score.
-     *
-     * @param intakeLocations  the locations at which to collect game pieces
-     * @param scoringLocations the locations at which to score
-     * @return the command
-     */
-    public static Command getDynamicAutonmousCommand(FlippablePose2d[] intakeLocations, FlippablePose2d... scoringLocations) {
+    public static Command getAutonomousCommand() {
         return new SequentialCommandGroup(
-                getDriveAndScoreCommand(scoringLocations),
-                getCollectCommand(intakeLocations)
-        ).repeatedly().withName(generateDynamicAutonomousRoutineName(intakeLocations, scoringLocations));
-    }
-
-    private static Command getCollectCommand(FlippablePose2d[] intakeLocations) {
-        return new ParallelCommandGroup(
-                getIntakeSequenceCommand(),
-                getDriveToGamePieceCommand(intakeLocations)
-        ).until(AutonomousCommands::hasGamePiece);
-    }
-
-    private static boolean hasGamePiece() {
-        //TODO: implement
-        return false;
-    }
-
-    private static Command getDriveToGamePieceCommand(FlippablePose2d[] intakeLocations) {
-        return new ConditionalCommand(
-                new GamePieceAutoDriveCommand().onlyWhile(() -> RobotContainer.OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot() != null),
-                getFindGamePieceCommand(intakeLocations),
-                AutonomousCommands::shouldCollectGamePiece
+                AutonomousConstants.FIRST_AUTONOMOUS_CHOOSER.get() == null ? new InstantCommand() : AutonomousConstants.FIRST_AUTONOMOUS_CHOOSER.get(),
+                AutonomousConstants.SECOND_AUTONOMOUS_CHOOSER.get() == null ? new InstantCommand() : AutonomousConstants.SECOND_AUTONOMOUS_CHOOSER.get(),
+                AutonomousConstants.THIRD_AUTONOMOUS_CHOOSER.get() == null ? new InstantCommand() : AutonomousConstants.THIRD_AUTONOMOUS_CHOOSER.get(),
+                getClimbCommand(AutonomousConstants.CLIMB_POSITION_CHOOSER.get()).onlyIf(() -> AutonomousConstants.CLIMB_POSITION_CHOOSER.get() != null)
         );
     }
 
-    private static boolean shouldCollectGamePiece() {
-        return RobotContainer.OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot() != null;
-    }
-
-    private static Command getFindGamePieceCommand(FlippablePose2d[] intakeLocations) {
-        //TODO: implement
-        return null;
-    }
-
-    private static Command getIntakeSequenceCommand() {
-        //TODO: implement
-        return null;
+    public static Command getDeliveryCommand() {
+        return new ParallelCommandGroup(
+                SwerveCommands.getDriveToPoseCommand(isRight() ? () -> FieldConstants.RIGHT_INTAKE_POSITION : () -> FieldConstants.LEFT_INTAKE_POSITION, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
+                FuelIntakeCommands.getIntakeCommand(),
+                GeneralCommands.getContinuousConditionalCommand(
+                        null, //TODO: getShootingCommand()
+                        null, //TODO: getDeliveryCommand()
+                        () -> false //TODO: isInAllianceZone()
+                )
+        ).withTimeout(AutonomousConstants.DELIVERY_TIMEOUT_SECONDS);
     }
 
-    private static Command getDriveAndScoreCommand(FlippablePose2d[] scoringLocations) {
+    public static Command getCollectFromNeutralZoneCommand() {
         return new ParallelCommandGroup(
-                getDriveToScoringLocationCommand(scoringLocations),
-                getScoringSequenceCommand()
-        );
+                SwerveCommands.getDriveToPoseCommand(isRight() ? () -> FieldConstants.RIGHT_INTAKE_POSITION : () -> FieldConstants.LEFT_INTAKE_POSITION, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
+                FuelIntakeCommands.getIntakeCommand()
+                //TODO: getShootingCommand().onlyIf(isInAllianceZone())
+        ).withTimeout(AutonomousConstants.NEUTRAL_ZONE_COLLECTION_TIMEOUT_SECONDS);
     }
 
-    private static Command getScoringSequenceCommand() {
-        return new SequentialCommandGroup(
-                new WaitUntilCommand(() -> TARGET_SCORING_POSE != null),
-                getScoreCommand()
+    public static Command getScoreCommand() {
+        return new ParallelCommandGroup(
+                SwerveCommands.getDriveToPoseCommand(isRight() ? () -> FieldConstants.RIGHT_IDEAL_SHOOTING_POSITION : () -> FieldConstants.LEFT_IDEAL_SHOOTING_POSITION, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS)
+                //TODO: getShootingCommand().onlyIf(isInAllianceZone())
         );
     }
 
-    private static Command getScoreCommand() {
-        //TODO: implement
-        return null;
+    public static Command getCollectFromDepotCommand() {
+        return new ParallelCommandGroup(
+                SwerveCommands.getDriveToPoseCommand(() -> FieldConstants.DEPOT_POSITION, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
+                FuelIntakeCommands.getIntakeCommand()
+                //TODO: Add shooting command
+        ).withTimeout(AutonomousConstants.DEPOT_COLLECTION_TIMEOUT_SECONDS);
     }
 
-    private static Command getDriveToScoringLocationCommand(FlippablePose2d[] scoringLocations) {
+    public static Command getClimbCommand(FlippablePose2d climbPosition) {
         return new SequentialCommandGroup(
-                new InstantCommand(() -> TARGET_SCORING_POSE = calculateBestOpenScoringPose(scoringLocations)),
-                new WaitUntilCommand(() -> TARGET_SCORING_POSE != null).raceWith(SwerveCommands.getClosedLoopSelfRelativeDriveCommand(() -> 0, () -> 0, () -> 0)),
-                SwerveCommands.getDriveToPoseCommand(() -> TARGET_SCORING_POSE, AutonomousConstants.DRIVE_TO_SCORING_LOCATION_CONSTRAINTS).repeatedly()
+                SwerveCommands.getDriveToPoseCommand(() -> climbPosition, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
+                null // TODO: Add climb command
         );
     }
 
-    private static FlippablePose2d calculateBestOpenScoringPose(FlippablePose2d[] scoringLocations) {
-        //TODO: implement
-        return null;
-    }
-
-    /**
-     * Generates a name for the dynamic autonomous routine based on the intake and scoring locations.
-     *
-     * @param intakeLocations  the intake locations
-     * @param scoringLocations the scoring locations
-     * @return the name of the dynamic autonomous routine
-     */
-    private static String generateDynamicAutonomousRoutineName(FlippablePose2d[] intakeLocations, FlippablePose2d[] scoringLocations) {
-        //TODO: implement
-        return "";
+    private static boolean isRight() {
+        return RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation().getY() < FieldConstants.FIELD_WIDTH_METERS / 2;
     }
 
     /**
diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
index 7842e1da..bdb645cf 100644
--- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
+++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
@@ -11,12 +11,17 @@
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.trigon.lib.hardware.RobotHardwareStats;
 import frc.trigon.lib.utilities.LocalADStarAK;
 import frc.trigon.lib.utilities.flippable.Flippable;
+import frc.trigon.lib.utilities.flippable.FlippablePose2d;
 import frc.trigon.robot.RobotContainer;
+import frc.trigon.robot.commands.commandfactories.AutonomousCommands;
 import org.json.simple.parser.ParseException;
+import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
 import java.io.IOException;
 
@@ -27,7 +32,20 @@ public class AutonomousConstants {
     public static final String DEFAULT_AUTO_NAME = "DefaultAutoName";
     public static final RobotConfig ROBOT_CONFIG = getRobotConfig();
     public static final double FEEDFORWARD_SCALAR = 0.5;//TODO: Calibrate
-    public static final PathConstraints DRIVE_TO_SCORING_LOCATION_CONSTRAINTS = new PathConstraints(2.5, 4.5, Units.degreesToRadians(450), Units.degreesToRadians(900));
+    public static final PathConstraints
+            DRIVE_IN_AUTONOMOUS_CONSTRAINTS = new PathConstraints(2.5, 4.5, Units.degreesToRadians(450), Units.degreesToRadians(900)),
+            DRIVE_SLOWLY_IN_AUTONOMOUS_CONSTRAINTS = new PathConstraints(1.5, 3.0, Units.degreesToRadians(300), Units.degreesToRadians(600));
+    public static LoggedDashboardChooser<Command>
+            FIRST_AUTONOMOUS_CHOOSER = new LoggedDashboardChooser<>("FirstAutonomousChooser", new SendableChooser<>()),
+            SECOND_AUTONOMOUS_CHOOSER = new LoggedDashboardChooser<>("SecondAutonomousChooser", new SendableChooser<>()),
+            THIRD_AUTONOMOUS_CHOOSER = new LoggedDashboardChooser<>("ThirdAutonomousChooser", new SendableChooser<>());
+    public static LoggedDashboardChooser<FlippablePose2d> CLIMB_POSITION_CHOOSER = new LoggedDashboardChooser<>("ClimbChooser", new SendableChooser<>());
+
+    public static double
+            DEPOT_COLLECTION_TIMEOUT_SECONDS = 4,
+            NEUTRAL_ZONE_COLLECTION_TIMEOUT_SECONDS = 10,
+            DELIVERY_TIMEOUT_SECONDS = 10,
+            TIME_TO_CLIMB_BEFORE_AUTO_ENDS_SECONDS = 3;
 
     private static final PIDConstants
             AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ?
@@ -59,6 +77,8 @@ public static void init() {
         CommandScheduler.getInstance().schedule(PathfindingCommand.warmupCommand());
         configureAutoBuilder();
         registerCommands();
+        initializeAutoChoosers();
+
     }
 
     private static void configureAutoBuilder() {
@@ -82,6 +102,28 @@ private static RobotConfig getRobotConfig() {
         }
     }
 
+    private static void initializeAutoChoosers() {
+        configureClimbPositionChooser();
+        configureAutonomousPositionChooser(FIRST_AUTONOMOUS_CHOOSER);
+        configureAutonomousPositionChooser(SECOND_AUTONOMOUS_CHOOSER);
+        configureAutonomousPositionChooser(THIRD_AUTONOMOUS_CHOOSER);
+    }
+
+    private static void configureAutonomousPositionChooser(LoggedDashboardChooser<Command> firstAutonomousChooser) {
+        firstAutonomousChooser.addOption("Depot", AutonomousCommands.getCollectFromDepotCommand());
+        firstAutonomousChooser.addOption("Score", AutonomousCommands.getScoreCommand());
+        firstAutonomousChooser.addOption("CollectFromNeutralZone", AutonomousCommands.getCollectFromNeutralZoneCommand());
+        firstAutonomousChooser.addOption("Delivery", AutonomousCommands.getDeliveryCommand());
+        firstAutonomousChooser.addDefaultOption("Nothing", null);
+    }
+
+    private static void configureClimbPositionChooser() {
+        CLIMB_POSITION_CHOOSER.addOption("LeftClimb", FieldConstants.LEFT_CLIMB_POSITION);
+        CLIMB_POSITION_CHOOSER.addOption("CenterClimb", FieldConstants.CENTER_CLIMB_POSITION);
+        CLIMB_POSITION_CHOOSER.addOption("RightClimb", FieldConstants.RIGHT_CLIMB_POSITION);
+        CLIMB_POSITION_CHOOSER.addDefaultOption("NoClimb", null);
+    }
+
     private static void registerCommands() {
         //TODO: Implement
     }
diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java
index ee1fc2ba..fcaa9a06 100644
--- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java
+++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java
@@ -4,11 +4,9 @@
 import edu.wpi.first.apriltag.AprilTag;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.*;
 import frc.trigon.lib.utilities.FilesHandler;
+import frc.trigon.lib.utilities.flippable.FlippablePose2d;
 import frc.trigon.lib.utilities.flippable.FlippableTranslation2d;
 
 import java.io.IOException;
@@ -33,6 +31,15 @@ public class FieldConstants {
     public static final HashMap<Integer, Pose3d> TAG_ID_TO_POSE = fieldLayoutToTagIDToPoseMap();
 
     public static final FlippableTranslation2d HUB_POSITION = new FlippableTranslation2d(4.636, 4, true); // TODO: Update to actual hub position
+    public static final FlippablePose2d
+            LEFT_CLIMB_POSITION = new FlippablePose2d(1.45, 4.25, Rotation2d.fromDegrees(0), true),
+            RIGHT_CLIMB_POSITION = new FlippablePose2d(LEFT_CLIMB_POSITION.get().getX(), 3.28, Rotation2d.fromDegrees(0), true),
+            CENTER_CLIMB_POSITION = new FlippablePose2d((LEFT_CLIMB_POSITION.get().getX() + LEFT_CLIMB_POSITION.get().getX()) / 2, LEFT_CLIMB_POSITION.get().getY(), Rotation2d.fromDegrees(0), true),
+            DEPOT_POSITION = new FlippablePose2d(0.9, 6, Rotation2d.fromDegrees(180), true),
+            LEFT_INTAKE_POSITION = new FlippablePose2d(6.2, 7, Rotation2d.fromDegrees(-30), true),
+            RIGHT_INTAKE_POSITION = new FlippablePose2d(LEFT_INTAKE_POSITION.get().getX(), FIELD_WIDTH_METERS - LEFT_INTAKE_POSITION.get().getY(), Rotation2d.fromDegrees(30), true),
+            LEFT_IDEAL_SHOOTING_POSITION = new FlippablePose2d(2.7, 5.8, Rotation2d.fromDegrees(0), true),
+            RIGHT_IDEAL_SHOOTING_POSITION = new FlippablePose2d(LEFT_IDEAL_SHOOTING_POSITION.get().getX(), FIELD_WIDTH_METERS - LEFT_IDEAL_SHOOTING_POSITION.get().getY(), Rotation2d.fromDegrees(0), true);
 
     private static AprilTagFieldLayout createAprilTagFieldLayout() {
         try {

From 5b53a6fd953396911b1ee9cafd0caf62af65c8e9 Mon Sep 17 00:00:00 2001
From: Nummun14 <142012009+Nummun14@users.noreply.github.com>
Date: Thu, 29 Jan 2026 23:32:17 +0200
Subject: [PATCH 02/12] dumb

---
 src/main/java/frc/trigon/robot/RobotContainer.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java
index b2fdefaf..5c88d1a8 100644
--- a/src/main/java/frc/trigon/robot/RobotContainer.java
+++ b/src/main/java/frc/trigon/robot/RobotContainer.java
@@ -62,7 +62,7 @@ public RobotContainer() {
      * @return the command to run in autonomous mode
      */
     public Command getAutonomousCommand() {
-        AutonomousCommands.getAutonomousCommand();
+        return AutonomousCommands.getAutonomousCommand();
     }
 
     private void configureBindings() {

From 8100afeaade8fd5d427fd2b6788f495dfa741601 Mon Sep 17 00:00:00 2001
From: Nummun14 <142012009+Nummun14@users.noreply.github.com>
Date: Thu, 29 Jan 2026 23:44:47 +0200
Subject: [PATCH 03/12] Added shooting commands

---
 .../commandfactories/AutonomousCommands.java  | 23 +++++++++++--------
 1 file changed, 14 insertions(+), 9 deletions(-)

diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
index 52a0255c..ee5ef276 100644
--- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
+++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
@@ -35,9 +35,9 @@ public static Command getDeliveryCommand() {
                 SwerveCommands.getDriveToPoseCommand(isRight() ? () -> FieldConstants.RIGHT_INTAKE_POSITION : () -> FieldConstants.LEFT_INTAKE_POSITION, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
                 FuelIntakeCommands.getIntakeCommand(),
                 GeneralCommands.getContinuousConditionalCommand(
-                        null, //TODO: getShootingCommand()
-                        null, //TODO: getDeliveryCommand()
-                        () -> false //TODO: isInAllianceZone()
+                        ShootingCommands.getShootAtHubCommand(),
+                        ShootingCommands.getDeliveryCommand(),
+                        AutonomousCommands::isInAllianceZone
                 )
         ).withTimeout(AutonomousConstants.DELIVERY_TIMEOUT_SECONDS);
     }
@@ -45,23 +45,23 @@ public static Command getDeliveryCommand() {
     public static Command getCollectFromNeutralZoneCommand() {
         return new ParallelCommandGroup(
                 SwerveCommands.getDriveToPoseCommand(isRight() ? () -> FieldConstants.RIGHT_INTAKE_POSITION : () -> FieldConstants.LEFT_INTAKE_POSITION, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
-                FuelIntakeCommands.getIntakeCommand()
-                //TODO: getShootingCommand().onlyIf(isInAllianceZone())
+                FuelIntakeCommands.getIntakeCommand(),
+                ShootingCommands.getShootAtHubCommand().onlyIf(AutonomousCommands::isInAllianceZone)
         ).withTimeout(AutonomousConstants.NEUTRAL_ZONE_COLLECTION_TIMEOUT_SECONDS);
     }
 
     public static Command getScoreCommand() {
         return new ParallelCommandGroup(
-                SwerveCommands.getDriveToPoseCommand(isRight() ? () -> FieldConstants.RIGHT_IDEAL_SHOOTING_POSITION : () -> FieldConstants.LEFT_IDEAL_SHOOTING_POSITION, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS)
-                //TODO: getShootingCommand().onlyIf(isInAllianceZone())
+                SwerveCommands.getDriveToPoseCommand(isRight() ? () -> FieldConstants.RIGHT_IDEAL_SHOOTING_POSITION : () -> FieldConstants.LEFT_IDEAL_SHOOTING_POSITION, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
+                ShootingCommands.getShootAtHubCommand().onlyIf(AutonomousCommands::isInAllianceZone)
         );
     }
 
     public static Command getCollectFromDepotCommand() {
         return new ParallelCommandGroup(
                 SwerveCommands.getDriveToPoseCommand(() -> FieldConstants.DEPOT_POSITION, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
-                FuelIntakeCommands.getIntakeCommand()
-                //TODO: Add shooting command
+                FuelIntakeCommands.getIntakeCommand(),
+                ShootingCommands.getShootAtHubCommand().onlyIf(AutonomousCommands::isInAllianceZone)
         ).withTimeout(AutonomousConstants.DEPOT_COLLECTION_TIMEOUT_SECONDS);
     }
 
@@ -76,6 +76,11 @@ private static boolean isRight() {
         return RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation().getY() < FieldConstants.FIELD_WIDTH_METERS / 2;
     }
 
+    private static boolean isInAllianceZone() {
+        final Pose2d currentRobotPose = new FlippablePose2d(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(), true).get();
+        return currentRobotPose.getX() < FieldConstants.ALLIANCE_ZONE_LENGTH;
+    }
+
     /**
      * Creates a command that resets the pose estimator's pose to the starting pose of the given autonomous as long as the robot is not enabled.
      *

From c235a7d49c450e29baf7a173ad7339fc5705b974 Mon Sep 17 00:00:00 2001
From: Nummun14 <142012009+Nummun14@users.noreply.github.com>
Date: Fri, 30 Jan 2026 01:45:13 +0200
Subject: [PATCH 04/12] logic changes and swerve cal. no work

---
 src/main/deploy/pathplanner/navgrid.json      |  2 +-
 .../java/frc/trigon/robot/RobotContainer.java |  2 +-
 .../commandfactories/AutonomousCommands.java  | 47 ++++++++++---------
 .../robot/constants/AutonomousConstants.java  | 20 ++++----
 .../swervemodule/SwerveModuleConstants.java   |  6 +--
 5 files changed, 39 insertions(+), 38 deletions(-)

diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
index 23e0db94..755a53a8 100644
--- a/src/main/deploy/pathplanner/navgrid.json
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -1 +1 @@
-{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
+{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false]]}
\ No newline at end of file
diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java
index 0ff49106..58fa6ccd 100644
--- a/src/main/java/frc/trigon/robot/RobotContainer.java
+++ b/src/main/java/frc/trigon/robot/RobotContainer.java
@@ -69,7 +69,7 @@ public Command getAutonomousCommand() {
     private void configureBindings() {
         bindDefaultCommands();
         bindControllerCommands();
-//        configureSysIDBindings(TURRET);
+//        configureSysIDBindings(SWERVE);
     }
 
     private void bindDefaultCommands() {
diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
index ee5ef276..0a7ad46f 100644
--- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
+++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
@@ -3,14 +3,14 @@
 import com.pathplanner.lib.commands.PathPlannerAuto;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.*;
 import frc.trigon.lib.utilities.flippable.FlippablePose2d;
 import frc.trigon.robot.RobotContainer;
+import frc.trigon.robot.commands.commandclasses.GamePieceAutoDriveCommand;
 import frc.trigon.robot.constants.AutonomousConstants;
 import frc.trigon.robot.constants.FieldConstants;
+import frc.trigon.robot.subsystems.intake.IntakeCommands;
+import frc.trigon.robot.subsystems.intake.IntakeConstants;
 import frc.trigon.robot.subsystems.swerve.SwerveCommands;
 import org.json.simple.parser.ParseException;
 
@@ -23,9 +23,9 @@
 public class AutonomousCommands {
     public static Command getAutonomousCommand() {
         return new SequentialCommandGroup(
-                AutonomousConstants.FIRST_AUTONOMOUS_CHOOSER.get() == null ? new InstantCommand() : AutonomousConstants.FIRST_AUTONOMOUS_CHOOSER.get(),
-                AutonomousConstants.SECOND_AUTONOMOUS_CHOOSER.get() == null ? new InstantCommand() : AutonomousConstants.SECOND_AUTONOMOUS_CHOOSER.get(),
-                AutonomousConstants.THIRD_AUTONOMOUS_CHOOSER.get() == null ? new InstantCommand() : AutonomousConstants.THIRD_AUTONOMOUS_CHOOSER.get(),
+                AutonomousConstants.FIRST_AUTONOMOUS_CHOOSER.get() == null ? new InstantCommand() : AutonomousConstants.FIRST_AUTONOMOUS_CHOOSER.get().get(),
+                AutonomousConstants.SECOND_AUTONOMOUS_CHOOSER.get() == null ? new InstantCommand() : AutonomousConstants.SECOND_AUTONOMOUS_CHOOSER.get().get(),
+                AutonomousConstants.THIRD_AUTONOMOUS_CHOOSER.get() == null ? new InstantCommand() : AutonomousConstants.THIRD_AUTONOMOUS_CHOOSER.get().get(),
                 getClimbCommand(AutonomousConstants.CLIMB_POSITION_CHOOSER.get()).onlyIf(() -> AutonomousConstants.CLIMB_POSITION_CHOOSER.get() != null)
         );
     }
@@ -33,7 +33,7 @@ public static Command getAutonomousCommand() {
     public static Command getDeliveryCommand() {
         return new ParallelCommandGroup(
                 SwerveCommands.getDriveToPoseCommand(isRight() ? () -> FieldConstants.RIGHT_INTAKE_POSITION : () -> FieldConstants.LEFT_INTAKE_POSITION, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
-                FuelIntakeCommands.getIntakeCommand(),
+                IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.INTAKE),
                 GeneralCommands.getContinuousConditionalCommand(
                         ShootingCommands.getShootAtHubCommand(),
                         ShootingCommands.getDeliveryCommand(),
@@ -43,32 +43,37 @@ public static Command getDeliveryCommand() {
     }
 
     public static Command getCollectFromNeutralZoneCommand() {
-        return new ParallelCommandGroup(
-                SwerveCommands.getDriveToPoseCommand(isRight() ? () -> FieldConstants.RIGHT_INTAKE_POSITION : () -> FieldConstants.LEFT_INTAKE_POSITION, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
-                FuelIntakeCommands.getIntakeCommand(),
-                ShootingCommands.getShootAtHubCommand().onlyIf(AutonomousCommands::isInAllianceZone)
-        ).withTimeout(AutonomousConstants.NEUTRAL_ZONE_COLLECTION_TIMEOUT_SECONDS);
+        return new SequentialCommandGroup(
+                new ParallelCommandGroup(
+                        new PrintCommand("Starting Neutral Zone Collection"),
+                        SwerveCommands.getDriveToPoseCommand(isRight() ? () -> FieldConstants.RIGHT_INTAKE_POSITION : () -> FieldConstants.LEFT_INTAKE_POSITION, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
+                        ShootingCommands.getShootAtHubCommand().onlyWhile(AutonomousCommands::isInAllianceZone)
+                ).until(() -> RobotContainer.SWERVE.atPose(isRight() ? FieldConstants.RIGHT_INTAKE_POSITION : FieldConstants.LEFT_INTAKE_POSITION)),
+                new GamePieceAutoDriveCommand()
+        ).alongWith(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.INTAKE)).withTimeout(AutonomousConstants.NEUTRAL_ZONE_COLLECTION_TIMEOUT_SECONDS);
     }
 
     public static Command getScoreCommand() {
         return new ParallelCommandGroup(
                 SwerveCommands.getDriveToPoseCommand(isRight() ? () -> FieldConstants.RIGHT_IDEAL_SHOOTING_POSITION : () -> FieldConstants.LEFT_IDEAL_SHOOTING_POSITION, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
-                ShootingCommands.getShootAtHubCommand().onlyIf(AutonomousCommands::isInAllianceZone)
-        );
+                ShootingCommands.getShootAtHubCommand().onlyWhile(AutonomousCommands::isInAllianceZone)
+        ).withTimeout(AutonomousConstants.SCORING_TIMEOUT_SECONDS);
     }
 
     public static Command getCollectFromDepotCommand() {
-        return new ParallelCommandGroup(
-                SwerveCommands.getDriveToPoseCommand(() -> FieldConstants.DEPOT_POSITION, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
-                FuelIntakeCommands.getIntakeCommand(),
-                ShootingCommands.getShootAtHubCommand().onlyIf(AutonomousCommands::isInAllianceZone)
-        ).withTimeout(AutonomousConstants.DEPOT_COLLECTION_TIMEOUT_SECONDS);
+        return new SequentialCommandGroup(
+                new ParallelCommandGroup(
+                        SwerveCommands.getDriveToPoseCommand(() -> FieldConstants.DEPOT_POSITION, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
+                        ShootingCommands.getShootAtHubCommand().onlyWhile(AutonomousCommands::isInAllianceZone)
+                ),
+                new GamePieceAutoDriveCommand()
+        ).alongWith(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.INTAKE)).withTimeout(AutonomousConstants.DEPOT_COLLECTION_TIMEOUT_SECONDS);
     }
 
     public static Command getClimbCommand(FlippablePose2d climbPosition) {
         return new SequentialCommandGroup(
                 SwerveCommands.getDriveToPoseCommand(() -> climbPosition, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
-                null // TODO: Add climb command
+                new InstantCommand() // TODO: Add climb command
         );
     }
 
diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
index bdb645cf..33df9499 100644
--- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
+++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
@@ -24,6 +24,7 @@
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
 import java.io.IOException;
+import java.util.function.Supplier;
 
 /**
  * A class that contains the constants and configurations for everything related to the 15-second autonomous period at the start of the match.
@@ -35,7 +36,7 @@ public class AutonomousConstants {
     public static final PathConstraints
             DRIVE_IN_AUTONOMOUS_CONSTRAINTS = new PathConstraints(2.5, 4.5, Units.degreesToRadians(450), Units.degreesToRadians(900)),
             DRIVE_SLOWLY_IN_AUTONOMOUS_CONSTRAINTS = new PathConstraints(1.5, 3.0, Units.degreesToRadians(300), Units.degreesToRadians(600));
-    public static LoggedDashboardChooser<Command>
+    public static LoggedDashboardChooser<Supplier<Command>>
             FIRST_AUTONOMOUS_CHOOSER = new LoggedDashboardChooser<>("FirstAutonomousChooser", new SendableChooser<>()),
             SECOND_AUTONOMOUS_CHOOSER = new LoggedDashboardChooser<>("SecondAutonomousChooser", new SendableChooser<>()),
             THIRD_AUTONOMOUS_CHOOSER = new LoggedDashboardChooser<>("ThirdAutonomousChooser", new SendableChooser<>());
@@ -45,7 +46,7 @@ public class AutonomousConstants {
             DEPOT_COLLECTION_TIMEOUT_SECONDS = 4,
             NEUTRAL_ZONE_COLLECTION_TIMEOUT_SECONDS = 10,
             DELIVERY_TIMEOUT_SECONDS = 10,
-            TIME_TO_CLIMB_BEFORE_AUTO_ENDS_SECONDS = 3;
+            SCORING_TIMEOUT_SECONDS = 6;
 
     private static final PIDConstants
             AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ?
@@ -76,7 +77,6 @@ public static void init() {
         Pathfinding.setPathfinder(new LocalADStarAK());
         CommandScheduler.getInstance().schedule(PathfindingCommand.warmupCommand());
         configureAutoBuilder();
-        registerCommands();
         initializeAutoChoosers();
 
     }
@@ -109,11 +109,11 @@ private static void initializeAutoChoosers() {
         configureAutonomousPositionChooser(THIRD_AUTONOMOUS_CHOOSER);
     }
 
-    private static void configureAutonomousPositionChooser(LoggedDashboardChooser<Command> firstAutonomousChooser) {
-        firstAutonomousChooser.addOption("Depot", AutonomousCommands.getCollectFromDepotCommand());
-        firstAutonomousChooser.addOption("Score", AutonomousCommands.getScoreCommand());
-        firstAutonomousChooser.addOption("CollectFromNeutralZone", AutonomousCommands.getCollectFromNeutralZoneCommand());
-        firstAutonomousChooser.addOption("Delivery", AutonomousCommands.getDeliveryCommand());
+    private static void configureAutonomousPositionChooser(LoggedDashboardChooser<Supplier<Command>> firstAutonomousChooser) {
+        firstAutonomousChooser.addOption("Depot", AutonomousCommands::getCollectFromDepotCommand);
+        firstAutonomousChooser.addOption("Score", AutonomousCommands::getScoreCommand);
+        firstAutonomousChooser.addOption("CollectFromNeutralZone", AutonomousCommands::getCollectFromNeutralZoneCommand);
+        firstAutonomousChooser.addOption("Delivery", AutonomousCommands::getDeliveryCommand);
         firstAutonomousChooser.addDefaultOption("Nothing", null);
     }
 
@@ -123,8 +123,4 @@ private static void configureClimbPositionChooser() {
         CLIMB_POSITION_CHOOSER.addOption("RightClimb", FieldConstants.RIGHT_CLIMB_POSITION);
         CLIMB_POSITION_CHOOSER.addDefaultOption("NoClimb", null);
     }
-
-    private static void registerCommands() {
-        //TODO: Implement
-    }
 }
\ No newline at end of file
diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java
index 5a0b67c1..294f4a00 100644
--- a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java
+++ b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java
@@ -80,9 +80,9 @@ static TalonFXConfiguration generateDriveMotorConfiguration() {
         config.Slot0.kP = RobotHardwareStats.isSimulation() ? 0 : 0;
         config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0;
         config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 0;
-        config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016046 : 0;
-        config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.8774 : 0;
-        config.Slot0.kA = RobotHardwareStats.isSimulation() ? 00.020691 : 0;
+        config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.0010955 : 0;
+        config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.74583 : 0;
+        config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.01247 : 0;
 
         config.Feedback.VelocityFilterTimeConstant = 0;
 

From a45a06c1064d12762cc4fd2820603b2ab525a7f5 Mon Sep 17 00:00:00 2001
From: Nummun14 <142012009+Nummun14@users.noreply.github.com>
Date: Fri, 30 Jan 2026 03:07:45 +0200
Subject: [PATCH 05/12] fixed some bugs, found 50 others. I am stupid (charles
 leclerc!!!)

---
 src/main/deploy/pathplanner/navgrid.json              |  2 +-
 src/main/deploy/pathplanner/settings.json             |  2 +-
 src/main/java/frc/trigon/lib                          |  2 +-
 .../commands/commandclasses/IntakeAssistCommand.java  |  2 +-
 .../commands/commandfactories/AutonomousCommands.java | 11 +++++------
 .../trigon/robot/constants/AutonomousConstants.java   |  8 ++++----
 .../frc/trigon/robot/constants/FieldConstants.java    |  8 ++++----
 .../swerve/swervemodule/SwerveModuleConstants.java    |  2 +-
 8 files changed, 18 insertions(+), 19 deletions(-)

diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
index 755a53a8..a43d0437 100644
--- a/src/main/deploy/pathplanner/navgrid.json
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -1 +1 @@
-{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false]]}
\ No newline at end of file
+{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
index a8ea08de..31fd213b 100644
--- a/src/main/deploy/pathplanner/settings.json
+++ b/src/main/deploy/pathplanner/settings.json
@@ -15,7 +15,7 @@
   "robotMOI": 5.018,
   "robotTrackwidth": 0.546,
   "driveWheelRadius": 0.049,
-  "driveGearing": 6.12,
+  "driveGearing": 7.03,
   "maxDriveSpeed": 4.4,
   "driveMotorType": "krakenX60FOC",
   "driveCurrentLimit": 100.0,
diff --git a/src/main/java/frc/trigon/lib b/src/main/java/frc/trigon/lib
index 6b6c7adb..9d24e820 160000
--- a/src/main/java/frc/trigon/lib
+++ b/src/main/java/frc/trigon/lib
@@ -1 +1 @@
-Subproject commit 6b6c7adbf9d1111fa67fdc0b801e0520044bc7f8
+Subproject commit 9d24e8203d6665b1a88361f773028b1d27fb8145
diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java
index fce03ab1..7383aeaf 100644
--- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java
+++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java
@@ -178,7 +178,7 @@ private double calculateMaximumAssistAngleDegrees(double gamePieceDistance) {
     private double calculateVelocityTowardsGamePiece(Translation2d fieldRelativeDistanceFromGamePiece) {
         final Translation2d fieldRelativeJoystickPosition = getFieldRelativeJoystickPosition();
 
-        final Rotation2d joystickAngle = fieldRelativeJoystickPosition.getAngle();
+        final Rotation2d joystickAngle = fieldRelativeJoystickPosition.getNorm() < 1e-6 ? new Rotation2d() : fieldRelativeJoystickPosition.getAngle();
         final Rotation2d fieldRelativeGamePieceAngleFromRobot = fieldRelativeDistanceFromGamePiece.getAngle();
         final Rotation2d angularOffset = joystickAngle.minus(fieldRelativeGamePieceAngleFromRobot);
 
diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
index 0a7ad46f..1fa8484e 100644
--- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
+++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
@@ -6,7 +6,7 @@
 import edu.wpi.first.wpilibj2.command.*;
 import frc.trigon.lib.utilities.flippable.FlippablePose2d;
 import frc.trigon.robot.RobotContainer;
-import frc.trigon.robot.commands.commandclasses.GamePieceAutoDriveCommand;
+import frc.trigon.robot.commands.commandclasses.IntakeAssistCommand;
 import frc.trigon.robot.constants.AutonomousConstants;
 import frc.trigon.robot.constants.FieldConstants;
 import frc.trigon.robot.subsystems.intake.IntakeCommands;
@@ -45,17 +45,16 @@ public static Command getDeliveryCommand() {
     public static Command getCollectFromNeutralZoneCommand() {
         return new SequentialCommandGroup(
                 new ParallelCommandGroup(
-                        new PrintCommand("Starting Neutral Zone Collection"),
                         SwerveCommands.getDriveToPoseCommand(isRight() ? () -> FieldConstants.RIGHT_INTAKE_POSITION : () -> FieldConstants.LEFT_INTAKE_POSITION, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
                         ShootingCommands.getShootAtHubCommand().onlyWhile(AutonomousCommands::isInAllianceZone)
                 ).until(() -> RobotContainer.SWERVE.atPose(isRight() ? FieldConstants.RIGHT_INTAKE_POSITION : FieldConstants.LEFT_INTAKE_POSITION)),
-                new GamePieceAutoDriveCommand()
+                new IntakeAssistCommand(1, 1, 1).alongWith(new PrintCommand("intakingmoving"))
         ).alongWith(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.INTAKE)).withTimeout(AutonomousConstants.NEUTRAL_ZONE_COLLECTION_TIMEOUT_SECONDS);
     }
 
     public static Command getScoreCommand() {
         return new ParallelCommandGroup(
-                SwerveCommands.getDriveToPoseCommand(isRight() ? () -> FieldConstants.RIGHT_IDEAL_SHOOTING_POSITION : () -> FieldConstants.LEFT_IDEAL_SHOOTING_POSITION, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
+                SwerveCommands.getDriveToPoseCommand(() -> isRight() ? FieldConstants.RIGHT_IDEAL_SHOOTING_POSITION : FieldConstants.LEFT_IDEAL_SHOOTING_POSITION, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
                 ShootingCommands.getShootAtHubCommand().onlyWhile(AutonomousCommands::isInAllianceZone)
         ).withTimeout(AutonomousConstants.SCORING_TIMEOUT_SECONDS);
     }
@@ -65,8 +64,8 @@ public static Command getCollectFromDepotCommand() {
                 new ParallelCommandGroup(
                         SwerveCommands.getDriveToPoseCommand(() -> FieldConstants.DEPOT_POSITION, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
                         ShootingCommands.getShootAtHubCommand().onlyWhile(AutonomousCommands::isInAllianceZone)
-                ),
-                new GamePieceAutoDriveCommand()
+                ).until(() -> RobotContainer.SWERVE.atPose(FieldConstants.DEPOT_POSITION)),
+                new IntakeAssistCommand(1, 1, 1).alongWith(new PrintCommand("intakingmoving"))
         ).alongWith(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.INTAKE)).withTimeout(AutonomousConstants.DEPOT_COLLECTION_TIMEOUT_SECONDS);
     }
 
diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
index 33df9499..ced13517 100644
--- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
+++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
@@ -44,16 +44,16 @@ public class AutonomousConstants {
 
     public static double
             DEPOT_COLLECTION_TIMEOUT_SECONDS = 4,
-            NEUTRAL_ZONE_COLLECTION_TIMEOUT_SECONDS = 10,
-            DELIVERY_TIMEOUT_SECONDS = 10,
+            NEUTRAL_ZONE_COLLECTION_TIMEOUT_SECONDS = 8,
+            DELIVERY_TIMEOUT_SECONDS = 8,
             SCORING_TIMEOUT_SECONDS = 6;
 
     private static final PIDConstants
             AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ?
-            new PIDConstants(0, 0, 0) :
+            new PIDConstants(5, 0, 0) :
             new PIDConstants(0, 0, 0),
             AUTO_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ?
-                    new PIDConstants(0, 0, 0) :
+                    new PIDConstants(2, 0, 0) :
                     new PIDConstants(0, 0, 0);
 
 
diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java
index d1a1372c..9dcd9d31 100644
--- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java
+++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java
@@ -31,13 +31,13 @@ public class FieldConstants {
 
     public static final FlippablePose2d
             LEFT_CLIMB_POSITION = new FlippablePose2d(1.45, 4.25, Rotation2d.fromDegrees(0), true),
-            RIGHT_CLIMB_POSITION = new FlippablePose2d(LEFT_CLIMB_POSITION.get().getX(), 3.28, Rotation2d.fromDegrees(0), true),
-            CENTER_CLIMB_POSITION = new FlippablePose2d((LEFT_CLIMB_POSITION.get().getX() + LEFT_CLIMB_POSITION.get().getX()) / 2, LEFT_CLIMB_POSITION.get().getY(), Rotation2d.fromDegrees(0), true),
+            RIGHT_CLIMB_POSITION = new FlippablePose2d(LEFT_CLIMB_POSITION.getBlueObject().getX(), 3.28, Rotation2d.fromDegrees(0), true),
+            CENTER_CLIMB_POSITION = new FlippablePose2d((LEFT_CLIMB_POSITION.getBlueObject().getX() + LEFT_CLIMB_POSITION.getBlueObject().getX()) / 2, LEFT_CLIMB_POSITION.getBlueObject().getY(), Rotation2d.fromDegrees(0), true),
             DEPOT_POSITION = new FlippablePose2d(0.9, 6, Rotation2d.fromDegrees(180), true),
             LEFT_INTAKE_POSITION = new FlippablePose2d(6.2, 7, Rotation2d.fromDegrees(-30), true),
-            RIGHT_INTAKE_POSITION = new FlippablePose2d(LEFT_INTAKE_POSITION.get().getX(), FIELD_WIDTH_METERS - LEFT_INTAKE_POSITION.get().getY(), Rotation2d.fromDegrees(30), true),
+            RIGHT_INTAKE_POSITION = new FlippablePose2d(LEFT_INTAKE_POSITION.getBlueObject().getX(), FIELD_WIDTH_METERS - LEFT_INTAKE_POSITION.getBlueObject().getY(), Rotation2d.fromDegrees(30), true),
             LEFT_IDEAL_SHOOTING_POSITION = new FlippablePose2d(2.7, 5.8, Rotation2d.fromDegrees(0), true),
-            RIGHT_IDEAL_SHOOTING_POSITION = new FlippablePose2d(LEFT_IDEAL_SHOOTING_POSITION.get().getX(), FIELD_WIDTH_METERS - LEFT_IDEAL_SHOOTING_POSITION.get().getY(), Rotation2d.fromDegrees(0), true);
+            RIGHT_IDEAL_SHOOTING_POSITION = new FlippablePose2d(LEFT_IDEAL_SHOOTING_POSITION.getBlueObject().getX(), FIELD_WIDTH_METERS - LEFT_IDEAL_SHOOTING_POSITION.getBlueObject().getY(), Rotation2d.fromDegrees(0), true);
     private static final double
             BLUE_RELATIVE_DELIVERY_POSITION_X = 3,
             DELIVERY_POSITION_Y_OFFSET_FROM_CENTER_METERS = 2.2;
diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java
index 294f4a00..4aa05fad 100644
--- a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java
+++ b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java
@@ -15,7 +15,7 @@
 
 public class SwerveModuleConstants {
     private static final double
-            DRIVE_MOTOR_GEAR_RATIO = 6.03,//R1: 7.03, R2: 6.03, R3: 5.27
+            DRIVE_MOTOR_GEAR_RATIO = 7.03,//R1: 7.03, R2: 6.03, R3: 5.27
             STEER_MOTOR_GEAR_RATIO = 287.0 / 11.0;
     static final boolean ENABLE_FOC = true;
 

From b9c0cf63b9a731b2b070c953a0b73e6ff302674b Mon Sep 17 00:00:00 2001
From: Nummun14 <142012009+Nummun14@users.noreply.github.com>
Date: Fri, 30 Jan 2026 05:44:58 +0200
Subject: [PATCH 06/12] GamePieceauto drive command works. Go vibe coding

---
 .../GamePieceAutoDriveCommand.java            |  77 -------
 .../commandclasses/IntakeAssistCommand.java   |   3 +-
 .../GamePieceAutoDriveCommand.java            | 190 ++++++++++++++++++
 .../GamePieceAutoDriveConstants.java          |  13 ++
 .../gamepieceautodrive/GamePieceCluster.java  | 115 +++++++++++
 .../commandfactories/AutonomousCommands.java  |  22 +-
 .../robot/constants/AutonomousConstants.java  |  12 +-
 .../robot/constants/FieldConstants.java       |   6 +-
 .../robot/subsystems/swerve/Swerve.java       |   1 +
 9 files changed, 344 insertions(+), 95 deletions(-)
 delete mode 100644 src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java
 create mode 100644 src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveCommand.java
 create mode 100644 src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveConstants.java
 create mode 100644 src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceCluster.java

diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java
deleted file mode 100644
index 51976806..00000000
--- a/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java
+++ /dev/null
@@ -1,77 +0,0 @@
-package frc.trigon.robot.commands.commandclasses;
-
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj2.command.*;
-import frc.trigon.lib.utilities.flippable.FlippableRotation2d;
-import frc.trigon.robot.RobotContainer;
-import frc.trigon.robot.commands.commandfactories.GeneralCommands;
-import frc.trigon.robot.constants.AutonomousConstants;
-import frc.trigon.robot.misc.objectdetection.ObjectPoseEstimator;
-import frc.trigon.robot.subsystems.swerve.SwerveCommands;
-import org.littletonrobotics.junction.Logger;
-
-import java.util.function.Supplier;
-
-public class GamePieceAutoDriveCommand extends ParallelCommandGroup {
-    private static final ObjectPoseEstimator OBJECT_POSE_ESTIMATOR = RobotContainer.OBJECT_POSE_ESTIMATOR;
-    private Translation2d distanceFromTrackedGamePiece;
-
-    public GamePieceAutoDriveCommand() {
-        addCommands(
-                getTrackGamePieceCommand(),
-                GeneralCommands.getContinuousConditionalCommand(
-                        getDriveToGamePieceCommand(() -> distanceFromTrackedGamePiece),
-                        GeneralCommands.getFieldRelativeDriveCommand(),
-                        () -> OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot() != null && shouldMoveTowardsGamePiece(distanceFromTrackedGamePiece)
-                )
-        );
-    }
-
-    private Command getTrackGamePieceCommand() {
-        return new RunCommand(() -> {
-            distanceFromTrackedGamePiece = calculateDistanceFromTrackedGamePiece();
-        });
-    }
-
-    public static Translation2d calculateDistanceFromTrackedGamePiece() {
-        final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
-        final Translation2d trackedObjectPositionOnField = OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot();
-        if (trackedObjectPositionOnField == null)
-            return null;
-
-        final Translation2d robotToGamePiece = robotPose.getTranslation().minus(trackedObjectPositionOnField);
-        var distanceFromTrackedGamePiece = robotToGamePiece.rotateBy(robotPose.getRotation().unaryMinus());
-        Logger.recordOutput("GamePieceAutoDrive/DistanceFromTrackedGamePiece", distanceFromTrackedGamePiece);
-        Logger.recordOutput("GamePieceAutoDrive/XDistanceFromTrackedGamePiece", AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.getSetpoint().position);
-        return distanceFromTrackedGamePiece;
-    }
-
-    public static Command getDriveToGamePieceCommand(Supplier<Translation2d> distanceFromTrackedGamePiece) {
-        return new SequentialCommandGroup(
-                new InstantCommand(() -> AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.get().getX(), RobotContainer.SWERVE.getSelfRelativeVelocity().getX())),
-                SwerveCommands.getClosedLoopSelfRelativeDriveCommand(
-                        () -> AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.get().getX()),
-                        () -> AutonomousConstants.GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.get().getY()),
-                        GamePieceAutoDriveCommand::calculateTargetAngle
-                )
-        );
-    }
-
-    public static boolean shouldMoveTowardsGamePiece(Translation2d distanceFromTrackedGamePiece) {
-        return distanceFromTrackedGamePiece != null &&
-                (distanceFromTrackedGamePiece.getNorm() > AutonomousConstants.AUTO_COLLECTION_INTAKE_OPEN_CHECK_DISTANCE_METERS);//TODO: If intake is open
-    }
-
-    public static FlippableRotation2d calculateTargetAngle() {
-        final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
-        final Translation2d trackedObjectFieldRelativePosition = OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot();
-        if (trackedObjectFieldRelativePosition == null)
-            return null;
-
-        final Translation2d objectDistanceFromRobot = trackedObjectFieldRelativePosition.minus(robotPose.getTranslation());
-        final Rotation2d angularDistanceToObject = new Rotation2d(Math.atan2(objectDistanceFromRobot.getY(), objectDistanceFromRobot.getX()));
-        return new FlippableRotation2d(angularDistanceToObject, false);
-    }
-}
\ No newline at end of file
diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java
index 7383aeaf..182e8955 100644
--- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java
+++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java
@@ -145,7 +145,8 @@ private Translation2d getClosestGamePiece(Translation2d robotPosition, ArrayList
 
     private ArrayList<Translation2d> getAssistableGamePieces(Pose2d robotPose) {
         final ArrayList<Translation2d> gamePiecePositionsOnField = new ArrayList<>(RobotContainer.OBJECT_POSE_ESTIMATOR.getObjectsOnField());
-        gamePiecePositionsOnField.removeIf(gamePiecePosition -> !isGamePieceAssistable(gamePiecePosition, robotPose));
+//        if (!DriverStation.isAutonomous())
+//            gamePiecePositionsOnField.removeIf(gamePiecePosition -> !isGamePieceAssistable(gamePiecePosition, robotPose));
 
         return gamePiecePositionsOnField;
     }
diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveCommand.java
new file mode 100644
index 00000000..9b38413a
--- /dev/null
+++ b/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveCommand.java
@@ -0,0 +1,190 @@
+package frc.trigon.robot.commands.commandclasses.gamepieceautodrive;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.*;
+import frc.trigon.lib.utilities.flippable.FlippableRotation2d;
+import frc.trigon.robot.RobotContainer;
+import frc.trigon.robot.constants.AutonomousConstants;
+import frc.trigon.robot.misc.objectdetection.ObjectPoseEstimator;
+import frc.trigon.robot.subsystems.swerve.SwerveCommands;
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.concurrent.atomic.AtomicReference;
+
+public class GamePieceAutoDriveCommand extends ParallelCommandGroup {
+    private static final ObjectPoseEstimator OBJECT_POSE_ESTIMATOR = RobotContainer.OBJECT_POSE_ESTIMATOR;
+    private final AtomicReference<GamePieceCluster> currentTargetCluster = new AtomicReference<>();
+
+    public GamePieceAutoDriveCommand() {
+        addCommands(
+                createTargetUpdateCommand(),
+                createDriveCommand()
+        );
+    }
+
+    /**
+     * Creates a command that periodically calculates the best cluster and updates the atomic reference.
+     * This acts as the "vision processing" thread for this command group.
+     *
+     * @return A {@link Command} that runs continuously to update the target.
+     */
+    private Command createTargetUpdateCommand() {
+        return new RunCommand(() -> {
+            GamePieceCluster bestCluster = findBestCluster();
+            currentTargetCluster.set(bestCluster);
+        });
+    }
+
+    /**
+     * Creates the drive command that moves the robot towards the currently selected target cluster.
+     *
+     * @return A {@link Command} that executes the swerve drive logic.
+     */
+    private Command createDriveCommand() {
+        return new SequentialCommandGroup(
+                new InstantCommand(AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER::reset),
+                SwerveCommands.getClosedLoopSelfRelativeDriveCommand(
+                        this::getXControllerOutput,
+                        this::getYControllerOutput,
+                        this::getTargetHeading
+                )
+        ).onlyWhile(this::hasValidTarget);
+    }
+
+    /**
+     * Calculates the output for the X-axis PID controller based on the current target.
+     *
+     * @return The calculated output for the X-axis velocity, or 0.0 if no target exists.
+     */
+    private double getXControllerOutput() {
+        Translation2d error = getTranslationError();
+        if (error == null)
+            return 0.0;
+        return AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.calculate(error.getX());
+    }
+
+    /**
+     * Calculates the output for the Y-axis PID controller based on the current target.
+     *
+     * @return The calculated output for the Y-axis velocity, or 0.0 if no target exists.
+     */
+    private double getYControllerOutput() {
+        Translation2d error = getTranslationError();
+        if (error == null) return 0.0;
+        return AutonomousConstants.GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER.calculate(error.getY());
+    }
+
+    /**
+     * Retrieves the target heading for the robot based on the selected cluster's approach angle.
+     *
+     * @return A {@link FlippableRotation2d} representing the desired heading, or null if no target exists.
+     */
+    private FlippableRotation2d getTargetHeading() {
+        GamePieceCluster cluster = currentTargetCluster.get();
+        if (cluster == null)
+            return null;
+        return new FlippableRotation2d(cluster.getApproachHeading(), false);
+    }
+
+    /**
+     * Calculates the vector from the robot to the target centroid, rotated to be robot-relative.
+     * This is used as the error input for the self-relative PID controllers.
+     *
+     * @return A {@link Translation2d} representing the distance error in robot-relative coordinates, or null if no target exists.
+     */
+    private Translation2d getTranslationError() {
+        GamePieceCluster cluster = currentTargetCluster.get();
+        if (cluster == null)
+            return null;
+
+        Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
+        Translation2d fieldRelativeError = cluster.getCentroid().minus(robotPose.getTranslation());
+
+        return fieldRelativeError.rotateBy(robotPose.getRotation().unaryMinus());
+    }
+
+    /**
+     * Checks if a valid target is currently identified and if the robot is far enough away to continue driving.
+     *
+     * @return {@code true} if a target exists and distance is greater than the intake check distance; {@code false} otherwise.
+     */
+    private boolean hasValidTarget() {
+        Translation2d error = getTranslationError();
+        return error != null && error.getNorm() > AutonomousConstants.AUTO_COLLECTION_INTAKE_OPEN_CHECK_DISTANCE_METERS;
+    }
+
+    /**
+     * Scans all tracked objects on the field, groups them into clusters, and identifies the "best" one to target.
+     * The selection is based on a scoring algorithm that weighs cluster size against distance from the robot.
+     *
+     * @return The {@link GamePieceCluster} with the highest score, or null if no valid game pieces are found.
+     */
+    private GamePieceCluster findBestCluster() {
+        List<Translation2d> allObjects = OBJECT_POSE_ESTIMATOR.getObjectsOnField();
+        Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
+
+        if (allObjects.isEmpty())
+            return null;
+
+        GamePieceCluster bestCluster = null;
+        double maxScore = -Double.MAX_VALUE;
+
+        for (Translation2d seed : allObjects) {
+            if (isOutOfBounds(seed)) continue;
+
+            List<Translation2d> clusterPieces = getNeighbors(seed, allObjects);
+            if (clusterPieces.isEmpty()) continue;
+
+            GamePieceCluster cluster = new GamePieceCluster(clusterPieces, robotPose);
+            double score = calculateClusterScore(clusterPieces.size(), cluster.getDistanceToRobot());
+
+            if (score > maxScore) {
+                maxScore = score;
+                bestCluster = cluster;
+            }
+        }
+        return bestCluster;
+    }
+
+    /**
+     * Finds all game pieces that are within a specific radius of a seed piece to form a cluster.
+     *
+     * @param seed       The central game piece used to find neighbors.
+     * @param allObjects A list of all detected game pieces on the field.
+     * @return A list of {@link Translation2d} objects representing the cluster, including the seed.
+     */
+    private List<Translation2d> getNeighbors(Translation2d seed, List<Translation2d> allObjects) {
+        List<Translation2d> neighbors = new ArrayList<>();
+        for (Translation2d other : allObjects) {
+            if (isOutOfBounds(other)) continue;
+
+            if (seed.getDistance(other) <= GamePieceAutoDriveConstants.CLUSTER_RADIUS_METERS)
+                neighbors.add(other);
+        }
+        return neighbors;
+    }
+
+    /**
+     * Checks if a game piece is outside the valid collection area (e.g., on the opponent's side of the field).
+     *
+     * @param piece The position of the game piece to check.
+     * @return {@code true} if the piece is out of bounds; {@code false} otherwise.
+     */
+    private boolean isOutOfBounds(Translation2d piece) {
+        return piece.getX() > GamePieceAutoDriveConstants.MAX_COLLECTION_X_METERS;
+    }
+
+    /**
+     * Calculates a score for a potential target cluster.
+     * Higher scores are better.
+     *
+     * @param count    The number of game pieces in the cluster.
+     * @param distance The distance from the robot to the cluster centroid in meters.
+     * @return The calculated score.
+     */
+    private double calculateClusterScore(int count, double distance) {
+        return (count * GamePieceAutoDriveConstants.SCORE_WEIGHT_COUNT) - (distance * GamePieceAutoDriveConstants.SCORE_PENALTY_DISTANCE);
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveConstants.java b/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveConstants.java
new file mode 100644
index 00000000..0bbe7a7e
--- /dev/null
+++ b/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveConstants.java
@@ -0,0 +1,13 @@
+package frc.trigon.robot.commands.commandclasses.gamepieceautodrive;
+
+import frc.trigon.robot.constants.FieldConstants;
+
+public class GamePieceAutoDriveConstants {
+    static final double CLUSTER_RADIUS_METERS = 1.0;
+    static final double MAX_COLLECTION_X_METERS = FieldConstants.FIELD_LENGTH_METERS / 2.0;
+    static final double SCORE_WEIGHT_COUNT = 2.5;
+    static final double SCORE_PENALTY_DISTANCE = 1.0;
+    static final double LINEARITY_THRESHOLD = 4.0;
+    static final double BLEND_START_DISTANCE_METERS = 1.5;
+    static final double BLEND_END_DISTANCE_METERS = 0.5;
+}
\ No newline at end of file
diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceCluster.java b/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceCluster.java
new file mode 100644
index 00000000..cfd8341e
--- /dev/null
+++ b/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceCluster.java
@@ -0,0 +1,115 @@
+package frc.trigon.robot.commands.commandclasses.gamepieceautodrive;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+
+import java.util.List;
+
+public class GamePieceCluster {
+    private final Translation2d centroid;
+    private final Rotation2d approachHeading;
+    private final double distanceToRobot;
+
+    public GamePieceCluster(List<Translation2d> gamePieces, Pose2d robotPose) {
+        this.centroid = calculateCentroid(gamePieces);
+        this.distanceToRobot = centroid.getDistance(robotPose.getTranslation());
+        this.approachHeading = calculateApproachHeading(gamePieces, robotPose);
+    }
+
+    public Translation2d getCentroid() {
+        return centroid;
+    }
+
+    public Rotation2d getApproachHeading() {
+        return approachHeading;
+    }
+
+    public double getDistanceToRobot() {
+        return distanceToRobot;
+    }
+
+    public int getSize() {
+        return 1;
+    }
+
+    private Translation2d calculateCentroid(List<Translation2d> pieces) {
+        double sumX = 0;
+        double sumY = 0;
+        for (Translation2d piece : pieces) {
+            sumX += piece.getX();
+            sumY += piece.getY();
+        }
+        return new Translation2d(sumX / pieces.size(), sumY / pieces.size());
+    }
+
+    private Rotation2d calculateApproachHeading(List<Translation2d> pieces, Pose2d robotPose) {
+        Rotation2d angleToCentroid = new Rotation2d(
+                Math.atan2(centroid.getY() - robotPose.getY(), centroid.getX() - robotPose.getX())
+        );
+
+        // If too few points, we cannot determine a line, so face the center
+        if (pieces.size() <= 2)
+            return angleToCentroid;
+
+        if (isLinear(pieces)) {
+            Rotation2d axisAngle = calculatePrincipalAxis(pieces);
+            Rotation2d alignedAxis = resolveAmbiguity(axisAngle, angleToCentroid);
+            return blendHeadings(angleToCentroid, alignedAxis);
+        } else
+            return angleToCentroid;
+    }
+
+    private boolean isLinear(List<Translation2d> pieces) {
+        double varianceX = 0, varianceY = 0, covariance = 0;
+
+        for (Translation2d p : pieces) {
+            double dx = p.getX() - centroid.getX();
+            double dy = p.getY() - centroid.getY();
+            varianceX += dx * dx;
+            varianceY += dy * dy;
+            covariance += dx * dy;
+        }
+
+        double trace = varianceX + varianceY;
+        double det = (varianceX * varianceY) - (covariance * covariance);
+        double gap = Math.sqrt(Math.max(0, trace * trace - 4 * det));
+
+        double majorEigenvalue = (trace + gap) / 2.0;
+        double minorEigenvalue = (trace - gap) / 2.0;
+
+        double aspectRatio = (minorEigenvalue == 0) ? Double.MAX_VALUE : (majorEigenvalue / minorEigenvalue);
+        return aspectRatio > GamePieceAutoDriveConstants.LINEARITY_THRESHOLD;
+    }
+
+    private Rotation2d calculatePrincipalAxis(List<Translation2d> pieces) {
+        double varianceX = 0, varianceY = 0, covariance = 0;
+
+        for (Translation2d p : pieces) {
+            double dx = p.getX() - centroid.getX();
+            double dy = p.getY() - centroid.getY();
+            varianceX += dx * dx;
+            varianceY += dy * dy;
+            covariance += dx * dy;
+        }
+
+        double angleRad = 0.5 * Math.atan2(2 * covariance, varianceX - varianceY);
+        return new Rotation2d(angleRad);
+    }
+
+    private Rotation2d resolveAmbiguity(Rotation2d axisAngle, Rotation2d angleToCentroid) {
+        if (Math.abs(axisAngle.minus(angleToCentroid).getDegrees()) > 90.0) {
+            return axisAngle.rotateBy(Rotation2d.fromDegrees(180));
+        }
+        return axisAngle;
+    }
+
+    private Rotation2d blendHeadings(Rotation2d centerHeading, Rotation2d axisHeading) {
+        double blendFactor = (GamePieceAutoDriveConstants.BLEND_START_DISTANCE_METERS - distanceToRobot)
+                / (GamePieceAutoDriveConstants.BLEND_START_DISTANCE_METERS - GamePieceAutoDriveConstants.BLEND_END_DISTANCE_METERS);
+
+        blendFactor = Math.max(0.0, Math.min(1.0, blendFactor)); // Clamp [0, 1]
+
+        return centerHeading.interpolate(axisHeading, blendFactor);
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
index 1fa8484e..ba5318b6 100644
--- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
+++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
@@ -3,10 +3,13 @@
 import com.pathplanner.lib.commands.PathPlannerAuto;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj2.command.*;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.trigon.lib.utilities.flippable.FlippablePose2d;
 import frc.trigon.robot.RobotContainer;
-import frc.trigon.robot.commands.commandclasses.IntakeAssistCommand;
+import frc.trigon.robot.commands.commandclasses.gamepieceautodrive.GamePieceAutoDriveCommand;
 import frc.trigon.robot.constants.AutonomousConstants;
 import frc.trigon.robot.constants.FieldConstants;
 import frc.trigon.robot.subsystems.intake.IntakeCommands;
@@ -47,15 +50,15 @@ public static Command getCollectFromNeutralZoneCommand() {
                 new ParallelCommandGroup(
                         SwerveCommands.getDriveToPoseCommand(isRight() ? () -> FieldConstants.RIGHT_INTAKE_POSITION : () -> FieldConstants.LEFT_INTAKE_POSITION, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
                         ShootingCommands.getShootAtHubCommand().onlyWhile(AutonomousCommands::isInAllianceZone)
-                ).until(() -> RobotContainer.SWERVE.atPose(isRight() ? FieldConstants.RIGHT_INTAKE_POSITION : FieldConstants.LEFT_INTAKE_POSITION)),
-                new IntakeAssistCommand(1, 1, 1).alongWith(new PrintCommand("intakingmoving"))
+                ).until(AutonomousCommands::shouldCollectGamePiecesFromNeutralZone),
+                new GamePieceAutoDriveCommand()
         ).alongWith(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.INTAKE)).withTimeout(AutonomousConstants.NEUTRAL_ZONE_COLLECTION_TIMEOUT_SECONDS);
     }
 
     public static Command getScoreCommand() {
         return new ParallelCommandGroup(
                 SwerveCommands.getDriveToPoseCommand(() -> isRight() ? FieldConstants.RIGHT_IDEAL_SHOOTING_POSITION : FieldConstants.LEFT_IDEAL_SHOOTING_POSITION, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
-                ShootingCommands.getShootAtHubCommand().onlyWhile(AutonomousCommands::isInAllianceZone)
+                ShootingCommands.getShootAtHubCommand().onlyWhile(AutonomousCommands::isInAllianceZone).repeatedly()
         ).withTimeout(AutonomousConstants.SCORING_TIMEOUT_SECONDS);
     }
 
@@ -63,9 +66,9 @@ public static Command getCollectFromDepotCommand() {
         return new SequentialCommandGroup(
                 new ParallelCommandGroup(
                         SwerveCommands.getDriveToPoseCommand(() -> FieldConstants.DEPOT_POSITION, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
-                        ShootingCommands.getShootAtHubCommand().onlyWhile(AutonomousCommands::isInAllianceZone)
+                        ShootingCommands.getShootAtHubCommand().onlyWhile(AutonomousCommands::isInAllianceZone).repeatedly()
                 ).until(() -> RobotContainer.SWERVE.atPose(FieldConstants.DEPOT_POSITION)),
-                new IntakeAssistCommand(1, 1, 1).alongWith(new PrintCommand("intakingmoving"))
+                new GamePieceAutoDriveCommand().alongWith(ShootingCommands.getShootAtHubCommand())
         ).alongWith(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.INTAKE)).withTimeout(AutonomousConstants.DEPOT_COLLECTION_TIMEOUT_SECONDS);
     }
 
@@ -85,6 +88,11 @@ private static boolean isInAllianceZone() {
         return currentRobotPose.getX() < FieldConstants.ALLIANCE_ZONE_LENGTH;
     }
 
+    private static boolean shouldCollectGamePiecesFromNeutralZone() {
+        final Pose2d currentRobotPose = new FlippablePose2d(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(), true).get();
+        return currentRobotPose.getX() > FieldConstants.DELIVERY_ZONE_START_BLUE_X && RobotContainer.OBJECT_POSE_ESTIMATOR.hasObjects();
+    }
+
     /**
      * Creates a command that resets the pose estimator's pose to the starting pose of the given autonomous as long as the robot is not enabled.
      *
diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
index ced13517..4f1bc361 100644
--- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
+++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
@@ -8,8 +8,6 @@
 import com.pathplanner.lib.path.PathConstraints;
 import com.pathplanner.lib.pathfinding.Pathfinding;
 import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.controller.ProfiledPIDController;
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -50,7 +48,7 @@ public class AutonomousConstants {
 
     private static final PIDConstants
             AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ?
-            new PIDConstants(5, 0, 0) :
+            new PIDConstants(7, 0, 0) :
             new PIDConstants(0, 0, 0),
             AUTO_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ?
                     new PIDConstants(2, 0, 0) :
@@ -60,10 +58,10 @@ public class AutonomousConstants {
     public static final PIDController GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER = RobotHardwareStats.isSimulation() ?
             new PIDController(0.5, 0, 0) :
             new PIDController(0.3, 0, 0.03);
-    public static final ProfiledPIDController GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER = RobotHardwareStats.isSimulation() ?
-            new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) :
-            new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5));
-    public static final double AUTO_COLLECTION_INTAKE_OPEN_CHECK_DISTANCE_METERS = 2;
+    public static final PIDController GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER = RobotHardwareStats.isSimulation() ?
+            new PIDController(0.5, 0, 0) :
+            new PIDController(2.4, 0, 0);
+    public static final double AUTO_COLLECTION_INTAKE_OPEN_CHECK_DISTANCE_METERS = 0.01;
 
     private static final PPHolonomicDriveController AUTO_PATH_FOLLOWING_CONTROLLER = new PPHolonomicDriveController(
             AUTO_TRANSLATION_PID_CONSTANTS,
diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java
index 9dcd9d31..5317f0ad 100644
--- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java
+++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java
@@ -34,8 +34,8 @@ public class FieldConstants {
             RIGHT_CLIMB_POSITION = new FlippablePose2d(LEFT_CLIMB_POSITION.getBlueObject().getX(), 3.28, Rotation2d.fromDegrees(0), true),
             CENTER_CLIMB_POSITION = new FlippablePose2d((LEFT_CLIMB_POSITION.getBlueObject().getX() + LEFT_CLIMB_POSITION.getBlueObject().getX()) / 2, LEFT_CLIMB_POSITION.getBlueObject().getY(), Rotation2d.fromDegrees(0), true),
             DEPOT_POSITION = new FlippablePose2d(0.9, 6, Rotation2d.fromDegrees(180), true),
-            LEFT_INTAKE_POSITION = new FlippablePose2d(6.2, 7, Rotation2d.fromDegrees(-30), true),
-            RIGHT_INTAKE_POSITION = new FlippablePose2d(LEFT_INTAKE_POSITION.getBlueObject().getX(), FIELD_WIDTH_METERS - LEFT_INTAKE_POSITION.getBlueObject().getY(), Rotation2d.fromDegrees(30), true),
+            LEFT_INTAKE_POSITION = new FlippablePose2d(7, 7.3, Rotation2d.fromDegrees(-40), true),
+            RIGHT_INTAKE_POSITION = new FlippablePose2d(LEFT_INTAKE_POSITION.getBlueObject().getX(), FIELD_WIDTH_METERS - LEFT_INTAKE_POSITION.getBlueObject().getY(), Rotation2d.fromDegrees(40), true),
             LEFT_IDEAL_SHOOTING_POSITION = new FlippablePose2d(2.7, 5.8, Rotation2d.fromDegrees(0), true),
             RIGHT_IDEAL_SHOOTING_POSITION = new FlippablePose2d(LEFT_IDEAL_SHOOTING_POSITION.getBlueObject().getX(), FIELD_WIDTH_METERS - LEFT_IDEAL_SHOOTING_POSITION.getBlueObject().getY(), Rotation2d.fromDegrees(0), true);
     private static final double
@@ -46,7 +46,7 @@ public class FieldConstants {
             RIGHT_DELIVERY_POSITION = new FlippableTranslation2d(BLUE_RELATIVE_DELIVERY_POSITION_X, (FIELD_WIDTH_METERS / 2) - DELIVERY_POSITION_Y_OFFSET_FROM_CENTER_METERS, true),
             LEFT_DELIVERY_POSITION = new FlippableTranslation2d(BLUE_RELATIVE_DELIVERY_POSITION_X, (FIELD_WIDTH_METERS / 2) + DELIVERY_POSITION_Y_OFFSET_FROM_CENTER_METERS, true);
     public static final double
-            ALLIANCE_ZONE_LENGTH = 4.625594,
+            ALLIANCE_ZONE_LENGTH = 4,
             DELIVERY_ZONE_START_BLUE_X = ALLIANCE_ZONE_LENGTH + 0.5;
 
     private static AprilTagFieldLayout createAprilTagFieldLayout() {
diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java
index 68b6b02b..a00c4c9f 100644
--- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java
+++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java
@@ -323,6 +323,7 @@ private SwerveModulePosition[] getSwerveWheelPositions(int odometryUpdateIndex)
     }
 
     private boolean atTranslationPosition(double currentTranslationPosition, double targetTranslationPosition, double currentTranslationVelocity) {
+//        System.out.println("Current Pos: " + currentTranslationPosition + ", Target Pos: " + targetTranslationPosition + ", Current Vel: " + currentTranslationVelocity);
         return Math.abs(currentTranslationPosition - targetTranslationPosition) < SwerveConstants.TRANSLATION_TOLERANCE_METERS &&
                 Math.abs(currentTranslationVelocity) < SwerveConstants.TRANSLATION_VELOCITY_TOLERANCE;
     }

From 5d88f912cc72353225fbfb5f83e2d99ab8dd40fe Mon Sep 17 00:00:00 2001
From: Nummun14 <142012009+Nummun14@users.noreply.github.com>
Date: Fri, 30 Jan 2026 07:02:09 +0200
Subject: [PATCH 07/12] safe under trench works!!!

---
 .../GamePieceAutoDriveCommand.java            | 101 +++++-------------
 .../GamePieceAutoDriveConstants.java          |   2 +-
 .../commandfactories/AutonomousCommands.java  |  75 ++++++++++---
 .../robot/constants/AutonomousConstants.java  |   6 +-
 .../robot/constants/FieldConstants.java       |   6 +-
 5 files changed, 92 insertions(+), 98 deletions(-)

diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveCommand.java
index 9b38413a..6606bf39 100644
--- a/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveCommand.java
+++ b/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveCommand.java
@@ -24,12 +24,6 @@ public GamePieceAutoDriveCommand() {
         );
     }
 
-    /**
-     * Creates a command that periodically calculates the best cluster and updates the atomic reference.
-     * This acts as the "vision processing" thread for this command group.
-     *
-     * @return A {@link Command} that runs continuously to update the target.
-     */
     private Command createTargetUpdateCommand() {
         return new RunCommand(() -> {
             GamePieceCluster bestCluster = findBestCluster();
@@ -37,11 +31,6 @@ private Command createTargetUpdateCommand() {
         });
     }
 
-    /**
-     * Creates the drive command that moves the robot towards the currently selected target cluster.
-     *
-     * @return A {@link Command} that executes the swerve drive logic.
-     */
     private Command createDriveCommand() {
         return new SequentialCommandGroup(
                 new InstantCommand(AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER::reset),
@@ -53,80 +42,62 @@ private Command createDriveCommand() {
         ).onlyWhile(this::hasValidTarget);
     }
 
-    /**
-     * Calculates the output for the X-axis PID controller based on the current target.
-     *
-     * @return The calculated output for the X-axis velocity, or 0.0 if no target exists.
-     */
     private double getXControllerOutput() {
         Translation2d error = getTranslationError();
-        if (error == null)
-            return 0.0;
-        return AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.calculate(error.getX());
+        if (error == null) return 0.0;
+
+        // CRITICAL FIX 1: Negate the error here.
+        // Controller calculates (Setpoint - Measurement).
+        // We want (0 - (-Distance)) = +Distance to drive forward.
+        return AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.calculate(-error.getX());
     }
 
-    /**
-     * Calculates the output for the Y-axis PID controller based on the current target.
-     *
-     * @return The calculated output for the Y-axis velocity, or 0.0 if no target exists.
-     */
     private double getYControllerOutput() {
         Translation2d error = getTranslationError();
         if (error == null) return 0.0;
-        return AutonomousConstants.GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER.calculate(error.getY());
+
+        // CRITICAL FIX 1: Negate the error here too.
+        return AutonomousConstants.GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER.calculate(-error.getY());
     }
 
-    /**
-     * Retrieves the target heading for the robot based on the selected cluster's approach angle.
-     *
-     * @return A {@link FlippableRotation2d} representing the desired heading, or null if no target exists.
-     */
     private FlippableRotation2d getTargetHeading() {
         GamePieceCluster cluster = currentTargetCluster.get();
-        if (cluster == null)
-            return null;
+        if (cluster == null) return null;
         return new FlippableRotation2d(cluster.getApproachHeading(), false);
     }
 
-    /**
-     * Calculates the vector from the robot to the target centroid, rotated to be robot-relative.
-     * This is used as the error input for the self-relative PID controllers.
-     *
-     * @return A {@link Translation2d} representing the distance error in robot-relative coordinates, or null if no target exists.
-     */
     private Translation2d getTranslationError() {
         GamePieceCluster cluster = currentTargetCluster.get();
-        if (cluster == null)
-            return null;
+        if (cluster == null) return null;
 
         Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
         Translation2d fieldRelativeError = cluster.getCentroid().minus(robotPose.getTranslation());
 
+        // CRITICAL FIX 2: You MUST use unaryMinus() here.
+        // This rotates the "World Vector" backwards to match the "Robot's Perspective".
+        // Without this, Left/Right are swapped, causing swerving.
         return fieldRelativeError.rotateBy(robotPose.getRotation().unaryMinus());
     }
 
-    /**
-     * Checks if a valid target is currently identified and if the robot is far enough away to continue driving.
-     *
-     * @return {@code true} if a target exists and distance is greater than the intake check distance; {@code false} otherwise.
-     */
     private boolean hasValidTarget() {
         Translation2d error = getTranslationError();
-        return error != null && error.getNorm() > AutonomousConstants.AUTO_COLLECTION_INTAKE_OPEN_CHECK_DISTANCE_METERS;
+        Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
+
+        // CRITICAL FIX 3: Stop if the ROBOT is past the line.
+        // Previously we only checked if the GAME PIECE was past the line.
+        boolean robotIsSafe = robotPose.getX() < GamePieceAutoDriveConstants.MAX_COLLECTION_X_METERS;
+
+        return error != null &&
+                robotIsSafe &&
+                error.getNorm() > AutonomousConstants.AUTO_COLLECTION_INTAKE_OPEN_CHECK_DISTANCE_METERS;
     }
 
-    /**
-     * Scans all tracked objects on the field, groups them into clusters, and identifies the "best" one to target.
-     * The selection is based on a scoring algorithm that weighs cluster size against distance from the robot.
-     *
-     * @return The {@link GamePieceCluster} with the highest score, or null if no valid game pieces are found.
-     */
+    // --- Vision Logic (Unchanged) ---
     private GamePieceCluster findBestCluster() {
         List<Translation2d> allObjects = OBJECT_POSE_ESTIMATOR.getObjectsOnField();
         Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
 
-        if (allObjects.isEmpty())
-            return null;
+        if (allObjects.isEmpty()) return null;
 
         GamePieceCluster bestCluster = null;
         double maxScore = -Double.MAX_VALUE;
@@ -148,42 +119,20 @@ private GamePieceCluster findBestCluster() {
         return bestCluster;
     }
 
-    /**
-     * Finds all game pieces that are within a specific radius of a seed piece to form a cluster.
-     *
-     * @param seed       The central game piece used to find neighbors.
-     * @param allObjects A list of all detected game pieces on the field.
-     * @return A list of {@link Translation2d} objects representing the cluster, including the seed.
-     */
     private List<Translation2d> getNeighbors(Translation2d seed, List<Translation2d> allObjects) {
         List<Translation2d> neighbors = new ArrayList<>();
         for (Translation2d other : allObjects) {
             if (isOutOfBounds(other)) continue;
-
             if (seed.getDistance(other) <= GamePieceAutoDriveConstants.CLUSTER_RADIUS_METERS)
                 neighbors.add(other);
         }
         return neighbors;
     }
 
-    /**
-     * Checks if a game piece is outside the valid collection area (e.g., on the opponent's side of the field).
-     *
-     * @param piece The position of the game piece to check.
-     * @return {@code true} if the piece is out of bounds; {@code false} otherwise.
-     */
     private boolean isOutOfBounds(Translation2d piece) {
         return piece.getX() > GamePieceAutoDriveConstants.MAX_COLLECTION_X_METERS;
     }
 
-    /**
-     * Calculates a score for a potential target cluster.
-     * Higher scores are better.
-     *
-     * @param count    The number of game pieces in the cluster.
-     * @param distance The distance from the robot to the cluster centroid in meters.
-     * @return The calculated score.
-     */
     private double calculateClusterScore(int count, double distance) {
         return (count * GamePieceAutoDriveConstants.SCORE_WEIGHT_COUNT) - (distance * GamePieceAutoDriveConstants.SCORE_PENALTY_DISTANCE);
     }
diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveConstants.java b/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveConstants.java
index 0bbe7a7e..578f9df1 100644
--- a/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveConstants.java
+++ b/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveConstants.java
@@ -7,7 +7,7 @@ public class GamePieceAutoDriveConstants {
     static final double MAX_COLLECTION_X_METERS = FieldConstants.FIELD_LENGTH_METERS / 2.0;
     static final double SCORE_WEIGHT_COUNT = 2.5;
     static final double SCORE_PENALTY_DISTANCE = 1.0;
-    static final double LINEARITY_THRESHOLD = 4.0;
+    static final double LINEARITY_THRESHOLD = 6.0;
     static final double BLEND_START_DISTANCE_METERS = 1.5;
     static final double BLEND_END_DISTANCE_METERS = 0.5;
 }
\ No newline at end of file
diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
index ba5318b6..a128b6d6 100644
--- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
+++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
@@ -3,10 +3,7 @@
 import com.pathplanner.lib.commands.PathPlannerAuto;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.*;
 import frc.trigon.lib.utilities.flippable.FlippablePose2d;
 import frc.trigon.robot.RobotContainer;
 import frc.trigon.robot.commands.commandclasses.gamepieceautodrive.GamePieceAutoDriveCommand;
@@ -35,7 +32,7 @@ public static Command getAutonomousCommand() {
 
     public static Command getDeliveryCommand() {
         return new ParallelCommandGroup(
-                SwerveCommands.getDriveToPoseCommand(isRight() ? () -> FieldConstants.RIGHT_INTAKE_POSITION : () -> FieldConstants.LEFT_INTAKE_POSITION, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
+                getSafeDriveToPoseCommand(isRight() ? () -> FieldConstants.RIGHT_INTAKE_POSITION : () -> FieldConstants.LEFT_INTAKE_POSITION),
                 IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.INTAKE),
                 GeneralCommands.getContinuousConditionalCommand(
                         ShootingCommands.getShootAtHubCommand(),
@@ -46,18 +43,19 @@ public static Command getDeliveryCommand() {
     }
 
     public static Command getCollectFromNeutralZoneCommand() {
-        return new SequentialCommandGroup(
-                new ParallelCommandGroup(
-                        SwerveCommands.getDriveToPoseCommand(isRight() ? () -> FieldConstants.RIGHT_INTAKE_POSITION : () -> FieldConstants.LEFT_INTAKE_POSITION, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
-                        ShootingCommands.getShootAtHubCommand().onlyWhile(AutonomousCommands::isInAllianceZone)
-                ).until(AutonomousCommands::shouldCollectGamePiecesFromNeutralZone),
-                new GamePieceAutoDriveCommand()
-        ).alongWith(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.INTAKE)).withTimeout(AutonomousConstants.NEUTRAL_ZONE_COLLECTION_TIMEOUT_SECONDS);
+        return new ParallelDeadlineGroup(
+                new SequentialCommandGroup(
+                        getSafeDriveToPoseCommand(isRight() ? () -> FieldConstants.RIGHT_INTAKE_POSITION : () -> FieldConstants.LEFT_INTAKE_POSITION),
+                        new GamePieceAutoDriveCommand().withTimeout(AutonomousConstants.NEUTRAL_ZONE_COLLECTION_TIMEOUT_SECONDS).finallyDo(() -> System.out.println("ENDED"))
+                ),
+                IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.INTAKE),
+                ShootingCommands.getShootAtHubCommand().onlyWhile(AutonomousCommands::isInAllianceZone)
+        );
     }
 
     public static Command getScoreCommand() {
         return new ParallelCommandGroup(
-                SwerveCommands.getDriveToPoseCommand(() -> isRight() ? FieldConstants.RIGHT_IDEAL_SHOOTING_POSITION : FieldConstants.LEFT_IDEAL_SHOOTING_POSITION, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
+                getSafeDriveToPoseCommand(() -> isRight() ? FieldConstants.RIGHT_IDEAL_SHOOTING_POSITION : FieldConstants.LEFT_IDEAL_SHOOTING_POSITION),
                 ShootingCommands.getShootAtHubCommand().onlyWhile(AutonomousCommands::isInAllianceZone).repeatedly()
         ).withTimeout(AutonomousConstants.SCORING_TIMEOUT_SECONDS);
     }
@@ -65,7 +63,7 @@ public static Command getScoreCommand() {
     public static Command getCollectFromDepotCommand() {
         return new SequentialCommandGroup(
                 new ParallelCommandGroup(
-                        SwerveCommands.getDriveToPoseCommand(() -> FieldConstants.DEPOT_POSITION, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
+                        getSafeDriveToPoseCommand(() -> FieldConstants.DEPOT_POSITION),
                         ShootingCommands.getShootAtHubCommand().onlyWhile(AutonomousCommands::isInAllianceZone).repeatedly()
                 ).until(() -> RobotContainer.SWERVE.atPose(FieldConstants.DEPOT_POSITION)),
                 new GamePieceAutoDriveCommand().alongWith(ShootingCommands.getShootAtHubCommand())
@@ -74,18 +72,61 @@ public static Command getCollectFromDepotCommand() {
 
     public static Command getClimbCommand(FlippablePose2d climbPosition) {
         return new SequentialCommandGroup(
-                SwerveCommands.getDriveToPoseCommand(() -> climbPosition, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
+                getSafeDriveToPoseCommand(() -> climbPosition),
                 new InstantCommand() // TODO: Add climb command
         );
     }
 
+    private static Command getSafeDriveToPoseCommand(Supplier<FlippablePose2d> targetPose) {
+        return new ConditionalCommand(
+                getDriveThroughTrenchCommand(targetPose),
+                SwerveCommands.getDriveToPoseCommand(targetPose, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
+                () -> shouldDriveThroughTrench(targetPose.get())
+        );
+    }
+
+    private static Command getDriveThroughTrenchCommand(Supplier<FlippablePose2d> targetPose) {
+        return new SequentialCommandGroup(
+                SwerveCommands.getDriveToPoseCommand(AutonomousCommands::getTrenchEntryPose, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS, 4),
+                SwerveCommands.getDriveToPoseCommand(AutonomousCommands::getTrenchExitPose, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS, 4),
+                SwerveCommands.getDriveToPoseCommand(targetPose, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS)
+        );
+    }
+
+    private static boolean shouldDriveThroughTrench(FlippablePose2d targetPose) {
+        return (!isInAllianceZone() && isPoseInAllianceZone(targetPose)) || (isInAllianceZone() && !isPoseInAllianceZone(targetPose));
+    }
+
+
+    private static FlippablePose2d getTrenchExitPose() {
+        final FlippablePose2d targetTrenchExitPose = isRight() ?
+                isInAllianceZone() ? FieldConstants.RIGHT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE : FieldConstants.RIGHT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE :
+                isInAllianceZone() ? FieldConstants.LEFT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE : FieldConstants.LEFT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE;
+        System.out.println("Target Trench Exit Pose: " + targetTrenchExitPose.get().toString());
+        if (RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().getDegrees() > 90)
+            return new FlippablePose2d(targetTrenchExitPose.getBlueObject().getTranslation(), Math.PI, true);
+        return targetTrenchExitPose;
+    }
+
+    private static FlippablePose2d getTrenchEntryPose() {
+        final FlippablePose2d targetTrenchEntryPose = isRight() ?
+                isInAllianceZone() ? FieldConstants.RIGHT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE : FieldConstants.RIGHT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE :
+                isInAllianceZone() ? FieldConstants.LEFT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE : FieldConstants.LEFT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE;
+        if (RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().getDegrees() > 90)
+            return new FlippablePose2d(targetTrenchEntryPose.getBlueObject().getTranslation(), Math.PI, true);
+        return targetTrenchEntryPose;
+    }
+
     private static boolean isRight() {
         return RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation().getY() < FieldConstants.FIELD_WIDTH_METERS / 2;
     }
 
     private static boolean isInAllianceZone() {
-        final Pose2d currentRobotPose = new FlippablePose2d(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(), true).get();
-        return currentRobotPose.getX() < FieldConstants.ALLIANCE_ZONE_LENGTH;
+        return isPoseInAllianceZone(new FlippablePose2d(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(), true));
+    }
+
+    private static boolean isPoseInAllianceZone(FlippablePose2d pose) {
+        return pose.get().getX() < FieldConstants.ALLIANCE_ZONE_LENGTH;
     }
 
     private static boolean shouldCollectGamePiecesFromNeutralZone() {
diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
index 4f1bc361..cbbf20b0 100644
--- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
+++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
@@ -30,7 +30,7 @@
 public class AutonomousConstants {
     public static final String DEFAULT_AUTO_NAME = "DefaultAutoName";
     public static final RobotConfig ROBOT_CONFIG = getRobotConfig();
-    public static final double FEEDFORWARD_SCALAR = 0.5;//TODO: Calibrate
+    public static final double FEEDFORWARD_SCALAR = 0.7;//TODO: Calibrate
     public static final PathConstraints
             DRIVE_IN_AUTONOMOUS_CONSTRAINTS = new PathConstraints(2.5, 4.5, Units.degreesToRadians(450), Units.degreesToRadians(900)),
             DRIVE_SLOWLY_IN_AUTONOMOUS_CONSTRAINTS = new PathConstraints(1.5, 3.0, Units.degreesToRadians(300), Units.degreesToRadians(600));
@@ -42,13 +42,13 @@ public class AutonomousConstants {
 
     public static double
             DEPOT_COLLECTION_TIMEOUT_SECONDS = 4,
-            NEUTRAL_ZONE_COLLECTION_TIMEOUT_SECONDS = 8,
+            NEUTRAL_ZONE_COLLECTION_TIMEOUT_SECONDS = 3,
             DELIVERY_TIMEOUT_SECONDS = 8,
             SCORING_TIMEOUT_SECONDS = 6;
 
     private static final PIDConstants
             AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ?
-            new PIDConstants(7, 0, 0) :
+            new PIDConstants(9, 0, 0) :
             new PIDConstants(0, 0, 0),
             AUTO_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ?
                     new PIDConstants(2, 0, 0) :
diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java
index 5317f0ad..09fe3019 100644
--- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java
+++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java
@@ -37,7 +37,11 @@ public class FieldConstants {
             LEFT_INTAKE_POSITION = new FlippablePose2d(7, 7.3, Rotation2d.fromDegrees(-40), true),
             RIGHT_INTAKE_POSITION = new FlippablePose2d(LEFT_INTAKE_POSITION.getBlueObject().getX(), FIELD_WIDTH_METERS - LEFT_INTAKE_POSITION.getBlueObject().getY(), Rotation2d.fromDegrees(40), true),
             LEFT_IDEAL_SHOOTING_POSITION = new FlippablePose2d(2.7, 5.8, Rotation2d.fromDegrees(0), true),
-            RIGHT_IDEAL_SHOOTING_POSITION = new FlippablePose2d(LEFT_IDEAL_SHOOTING_POSITION.getBlueObject().getX(), FIELD_WIDTH_METERS - LEFT_IDEAL_SHOOTING_POSITION.getBlueObject().getY(), Rotation2d.fromDegrees(0), true);
+            RIGHT_IDEAL_SHOOTING_POSITION = new FlippablePose2d(LEFT_IDEAL_SHOOTING_POSITION.getBlueObject().getX(), FIELD_WIDTH_METERS - LEFT_IDEAL_SHOOTING_POSITION.getBlueObject().getY(), Rotation2d.fromDegrees(0), true),
+            LEFT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE = new FlippablePose2d(3.9, 7.4, Rotation2d.kZero, true),
+            RIGHT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE = new FlippablePose2d(LEFT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE.getBlueObject().getX(), FIELD_WIDTH_METERS - LEFT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE.getBlueObject().getY(), Rotation2d.kZero, true),
+            LEFT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE = new FlippablePose2d(5.53, LEFT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE.getBlueObject().getY(), Rotation2d.kZero, true),
+            RIGHT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE = new FlippablePose2d(LEFT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE.getBlueObject().getX(), FIELD_WIDTH_METERS - LEFT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE.getBlueObject().getY(), Rotation2d.kZero, true);
     private static final double
             BLUE_RELATIVE_DELIVERY_POSITION_X = 3,
             DELIVERY_POSITION_Y_OFFSET_FROM_CENTER_METERS = 2.2;

From 89f8e637e6109db7282c8c3481fd51deaaabd452 Mon Sep 17 00:00:00 2001
From: Nummun14 <142012009+Nummun14@users.noreply.github.com>
Date: Fri, 30 Jan 2026 08:05:18 +0200
Subject: [PATCH 08/12] Stuff does stuff

---
 .../commandfactories/AutonomousCommands.java  | 41 +++++++++++++------
 .../robot/constants/AutonomousConstants.java  |  2 +-
 .../robot/constants/FieldConstants.java       |  2 +-
 3 files changed, 30 insertions(+), 15 deletions(-)

diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
index a128b6d6..0eec1359 100644
--- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
+++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
@@ -45,8 +45,8 @@ public static Command getDeliveryCommand() {
     public static Command getCollectFromNeutralZoneCommand() {
         return new ParallelDeadlineGroup(
                 new SequentialCommandGroup(
-                        getSafeDriveToPoseCommand(isRight() ? () -> FieldConstants.RIGHT_INTAKE_POSITION : () -> FieldConstants.LEFT_INTAKE_POSITION),
-                        new GamePieceAutoDriveCommand().withTimeout(AutonomousConstants.NEUTRAL_ZONE_COLLECTION_TIMEOUT_SECONDS).finallyDo(() -> System.out.println("ENDED"))
+                        getSafeDriveToPoseCommand(isRight() ? () -> FieldConstants.RIGHT_INTAKE_POSITION : () -> FieldConstants.LEFT_INTAKE_POSITION, 3),
+                        new GamePieceAutoDriveCommand().withTimeout(AutonomousConstants.NEUTRAL_ZONE_COLLECTION_TIMEOUT_SECONDS)
                 ),
                 IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.INTAKE),
                 ShootingCommands.getShootAtHubCommand().onlyWhile(AutonomousCommands::isInAllianceZone)
@@ -78,18 +78,22 @@ public static Command getClimbCommand(FlippablePose2d climbPosition) {
     }
 
     private static Command getSafeDriveToPoseCommand(Supplier<FlippablePose2d> targetPose) {
+        return getSafeDriveToPoseCommand(targetPose, 0);
+    }
+
+    private static Command getSafeDriveToPoseCommand(Supplier<FlippablePose2d> targetPose, double endVelocity) {
         return new ConditionalCommand(
-                getDriveThroughTrenchCommand(targetPose),
-                SwerveCommands.getDriveToPoseCommand(targetPose, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS),
+                getDriveThroughTrenchCommand(targetPose, endVelocity),
+                SwerveCommands.getDriveToPoseCommand(targetPose, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS, endVelocity),
                 () -> shouldDriveThroughTrench(targetPose.get())
         );
     }
 
-    private static Command getDriveThroughTrenchCommand(Supplier<FlippablePose2d> targetPose) {
+    private static Command getDriveThroughTrenchCommand(Supplier<FlippablePose2d> targetPose, double endVelocity) {
         return new SequentialCommandGroup(
                 SwerveCommands.getDriveToPoseCommand(AutonomousCommands::getTrenchEntryPose, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS, 4),
-                SwerveCommands.getDriveToPoseCommand(AutonomousCommands::getTrenchExitPose, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS, 4),
-                SwerveCommands.getDriveToPoseCommand(targetPose, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS)
+                SwerveCommands.getDriveToPoseCommand(() -> AutonomousCommands.getTrenchExitPose(targetPose.get()), AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS, 4),
+                SwerveCommands.getDriveToPoseCommand(targetPose, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS, endVelocity)
         );
     }
 
@@ -97,13 +101,11 @@ private static boolean shouldDriveThroughTrench(FlippablePose2d targetPose) {
         return (!isInAllianceZone() && isPoseInAllianceZone(targetPose)) || (isInAllianceZone() && !isPoseInAllianceZone(targetPose));
     }
 
-
-    private static FlippablePose2d getTrenchExitPose() {
+    private static FlippablePose2d getTrenchExitPose(FlippablePose2d targetPose) {
         final FlippablePose2d targetTrenchExitPose = isRight() ?
-                isInAllianceZone() ? FieldConstants.RIGHT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE : FieldConstants.RIGHT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE :
-                isInAllianceZone() ? FieldConstants.LEFT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE : FieldConstants.LEFT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE;
-        System.out.println("Target Trench Exit Pose: " + targetTrenchExitPose.get().toString());
-        if (RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().getDegrees() > 90)
+                getClosestPoseToPose(targetPose, FieldConstants.RIGHT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE, FieldConstants.RIGHT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE) :
+                getClosestPoseToPose(targetPose, FieldConstants.LEFT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE, FieldConstants.LEFT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE);
+        if (RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().getDegrees() > 90 || RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().getDegrees() < -90)
             return new FlippablePose2d(targetTrenchExitPose.getBlueObject().getTranslation(), Math.PI, true);
         return targetTrenchExitPose;
     }
@@ -117,6 +119,19 @@ private static FlippablePose2d getTrenchEntryPose() {
         return targetTrenchEntryPose;
     }
 
+    private static FlippablePose2d getClosestPoseToPose(FlippablePose2d pose, FlippablePose2d... poses) {
+        FlippablePose2d closestPose = null;
+        double closestDistance = Double.MAX_VALUE;
+        for (FlippablePose2d candidatePose : poses) {
+            final double distance = pose.get().getTranslation().getDistance(candidatePose.get().getTranslation());
+            if (distance < closestDistance) {
+                closestDistance = distance;
+                closestPose = candidatePose;
+            }
+        }
+        return closestPose;
+    }
+
     private static boolean isRight() {
         return RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation().getY() < FieldConstants.FIELD_WIDTH_METERS / 2;
     }
diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
index cbbf20b0..8bf14b4e 100644
--- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
+++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
@@ -42,7 +42,7 @@ public class AutonomousConstants {
 
     public static double
             DEPOT_COLLECTION_TIMEOUT_SECONDS = 4,
-            NEUTRAL_ZONE_COLLECTION_TIMEOUT_SECONDS = 3,
+            NEUTRAL_ZONE_COLLECTION_TIMEOUT_SECONDS = 1.5,
             DELIVERY_TIMEOUT_SECONDS = 8,
             SCORING_TIMEOUT_SECONDS = 6;
 
diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java
index 09fe3019..b32c4768 100644
--- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java
+++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java
@@ -41,7 +41,7 @@ public class FieldConstants {
             LEFT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE = new FlippablePose2d(3.9, 7.4, Rotation2d.kZero, true),
             RIGHT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE = new FlippablePose2d(LEFT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE.getBlueObject().getX(), FIELD_WIDTH_METERS - LEFT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE.getBlueObject().getY(), Rotation2d.kZero, true),
             LEFT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE = new FlippablePose2d(5.53, LEFT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE.getBlueObject().getY(), Rotation2d.kZero, true),
-            RIGHT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE = new FlippablePose2d(LEFT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE.getBlueObject().getX(), FIELD_WIDTH_METERS - LEFT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE.getBlueObject().getY(), Rotation2d.kZero, true);
+            RIGHT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE = new FlippablePose2d(LEFT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE.getBlueObject().getX(), FIELD_WIDTH_METERS - LEFT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE.getBlueObject().getY(), Rotation2d.kZero, true);
     private static final double
             BLUE_RELATIVE_DELIVERY_POSITION_X = 3,
             DELIVERY_POSITION_Y_OFFSET_FROM_CENTER_METERS = 2.2;

From 39edd3be12bea070c2865e5efbe83544af3e4eee Mon Sep 17 00:00:00 2001
From: Nummun14 <142012009+Nummun14@users.noreply.github.com>
Date: Fri, 30 Jan 2026 08:26:15 +0200
Subject: [PATCH 09/12] bsuafayufhavyfuafvasuyfavyufcyucf

---
 .../commandclasses/IntakeAssistCommand.java       |  3 +--
 .../commandfactories/AutonomousCommands.java      |  2 +-
 .../robot/subsystems/swerve/SwerveCommands.java   | 15 ++++-----------
 3 files changed, 6 insertions(+), 14 deletions(-)

diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java
index 182e8955..7383aeaf 100644
--- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java
+++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java
@@ -145,8 +145,7 @@ private Translation2d getClosestGamePiece(Translation2d robotPosition, ArrayList
 
     private ArrayList<Translation2d> getAssistableGamePieces(Pose2d robotPose) {
         final ArrayList<Translation2d> gamePiecePositionsOnField = new ArrayList<>(RobotContainer.OBJECT_POSE_ESTIMATOR.getObjectsOnField());
-//        if (!DriverStation.isAutonomous())
-//            gamePiecePositionsOnField.removeIf(gamePiecePosition -> !isGamePieceAssistable(gamePiecePosition, robotPose));
+        gamePiecePositionsOnField.removeIf(gamePiecePosition -> !isGamePieceAssistable(gamePiecePosition, robotPose));
 
         return gamePiecePositionsOnField;
     }
diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
index 0eec1359..af7851c8 100644
--- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
+++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
@@ -45,7 +45,7 @@ public static Command getDeliveryCommand() {
     public static Command getCollectFromNeutralZoneCommand() {
         return new ParallelDeadlineGroup(
                 new SequentialCommandGroup(
-                        getSafeDriveToPoseCommand(isRight() ? () -> FieldConstants.RIGHT_INTAKE_POSITION : () -> FieldConstants.LEFT_INTAKE_POSITION, 3),
+                        getSafeDriveToPoseCommand(() -> isRight() ? FieldConstants.RIGHT_INTAKE_POSITION : FieldConstants.LEFT_INTAKE_POSITION, 3),
                         new GamePieceAutoDriveCommand().withTimeout(AutonomousConstants.NEUTRAL_ZONE_COLLECTION_TIMEOUT_SECONDS)
                 ),
                 IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.INTAKE),
diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java
index 243d5e3f..be98a67c 100644
--- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java
+++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java
@@ -3,10 +3,10 @@
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.path.PathConstraints;
 import edu.wpi.first.wpilibj2.command.*;
-import frc.trigon.robot.RobotContainer;
 import frc.trigon.lib.commands.InitExecuteCommand;
 import frc.trigon.lib.utilities.flippable.FlippablePose2d;
 import frc.trigon.lib.utilities.flippable.FlippableRotation2d;
+import frc.trigon.robot.RobotContainer;
 
 import java.util.Set;
 import java.util.function.DoubleSupplier;
@@ -129,25 +129,18 @@ public static Command getOpenLoopSelfRelativeDriveCommand(DoubleSupplier xSuppli
     }
 
     public static Command getDriveToPoseCommand(Supplier<FlippablePose2d> targetPose, PathConstraints constraints) {
-        return new DeferredCommand(() -> getCurrentDriveToPoseCommand(targetPose.get(), constraints), Set.of(RobotContainer.SWERVE));
+        return getDriveToPoseCommand(targetPose, constraints, 0.0);
     }
 
     public static Command getDriveToPoseCommand(Supplier<FlippablePose2d> targetPose, PathConstraints constraints, double endVelocity) {
         return new DeferredCommand(() -> getCurrentDriveToPoseCommand(targetPose.get(), constraints, endVelocity), Set.of(RobotContainer.SWERVE));
     }
 
-    private static Command getCurrentDriveToPoseCommand(FlippablePose2d targetPose, PathConstraints constraints) {
-        return new SequentialCommandGroup(
-                new InstantCommand(() -> RobotContainer.SWERVE.initializeDrive(true)),
-                AutoBuilder.pathfindToPose(targetPose.get(), constraints),
-                getPIDToPoseCommand(targetPose)
-        );
-    }
-
     private static Command getCurrentDriveToPoseCommand(FlippablePose2d targetPose, PathConstraints constraints, double endVelocity) {
         return new SequentialCommandGroup(
                 new InstantCommand(() -> RobotContainer.SWERVE.initializeDrive(true)),
-                AutoBuilder.pathfindToPose(targetPose.get(), constraints, endVelocity)
+                AutoBuilder.pathfindToPose(targetPose.get(), constraints, endVelocity),
+                getPIDToPoseCommand(targetPose).onlyIf(() -> endVelocity < 1e-3)
         );
     }
 

From 8d81cfa2944d944149ec6dba8e54a2d214180f1b Mon Sep 17 00:00:00 2001
From: Trigon5990 <Trigon5990@gmail.com>
Date: Fri, 30 Jan 2026 10:00:27 +0200
Subject: [PATCH 10/12] improvements

---
 .../commandclasses/IntakeAssistCommand.java   | 87 ++++++++-----------
 .../commandfactories/AutonomousCommands.java  |  5 +-
 .../commandfactories/FuelIntakeCommands.java  | 14 +--
 .../robot/constants/AutonomousConstants.java  |  2 +-
 .../robot/constants/FieldConstants.java       |  4 +-
 .../shootingphysics/ShootingCalculations.java |  4 +-
 .../subsystems/turret/TurretConstants.java    |  8 +-
 7 files changed, 57 insertions(+), 67 deletions(-)

diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java
index 7383aeaf..52f10780 100644
--- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java
+++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java
@@ -1,16 +1,16 @@
 package frc.trigon.robot.commands.commandclasses;
 
 import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj2.command.*;
 import frc.trigon.lib.hardware.RobotHardwareStats;
 import frc.trigon.lib.utilities.flippable.Flippable;
 import frc.trigon.robot.RobotContainer;
 import frc.trigon.robot.constants.OperatorConstants;
+import frc.trigon.robot.misc.shootingphysics.ShootingCalculations;
 import frc.trigon.robot.subsystems.swerve.SwerveCommands;
 import frc.trigon.robot.subsystems.swerve.SwerveConstants;
 import org.littletonrobotics.junction.Logger;
@@ -21,26 +21,26 @@
  * A command class to assist in the intaking of a game piece.
  */
 public class IntakeAssistCommand extends ParallelCommandGroup {
-    private final ProfiledPIDController
+    private final PIDController
             xPIDController = RobotHardwareStats.isSimulation() ?
-            new ProfiledPIDController(5, 0, 0, new TrapezoidProfile.Constraints(5, 15)) :
-            new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)),
+            new PIDController(5, 0, 0) :
+            new PIDController(2.4, 0, 0),
             yPIDController = RobotHardwareStats.isSimulation() ?
-                    new ProfiledPIDController(3, 0, 0.01, new TrapezoidProfile.Constraints(5, 15)) :
-                    new ProfiledPIDController(0.3, 0, 0.03, new TrapezoidProfile.Constraints(2.65, 5.5)),
+                    new PIDController(0.25, 0, 0) :
+                    new PIDController(0.26, 0, 0),
             thetaPIDController = RobotHardwareStats.isSimulation() ?
-                    new ProfiledPIDController(0.1, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) :
-                    new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5));
+                    new PIDController(0.1, 0, 0) :
+                    new PIDController(2.4, 0, 0);
     private Translation2d distanceFromTrackedGamePiece;
 
     /**
-     * Creates a new intake assist command that assists the driver with intaking a game piece based on the powers.
+     * Creates a new intake assist command that assists the driver with intaking a game piece.
      *
-     * @param xAssistPower     the amount of assist to apply to the X-axis (Forward and backwards)
-     * @param yAssistPower     the amount of assist to apply to the Y-axis (Sideways)
-     * @param thetaAssistPower the amount of assist to apply to the rotation
+     * @param shouldAssistX     should apply to the X-axis (Forward and backwards)
+     * @param shouldAssistY     should apply to the Y-axis (Sideways)
+     * @param shouldAssistTheta should apply to the rotation
      */
-    public IntakeAssistCommand(double xAssistPower, double yAssistPower, double thetaAssistPower) {
+    public IntakeAssistCommand(boolean shouldAssistX, boolean shouldAssistY, boolean shouldAssistTheta) {
         addCommands(
                 new RunCommand(this::trackGamePiece),
                 new SequentialCommandGroup(
@@ -49,28 +49,28 @@ public IntakeAssistCommand(double xAssistPower, double yAssistPower, double thet
                         new WaitUntilCommand(this::hasNoTrackedGamePiece)
                 ).repeatedly(),
                 SwerveCommands.getClosedLoopSelfRelativeDriveCommand(
-                        () -> calculateTranslationPower(true, xAssistPower),
-                        () -> calculateTranslationPower(false, yAssistPower),
-                        () -> calculateThetaPower(thetaAssistPower)
+                        () -> calculateTranslationPower(true, shouldAssistX),
+                        () -> calculateTranslationPower(false, shouldAssistY),
+                        () -> calculateThetaPower(shouldAssistTheta)
                 )
         );
     }
 
     private void resetPIDControllers() {
-        xPIDController.reset(distanceFromTrackedGamePiece.getX(), RobotContainer.SWERVE.getSelfRelativeVelocity().getX());
-        yPIDController.reset(distanceFromTrackedGamePiece.getY(), RobotContainer.SWERVE.getSelfRelativeVelocity().getY());
-        thetaPIDController.reset(distanceFromTrackedGamePiece.getAngle().getRadians(), RobotContainer.SWERVE.getRotationalVelocityRadiansPerSecond());
+        xPIDController.reset();
+        yPIDController.reset();
+        thetaPIDController.reset();
 
-        xPIDController.setGoal(0);
-        yPIDController.setGoal(0);
-        thetaPIDController.setGoal(0);
+        xPIDController.setSetpoint(0);
+        yPIDController.setSetpoint(0);
+        thetaPIDController.setSetpoint(0);
     }
 
-    private double calculateTranslationPower(boolean isXAxis, double assistScalar) {
+    private double calculateTranslationPower(boolean isXAxis, boolean shouldAssist) {
         final Translation2d selfRelativeJoystickValue = getSelfRelativeJoystickPosition();
         final double joystickValue = isXAxis ? selfRelativeJoystickValue.getX() : selfRelativeJoystickValue.getY();
 
-        if (hasNoTrackedGamePiece())
+        if (!shouldAssist || hasNoTrackedGamePiece())
             return joystickValue;
 
         final double
@@ -78,27 +78,25 @@ private double calculateTranslationPower(boolean isXAxis, double assistScalar) {
                 xPIDController.calculate(distanceFromTrackedGamePiece.getX()) :
                 yPIDController.calculate(distanceFromTrackedGamePiece.getY()));
 
-        return calculateAssistPower(assistScalar, pidOutput, joystickValue);
+        return calculateAssistPower(pidOutput, joystickValue);
     }
 
-    private double calculateThetaPower(double assistScalar) {
+    private double calculateThetaPower(boolean shouldAssist) {
         final double joystickValue = OperatorConstants.DRIVER_CONTROLLER.getRightX();
 
-        if (hasNoTrackedGamePiece())
+        if (!shouldAssist || hasNoTrackedGamePiece())
             return joystickValue;
 
-        return calculateThetaAssistPower(assistScalar, joystickValue, distanceFromTrackedGamePiece.getAngle().unaryMinus());
+        return calculateThetaAssistPower(joystickValue, distanceFromTrackedGamePiece.getAngle().unaryMinus());
     }
 
-    private double calculateThetaAssistPower(double assistScalar, double joystickValue, Rotation2d thetaOffset) {
+    private double calculateThetaAssistPower(double joystickValue, Rotation2d thetaOffset) {
         final double pidOutput = clampToSwerveOutputRange(thetaPIDController.calculate(thetaOffset.getRadians()));
-        return calculateAssistPower(assistScalar, pidOutput, joystickValue);
+        return calculateAssistPower(pidOutput, joystickValue);
     }
 
-    private double calculateAssistPower(double assistScalar, double pidOutput, double joystickValue) {
-        if (assistScalar == 0)
-            return joystickValue;
-        return (joystickValue * (1 - assistScalar)) + (pidOutput * assistScalar);
+    private double calculateAssistPower(double pidOutput, double joystickValue) {
+        return joystickValue + pidOutput;
     }
 
     private boolean hasNoTrackedGamePiece() {
@@ -165,9 +163,10 @@ private boolean isGamePieceWithinAssistFOV(Translation2d gamePiecePosition, Pose
     }
 
     private boolean isDrivingTowardsGamePiece(Translation2d gamePieceFieldRelativePosition, Pose2d robotPose) {
-        final Translation2d fieldRelativeDistanceFromGamePiece = gamePieceFieldRelativePosition.minus(robotPose.getTranslation());
-
-        final double velocityTowardsGamePiece = calculateVelocityTowardsGamePiece(fieldRelativeDistanceFromGamePiece);
+        final Translation2d velocityRelativeToGamePiece = ShootingCalculations.calculateVelocityRelativeToPoint(gamePieceFieldRelativePosition, robotPose.getTranslation(), getFieldRelativeJoystickPosition());
+        if (velocityRelativeToGamePiece.getX() < Math.abs(velocityRelativeToGamePiece.getY()))
+            return false;
+        final double velocityTowardsGamePiece = velocityRelativeToGamePiece.getX() * SwerveConstants.MAXIMUM_SPEED_METERS_PER_SECOND;
         return velocityTowardsGamePiece > OperatorConstants.MINIMUM_VELOCITY_TOWARDS_GAME_PIECE_FOR_INTAKE_ASSIST_METERS_PER_SECOND;
     }
 
@@ -175,18 +174,6 @@ private double calculateMaximumAssistAngleDegrees(double gamePieceDistance) {
         return OperatorConstants.INTAKE_ASSIST_MAXIMUM_ASSISTABLE_ANGLE_FORMULA.applyAsDouble(gamePieceDistance);
     }
 
-    private double calculateVelocityTowardsGamePiece(Translation2d fieldRelativeDistanceFromGamePiece) {
-        final Translation2d fieldRelativeJoystickPosition = getFieldRelativeJoystickPosition();
-
-        final Rotation2d joystickAngle = fieldRelativeJoystickPosition.getNorm() < 1e-6 ? new Rotation2d() : fieldRelativeJoystickPosition.getAngle();
-        final Rotation2d fieldRelativeGamePieceAngleFromRobot = fieldRelativeDistanceFromGamePiece.getAngle();
-        final Rotation2d angularOffset = joystickAngle.minus(fieldRelativeGamePieceAngleFromRobot);
-
-        final double desiredRobotVelocity = fieldRelativeJoystickPosition.getNorm() * SwerveConstants.MAXIMUM_SPEED_METERS_PER_SECOND;
-
-        return angularOffset.getCos() * desiredRobotVelocity;
-    }
-
     private double clampToSwerveOutputRange(double value) {
         return MathUtil.clamp(value, -1, 1);
     }
diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
index af7851c8..64e59a83 100644
--- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
+++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
@@ -32,7 +32,10 @@ public static Command getAutonomousCommand() {
 
     public static Command getDeliveryCommand() {
         return new ParallelCommandGroup(
-                getSafeDriveToPoseCommand(isRight() ? () -> FieldConstants.RIGHT_INTAKE_POSITION : () -> FieldConstants.LEFT_INTAKE_POSITION),
+                new SequentialCommandGroup(
+                        getSafeDriveToPoseCommand(() -> isRight() ? FieldConstants.RIGHT_INTAKE_POSITION : FieldConstants.LEFT_INTAKE_POSITION, 3),
+                        new GamePieceAutoDriveCommand()
+                ),
                 IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.INTAKE),
                 GeneralCommands.getContinuousConditionalCommand(
                         ShootingCommands.getShootAtHubCommand(),
diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/FuelIntakeCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/FuelIntakeCommands.java
index beac4875..30ce2611 100644
--- a/src/main/java/frc/trigon/robot/commands/commandfactories/FuelIntakeCommands.java
+++ b/src/main/java/frc/trigon/robot/commands/commandfactories/FuelIntakeCommands.java
@@ -13,10 +13,10 @@ public class FuelIntakeCommands {
             SHOULD_KEEP_INTAKE_OPEN = new LoggedNetworkBoolean("/SmartDashboard/ShouldKeepIntakeOpen", true),
             SHOULD_ASSIST_INTAKE = new LoggedNetworkBoolean("/SmartDashboard/ShouldAssistIntake", true);
 
-    public static final double
-            X_ASSIST_POWER = 0.0,
-            Y_ASSIST_POWER = 0.5,
-            THETA_ASSIST_POWER = 0.0;
+    public static final boolean
+            SHOULD_ASSIST_X = false,
+            SHOULD_ASSIST_Y = true,
+            SHOULD_ASSIST_THETA = false;
 
     public static Command getIntakeCommand() {
         return new ParallelCommandGroup(
@@ -45,9 +45,9 @@ public static Command getDisableIntakeAssistCommand() {
 
     private static Command getIntakeAssistCommand() {
         return new IntakeAssistCommand(
-                X_ASSIST_POWER,
-                Y_ASSIST_POWER,
-                THETA_ASSIST_POWER
+                SHOULD_ASSIST_X,
+                SHOULD_ASSIST_Y,
+                SHOULD_ASSIST_THETA
         );
     }
 }
\ No newline at end of file
diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
index 8bf14b4e..12360960 100644
--- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
+++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
@@ -60,7 +60,7 @@ public class AutonomousConstants {
             new PIDController(0.3, 0, 0.03);
     public static final PIDController GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER = RobotHardwareStats.isSimulation() ?
             new PIDController(0.5, 0, 0) :
-            new PIDController(2.4, 0, 0);
+            new PIDController(1, 0, 0);
     public static final double AUTO_COLLECTION_INTAKE_OPEN_CHECK_DISTANCE_METERS = 0.01;
 
     private static final PPHolonomicDriveController AUTO_PATH_FOLLOWING_CONTROLLER = new PPHolonomicDriveController(
diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java
index b32c4768..3148add9 100644
--- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java
+++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java
@@ -46,12 +46,12 @@ public class FieldConstants {
             BLUE_RELATIVE_DELIVERY_POSITION_X = 3,
             DELIVERY_POSITION_Y_OFFSET_FROM_CENTER_METERS = 2.2;
     public static final FlippableTranslation2d
-            HUB_POSITION = new FlippableTranslation2d(4.625594, FIELD_WIDTH_METERS / 2, true),
+            HUB_POSITION = new FlippableTranslation2d(4.7, FIELD_WIDTH_METERS / 2, true),
             RIGHT_DELIVERY_POSITION = new FlippableTranslation2d(BLUE_RELATIVE_DELIVERY_POSITION_X, (FIELD_WIDTH_METERS / 2) - DELIVERY_POSITION_Y_OFFSET_FROM_CENTER_METERS, true),
             LEFT_DELIVERY_POSITION = new FlippableTranslation2d(BLUE_RELATIVE_DELIVERY_POSITION_X, (FIELD_WIDTH_METERS / 2) + DELIVERY_POSITION_Y_OFFSET_FROM_CENTER_METERS, true);
     public static final double
             ALLIANCE_ZONE_LENGTH = 4,
-            DELIVERY_ZONE_START_BLUE_X = ALLIANCE_ZONE_LENGTH + 0.5;
+            DELIVERY_ZONE_START_BLUE_X = ALLIANCE_ZONE_LENGTH + 1;
 
     private static AprilTagFieldLayout createAprilTagFieldLayout() {
         try {
diff --git a/src/main/java/frc/trigon/robot/misc/shootingphysics/ShootingCalculations.java b/src/main/java/frc/trigon/robot/misc/shootingphysics/ShootingCalculations.java
index aa84bca2..e8f9466a 100644
--- a/src/main/java/frc/trigon/robot/misc/shootingphysics/ShootingCalculations.java
+++ b/src/main/java/frc/trigon/robot/misc/shootingphysics/ShootingCalculations.java
@@ -132,12 +132,12 @@ private Rotation2d calculateTargetRobotAngle(Translation2d robotFieldRelativeVel
         return robotAngleToHub.plus(targetSelfRelativeYaw);
     }
 
-    private Translation2d calculateVelocityRelativeToPoint(Translation2d fieldPoint, Translation2d currentPosition, Translation2d robotFieldRelativeVelocity) {
+    public static Translation2d calculateVelocityRelativeToPoint(Translation2d fieldPoint, Translation2d currentPosition, Translation2d robotFieldRelativeVelocity) {
         final Rotation2d angleToPoint = calculateAngleToPoint(fieldPoint, currentPosition);
         return robotFieldRelativeVelocity.rotateBy(angleToPoint.unaryMinus());
     }
 
-    private Rotation2d calculateAngleToPoint(Translation2d fieldPoint, Translation2d currentPosition) {
+    public static Rotation2d calculateAngleToPoint(Translation2d fieldPoint, Translation2d currentPosition) {
         return fieldPoint.minus(currentPosition).getAngle();
     }
 }
diff --git a/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java
index 3a9ae6fd..365f84d1 100644
--- a/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java
+++ b/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java
@@ -38,7 +38,7 @@ public class TurretConstants {
     private static final double GEAR_RATIO = 52;
     private static final double CURRENT_LIMIT_AMPS = 100;
     private static final MotorAlignmentValue FOLLOWER_ALIGNMENT_TO_MASTER = MotorAlignmentValue.Aligned;
-    static final double RESIST_SWERVE_ROTATION_FEEDFORWARD_GAIN = RobotHardwareStats.isSimulation() ? 0.5 : 0;
+    static final double RESIST_SWERVE_ROTATION_FEEDFORWARD_GAIN = RobotHardwareStats.isSimulation() ? 0.2 : 0;
 
     private static final int MOTOR_AMOUNT = 2;
     private static final DCMotor GEARBOX = DCMotor.getFalcon500Foc(MOTOR_AMOUNT);
@@ -96,12 +96,12 @@ private static void configureMasterMotor() {
         config.ClosedLoopGeneral.GainSchedKpBehavior = GainSchedKpBehaviorValue.Discontinuous;
         config.ClosedLoopGeneral.GainSchedErrorThreshold = 0.007;
 
-        config.Slot0.kP = RobotHardwareStats.isSimulation() ? 75 : 0;
+        config.Slot0.kP = RobotHardwareStats.isSimulation() ? 110 : 0;
         config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0;
-        config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.3 : 0;
+        config.Slot0.kD = RobotHardwareStats.isSimulation() ? 1 : 0;
         config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.02 : 0;
         config.Slot0.kV = RobotHardwareStats.isSimulation() ? 6.2 : 0;
-        config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0 : 0;
+        config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.05 : 0;
         config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign;
         config.Slot0.GainSchedBehavior = GainSchedBehaviorValue.Inactive;
 

From 0a7fc5655f3942bb70729b26ee498193b12a5f3e Mon Sep 17 00:00:00 2001
From: Trigon5990 <Trigon5990@gmail.com>
Date: Fri, 30 Jan 2026 12:05:13 +0200
Subject: [PATCH 11/12] Calib (not really)

---
 .../robot/constants/AutonomousConstants.java  |  4 ++--
 .../subsystems/turret/TurretConstants.java    | 20 +++++++++----------
 2 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
index 12360960..496dfe71 100644
--- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
+++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
@@ -32,7 +32,7 @@ public class AutonomousConstants {
     public static final RobotConfig ROBOT_CONFIG = getRobotConfig();
     public static final double FEEDFORWARD_SCALAR = 0.7;//TODO: Calibrate
     public static final PathConstraints
-            DRIVE_IN_AUTONOMOUS_CONSTRAINTS = new PathConstraints(2.5, 4.5, Units.degreesToRadians(450), Units.degreesToRadians(900)),
+            DRIVE_IN_AUTONOMOUS_CONSTRAINTS = new PathConstraints(3.3, 3, Units.degreesToRadians(200), Units.degreesToRadians(200)),
             DRIVE_SLOWLY_IN_AUTONOMOUS_CONSTRAINTS = new PathConstraints(1.5, 3.0, Units.degreesToRadians(300), Units.degreesToRadians(600));
     public static LoggedDashboardChooser<Supplier<Command>>
             FIRST_AUTONOMOUS_CHOOSER = new LoggedDashboardChooser<>("FirstAutonomousChooser", new SendableChooser<>()),
@@ -41,7 +41,7 @@ public class AutonomousConstants {
     public static LoggedDashboardChooser<FlippablePose2d> CLIMB_POSITION_CHOOSER = new LoggedDashboardChooser<>("ClimbChooser", new SendableChooser<>());
 
     public static double
-            DEPOT_COLLECTION_TIMEOUT_SECONDS = 4,
+            DEPOT_COLLECTION_TIMEOUT_SECONDS = 6,
             NEUTRAL_ZONE_COLLECTION_TIMEOUT_SECONDS = 1.5,
             DELIVERY_TIMEOUT_SECONDS = 8,
             SCORING_TIMEOUT_SECONDS = 6;
diff --git a/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java
index 365f84d1..e59e5c6f 100644
--- a/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java
+++ b/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java
@@ -35,10 +35,10 @@ public class TurretConstants {
     static final CANcoderEncoder ENCODER = new CANcoderEncoder(ENCODER_ID, ENCODER_NAME);
 
     static final boolean FOC_ENABLED = true;
-    private static final double GEAR_RATIO = 52;
+    private static final double GEAR_RATIO = 64.8;
     private static final double CURRENT_LIMIT_AMPS = 100;
     private static final MotorAlignmentValue FOLLOWER_ALIGNMENT_TO_MASTER = MotorAlignmentValue.Aligned;
-    static final double RESIST_SWERVE_ROTATION_FEEDFORWARD_GAIN = RobotHardwareStats.isSimulation() ? 0.2 : 0;
+    static final double RESIST_SWERVE_ROTATION_FEEDFORWARD_GAIN = RobotHardwareStats.isSimulation() ? 0 : 0;
 
     private static final int MOTOR_AMOUNT = 2;
     private static final DCMotor GEARBOX = DCMotor.getFalcon500Foc(MOTOR_AMOUNT);
@@ -93,15 +93,15 @@ private static void configureMasterMotor() {
         config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
         config.Feedback.SensorToMechanismRatio = GEAR_RATIO;
 
-        config.ClosedLoopGeneral.GainSchedKpBehavior = GainSchedKpBehaviorValue.Discontinuous;
+        config.ClosedLoopGeneral.GainSchedKpBehavior = GainSchedKpBehaviorValue.Continuous;
         config.ClosedLoopGeneral.GainSchedErrorThreshold = 0.007;
 
-        config.Slot0.kP = RobotHardwareStats.isSimulation() ? 110 : 0;
+        config.Slot0.kP = RobotHardwareStats.isSimulation() ? 75 : 0;
         config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0;
-        config.Slot0.kD = RobotHardwareStats.isSimulation() ? 1 : 0;
-        config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.02 : 0;
-        config.Slot0.kV = RobotHardwareStats.isSimulation() ? 6.2 : 0;
-        config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.05 : 0;
+        config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 0;
+        config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.01 : 0;
+        config.Slot0.kV = RobotHardwareStats.isSimulation() ? 7.5 : 0;
+        config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.0005 : 0;
         config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign;
         config.Slot0.GainSchedBehavior = GainSchedBehaviorValue.Inactive;
 
@@ -113,8 +113,8 @@ private static void configureMasterMotor() {
         config.Slot1.kA = 0;
         config.Slot1.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign;
 
-        config.MotionMagic.MotionMagicCruiseVelocity = RobotHardwareStats.isSimulation() ? 1.5 : 5;
-        config.MotionMagic.MotionMagicAcceleration = RobotHardwareStats.isSimulation() ? 90 : 5;
+        config.MotionMagic.MotionMagicCruiseVelocity = RobotHardwareStats.isSimulation() ? 1 : 5;
+        config.MotionMagic.MotionMagicAcceleration = RobotHardwareStats.isSimulation() ? 70 : 5;
         config.MotionMagic.MotionMagicJerk = 0;
 
         config.CurrentLimits.StatorCurrentLimitEnable = true;

From 738db4d239020e8a62a8561780a79e6efc4cee2f Mon Sep 17 00:00:00 2001
From: Trigon5990 <Trigon5990@gmail.com>
Date: Sun, 1 Feb 2026 05:28:28 +0200
Subject: [PATCH 12/12] TONS OF UGLY CODE AND WIBE CODING

---
 .../java/frc/trigon/robot/RobotContainer.java |   6 +-
 .../commandclasses/IntakeAssistCommand.java   |   7 +-
 .../commandclasses/LEDAutoSetupCommand.java   |   4 +-
 .../GamePieceAutoDriveCommand.java            | 290 +++++++++++++++---
 .../GamePieceAutoDriveConstants.java          |  11 +
 .../commandfactories/AutonomousCommands.java  | 187 -----------
 .../commandfactories/ShootingCommands.java    |  13 +-
 .../autonomous/GeneralAutonomousCommands.java | 146 +++++++++
 .../SafeAutonomousDriveCommands.java          | 107 +++++++
 .../robot/constants/AutonomousConstants.java  |  19 +-
 .../robot/constants/FieldConstants.java       |   6 +-
 .../SimulatedGamePieceConstants.java          |  25 ++
 .../SimulationFieldHandler.java               |   8 +-
 .../robot/subsystems/hood/HoodConstants.java  |   2 +-
 .../subsystems/shooter/ShooterConstants.java  |   2 +-
 .../spindexer/SpindexerConstants.java         |   2 +-
 .../robot/subsystems/turret/Turret.java       |  19 +-
 .../subsystems/turret/TurretConstants.java    |  14 +-
 18 files changed, 606 insertions(+), 262 deletions(-)
 delete mode 100644 src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
 create mode 100644 src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java
 create mode 100644 src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/SafeAutonomousDriveCommands.java

diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java
index 58fa6ccd..dc27ee49 100644
--- a/src/main/java/frc/trigon/robot/RobotContainer.java
+++ b/src/main/java/frc/trigon/robot/RobotContainer.java
@@ -10,7 +10,7 @@
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.trigon.lib.utilities.flippable.Flippable;
 import frc.trigon.robot.commands.CommandConstants;
-import frc.trigon.robot.commands.commandfactories.AutonomousCommands;
+import frc.trigon.robot.commands.commandfactories.autonomous.GeneralAutonomousCommands;
 import frc.trigon.robot.commands.commandfactories.FuelIntakeCommands;
 import frc.trigon.robot.commands.commandfactories.GeneralCommands;
 import frc.trigon.robot.commands.commandfactories.ShootingCommands;
@@ -63,7 +63,7 @@ public RobotContainer() {
      * @return the command to run in autonomous mode
      */
     public Command getAutonomousCommand() {
-        return AutonomousCommands.getAutonomousCommand();
+        return GeneralAutonomousCommands.getAutonomousCommand();
     }
 
     private void configureBindings() {
@@ -100,7 +100,7 @@ private void bindControllerCommands() {
         OperatorConstants.SET_FIXED_SHOOTING_POSITION_LEFT_CORNER_TRIGGER.onTrue(ShootingCommands.getChangeFixedShootingPositionCommand(ShootingCommands.FixedShootingPosition.LEFT_CORNER));
         OperatorConstants.SET_FIXED_SHOOTING_POSITION_CLOSE_TO_TOWER_TRIGGER.onTrue(ShootingCommands.getChangeFixedShootingPositionCommand(ShootingCommands.FixedShootingPosition.CLOSE_TO_TOWER));
         OperatorConstants.SET_FIXED_SHOOTING_POSITION_CLOSE_TO_OUTPOST_TRIGGER.onTrue(ShootingCommands.getChangeFixedShootingPositionCommand(ShootingCommands.FixedShootingPosition.CLOSE_TO_OUTPOST));
-        OperatorConstants.SHORT_EJECTION_TRIGGER.whileTrue(ShootingCommands.getShortEjectFuelCommand());
+        OperatorConstants.SHORT_EJECTION_TRIGGER.whileTrue(TurretCommands.getDebuggingCommand());
     }
 
     private void configureSysIDBindings(MotorSubsystem subsystem) {
diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java
index 52f10780..9536eab2 100644
--- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java
+++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java
@@ -9,6 +9,7 @@
 import frc.trigon.lib.hardware.RobotHardwareStats;
 import frc.trigon.lib.utilities.flippable.Flippable;
 import frc.trigon.robot.RobotContainer;
+import frc.trigon.robot.commands.CommandConstants;
 import frc.trigon.robot.constants.OperatorConstants;
 import frc.trigon.robot.misc.shootingphysics.ShootingCalculations;
 import frc.trigon.robot.subsystems.swerve.SwerveCommands;
@@ -26,7 +27,7 @@ public class IntakeAssistCommand extends ParallelCommandGroup {
             new PIDController(5, 0, 0) :
             new PIDController(2.4, 0, 0),
             yPIDController = RobotHardwareStats.isSimulation() ?
-                    new PIDController(0.25, 0, 0) :
+                    new PIDController(0.3, 0, 0) :
                     new PIDController(0.26, 0, 0),
             thetaPIDController = RobotHardwareStats.isSimulation() ?
                     new PIDController(0.1, 0, 0) :
@@ -68,7 +69,7 @@ private void resetPIDControllers() {
 
     private double calculateTranslationPower(boolean isXAxis, boolean shouldAssist) {
         final Translation2d selfRelativeJoystickValue = getSelfRelativeJoystickPosition();
-        final double joystickValue = isXAxis ? selfRelativeJoystickValue.getX() : selfRelativeJoystickValue.getY();
+        final double joystickValue = CommandConstants.calculateDriveStickAxisValue(isXAxis ? selfRelativeJoystickValue.getX() : selfRelativeJoystickValue.getY());
 
         if (!shouldAssist || hasNoTrackedGamePiece())
             return joystickValue;
@@ -82,7 +83,7 @@ private double calculateTranslationPower(boolean isXAxis, boolean shouldAssist)
     }
 
     private double calculateThetaPower(boolean shouldAssist) {
-        final double joystickValue = OperatorConstants.DRIVER_CONTROLLER.getRightX();
+        final double joystickValue = CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getRightX());
 
         if (!shouldAssist || hasNoTrackedGamePiece())
             return joystickValue;
diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/LEDAutoSetupCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/LEDAutoSetupCommand.java
index aa76f81f..f95525e4 100644
--- a/src/main/java/frc/trigon/robot/commands/commandclasses/LEDAutoSetupCommand.java
+++ b/src/main/java/frc/trigon/robot/commands/commandclasses/LEDAutoSetupCommand.java
@@ -7,7 +7,7 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.trigon.robot.RobotContainer;
-import frc.trigon.robot.commands.commandfactories.AutonomousCommands;
+import frc.trigon.robot.commands.commandfactories.autonomous.GeneralAutonomousCommands;
 import org.littletonrobotics.junction.Logger;
 
 import java.util.function.Supplier;
@@ -44,7 +44,7 @@ public boolean runsWhenDisabled() {
 
     private Command getUpdateAutoStartPoseCommand() {
         return new InstantCommand(() -> {
-            this.autoStartPose = AutonomousCommands.getAutoStartPose(autoName.get());
+            this.autoStartPose = GeneralAutonomousCommands.getAutoStartPose(autoName.get());
             Logger.recordOutput("PathPlanner/AutoStartPose", autoStartPose);
         });
     }
diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveCommand.java
index 6606bf39..2f11f300 100644
--- a/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveCommand.java
+++ b/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveCommand.java
@@ -1,6 +1,7 @@
 package frc.trigon.robot.commands.commandclasses.gamepieceautodrive;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.*;
 import frc.trigon.lib.utilities.flippable.FlippableRotation2d;
@@ -16,8 +17,19 @@
 public class GamePieceAutoDriveCommand extends ParallelCommandGroup {
     private static final ObjectPoseEstimator OBJECT_POSE_ESTIMATOR = RobotContainer.OBJECT_POSE_ESTIMATOR;
     private final AtomicReference<GamePieceCluster> currentTargetCluster = new AtomicReference<>();
+    // Raw approach heading (optionally rate-limited).  NOT wall-clamped — the
+    // clamp is applied only in getTargetHeading() so the position clamp can
+    // still see the unclamped heading.
+    private final AtomicReference<FlippableRotation2d> commandedHeading = new AtomicReference<>();
+    // Keeps the last heading that was actually valid so that brief vision gaps
+    // (cluster == null for a cycle) don't send null to the swerve and release
+    // heading control, letting the robot spin freely.
+    private volatile FlippableRotation2d lastValidHeading = null;
+    /** When true, drive speed ramps down and rotation is rate-limited near the piece. */
+    private final boolean slowIntake;
 
-    public GamePieceAutoDriveCommand() {
+    public GamePieceAutoDriveCommand(boolean slowIntake) {
+        this.slowIntake = slowIntake;
         addCommands(
                 createTargetUpdateCommand(),
                 createDriveCommand()
@@ -26,73 +38,274 @@ public GamePieceAutoDriveCommand() {
 
     private Command createTargetUpdateCommand() {
         return new RunCommand(() -> {
+            GamePieceCluster prev = currentTargetCluster.get();
             GamePieceCluster bestCluster = findBestCluster();
             currentTargetCluster.set(bestCluster);
+            if (prev == null && bestCluster != null) {
+                AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.reset();
+                AutonomousConstants.GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER.reset();
+            }
+            updateCommandedHeading();
         });
     }
 
     private Command createDriveCommand() {
-        return new SequentialCommandGroup(
-                new InstantCommand(AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER::reset),
-                SwerveCommands.getClosedLoopSelfRelativeDriveCommand(
-                        this::getXControllerOutput,
-                        this::getYControllerOutput,
-                        this::getTargetHeading
-                )
-        ).onlyWhile(this::hasValidTarget);
+        return SwerveCommands.getClosedLoopSelfRelativeDriveCommand(
+                this::getXControllerOutput,
+                this::getYControllerOutput,
+                this::getTargetHeading
+        );
+    }
+
+    // =========================================================================
+    // HEADING
+    // =========================================================================
+
+    private void updateCommandedHeading() {
+        GamePieceCluster cluster = currentTargetCluster.get();
+        if (cluster == null) {
+            // Don't null out — keep last heading so the swerve maintains
+            // orientation during brief vision gaps instead of spinning freely.
+            return;
+        }
+
+        FlippableRotation2d target = new FlippableRotation2d(cluster.getApproachHeading(), false);
+
+        if (!slowIntake || !isWithinIntakeDistance()) {
+            commandedHeading.set(target);
+            lastValidHeading = target;
+            return;
+        }
+
+        // Rate-limit inside the intake zone.
+        FlippableRotation2d prev = commandedHeading.get();
+        if (prev == null) {
+            commandedHeading.set(target);
+            lastValidHeading = target;
+            return;
+        }
+
+        double maxDeltaDeg = GamePieceAutoDriveConstants.MAX_INTAKE_ROTATION_RATE_DEG_PER_SEC * 0.02;
+        double deltaDeg = Math.IEEEremainder(target.get().getDegrees() - prev.get().getDegrees(), 360.0);
+        if (Math.abs(deltaDeg) > maxDeltaDeg) {
+            deltaDeg = Math.signum(deltaDeg) * maxDeltaDeg;
+        }
+
+        FlippableRotation2d result = new FlippableRotation2d(
+                Rotation2d.fromDegrees(prev.get().getDegrees() + deltaDeg), false);
+        commandedHeading.set(result);
+        lastValidHeading = result;
     }
 
+    /**
+     * What actually goes to the swerve.  The wall clamp is applied HERE so
+     * that commandedHeading stays clean for the position clamp to read.
+     */
+    private FlippableRotation2d getTargetHeading() {
+        FlippableRotation2d heading = commandedHeading.get();
+        if (heading == null) heading = lastValidHeading;
+        return clampHeadingForWall(heading);
+    }
+
+    /**
+     * Prevents the intake from hitting the wall by blocking headings that
+     * would swing the intake tip past the effective wall.
+     *
+     * ── Geometry (intake at BACK, i.e. 180° in robot frame) ──
+     * The intake tip is (ROBOT_HALF_WIDTH + INTAKE_REACH) behind the robot
+     * centre.  In world frame:
+     *     tipX = robotX  –  intakeLength * cos(heading)
+     *
+     * Require tipX >= effectiveWallX:
+     *     cos(heading)  <=  (robotX – effectiveWallX) / intakeLength  =  cosMax
+     *
+     * cos(θ) <= cosMax is satisfied when |θ| >= acos(cosMax).
+     * So the FORBIDDEN zone is  |heading| < acos(cosMax)  (near 0°, where the
+     * intake points toward the wall).
+     * The ALLOWED zone is everything else (near ±180°, intake away from wall).
+     *
+     * If the heading is in the forbidden zone it is pushed to the nearest
+     * boundary (± acos(cosMax)), which keeps it as close to the desired
+     * approach as possible while staying safe.
+     */
+    private FlippableRotation2d clampHeadingForWall(FlippableRotation2d heading) {
+        if (heading == null) return null;
+
+        Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
+        double intakeLength = GamePieceAutoDriveConstants.ROBOT_HALF_WIDTH
+                + GamePieceAutoDriveConstants.INTAKE_REACH;
+
+        double cosMax = (robotPose.getX() - getEffectiveWallX()) / intakeLength;
+
+        // Robot is far enough that even heading = 0° (intake straight at wall)
+        // keeps the tip clear.  No clamping needed.
+        if (cosMax >= 1.0) return heading;
+
+        // Robot is so close that NO heading is safe.  Face 180° (intake
+        // maximally away from wall) as best-effort emergency.
+        if (cosMax < -1.0) {
+            return new FlippableRotation2d(Rotation2d.fromDegrees(180), false);
+        }
+
+        // Normal case: forbidden zone is |heading| < minAbsAngle.
+        double minAbsAngleDeg = Math.toDegrees(Math.acos(cosMax));
+        double headingDeg = Math.IEEEremainder(heading.get().getDegrees(), 360.0);
+
+        if (Math.abs(headingDeg) < minAbsAngleDeg) {
+            // In the forbidden zone — push to the nearest allowed boundary.
+            // signum preserves which side of 0° we were on so the robot doesn't
+            // snap across; exact 0° defaults to +minAbsAngle.
+            headingDeg = (headingDeg >= 0) ? minAbsAngleDeg : -minAbsAngleDeg;
+        }
+
+        return new FlippableRotation2d(Rotation2d.fromDegrees(headingDeg), false);
+    }
+
+    // =========================================================================
+    // DRIVE OUTPUTS
+    // =========================================================================
+
     private double getXControllerOutput() {
+        if (!shouldDrive()) return 0.0;
         Translation2d error = getTranslationError();
         if (error == null) return 0.0;
-
-        // CRITICAL FIX 1: Negate the error here.
-        // Controller calculates (Setpoint - Measurement).
-        // We want (0 - (-Distance)) = +Distance to drive forward.
-        return AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.calculate(-error.getX());
+        double output = AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.calculate(-error.getX());
+        return output * getIntakeSpeedScale();
     }
 
     private double getYControllerOutput() {
+        if (!shouldDrive()) return 0.0;
         Translation2d error = getTranslationError();
         if (error == null) return 0.0;
+        double output = AutonomousConstants.GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER.calculate(-error.getY());
+        return output * getIntakeSpeedScale();
+    }
 
-        // CRITICAL FIX 1: Negate the error here too.
-        return AutonomousConstants.GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER.calculate(-error.getY());
+    /**
+     * Drive is active as long as there is a reachable target and the robot
+     * hasn't closed to intake distance.  No emergency "past-the-wall stop"
+     * here: if the robot has overshot the clamped target, the target is now
+     * behind it and the PID error naturally drives it back.  Stopping would
+     * lock it in the unsafe position.
+     */
+    private boolean shouldDrive() {
+        Translation2d error = getTranslationError();
+        if (error == null) return false;
+        if (error.getNorm() <= AutonomousConstants.AUTO_COLLECTION_INTAKE_OPEN_CHECK_DISTANCE_METERS)
+            return false;
+        return true;
     }
 
-    private FlippableRotation2d getTargetHeading() {
-        GamePieceCluster cluster = currentTargetCluster.get();
-        if (cluster == null) return null;
-        return new FlippableRotation2d(cluster.getApproachHeading(), false);
+    /**
+     * Linear ramp from 1.0 (at INTAKE_SLOWDOWN_DISTANCE) down to
+     * INTAKE_DRIVE_SPEED_SCALE (at distance 0).  Only active when slowIntake
+     * is enabled.
+     */
+    private double getIntakeSpeedScale() {
+        if (!slowIntake) return 1.0;
+
+        Translation2d error = getTranslationError();
+        if (error == null) return 1.0;
+
+        double dist = error.getNorm();
+        double slowdownDist = GamePieceAutoDriveConstants.INTAKE_SLOWDOWN_DISTANCE_METERS;
+        if (dist >= slowdownDist) return 1.0;
+
+        double t = dist / slowdownDist;
+        return GamePieceAutoDriveConstants.INTAKE_DRIVE_SPEED_SCALE
+                + t * (1.0 - GamePieceAutoDriveConstants.INTAKE_DRIVE_SPEED_SCALE);
     }
 
-    private Translation2d getTranslationError() {
+    private boolean isWithinIntakeDistance() {
+        Translation2d error = getTranslationError();
+        return error != null
+                && error.getNorm() < GamePieceAutoDriveConstants.INTAKE_SLOWDOWN_DISTANCE_METERS;
+    }
+
+    // =========================================================================
+    // WALL SAFETY & TARGET CLAMPING
+    // =========================================================================
+
+    /**
+     * The real wall shifted inward by the safety margin.  All geometry
+     * checks use this as "the wall" so the robot + intake never get closer
+     * than WALL_SAFETY_MARGIN_METERS to the actual wall.
+     */
+    private static double getEffectiveWallX() {
+        return GamePieceAutoDriveConstants.ALLIANCE_WALL_X_METERS
+                + 0.05;
+    }
+
+    /**
+     * Cluster centroid clamped in X so the robot is safe at ANY heading.
+     *
+     * Uses the worst-case heading (0° = intake pointing directly at the wall,
+     * because the intake is at the back).  This is the global maximum of minX
+     * across all headings, so it guarantees safety regardless of what heading
+     * the robot passes through while rotating.  Only pieces near the wall are
+     * affected; pieces further out are unclamped.
+     *
+     * If the robot has overshot (is closer than the clamped target), the
+     * target is behind it and the PID drives it back automatically.
+     */
+    private Translation2d getEffectiveTarget() {
         GamePieceCluster cluster = currentTargetCluster.get();
         if (cluster == null) return null;
 
-        Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
-        Translation2d fieldRelativeError = cluster.getCentroid().minus(robotPose.getTranslation());
+        Translation2d target = cluster.getCentroid();
+        // Worst case heading for intake-at-back is 0° (intake straight at wall).
+        double minX = getMinSafeRobotX(Rotation2d.fromDegrees(0));
 
-        // CRITICAL FIX 2: You MUST use unaryMinus() here.
-        // This rotates the "World Vector" backwards to match the "Robot's Perspective".
-        // Without this, Left/Right are swapped, causing swerving.
-        return fieldRelativeError.rotateBy(robotPose.getRotation().unaryMinus());
+        if (target.getX() < minX) {
+            target = new Translation2d(minX, target.getY());
+        }
+        return target;
     }
 
-    private boolean hasValidTarget() {
-        Translation2d error = getTranslationError();
-        Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
+    /**
+     * Minimum robot-centre X that keeps the body and intake clear of the
+     * effective wall, for a given heading.
+     *
+     * ── Body ──
+     * Square, half-width ROBOT_HALF_WIDTH.  The farthest any corner extends
+     * in –X from centre = halfWidth × (|cos θ| + |sin θ|).
+     *
+     * ── Intake (at BACK = 180° in robot frame) ──
+     * Tip is intakeLength behind centre.  In world frame:
+     *     tipX = robotX – intakeLength × cos(θ)
+     * The tip extends toward –X (the wall) when cos(θ) > 0, i.e. |θ| < 90°.
+     * The backward (toward-wall) extent is therefore max(0, intakeLength × cos(θ)).
+     */
+    private double getMinSafeRobotX(Rotation2d heading) {
+        double cosH = Math.abs(heading.getCos());
+        double sinH = Math.abs(heading.getSin());
+
+        double bodyBackwardExtent = GamePieceAutoDriveConstants.ROBOT_HALF_WIDTH * (cosH + sinH);
 
-        // CRITICAL FIX 3: Stop if the ROBOT is past the line.
-        // Previously we only checked if the GAME PIECE was past the line.
-        boolean robotIsSafe = robotPose.getX() < GamePieceAutoDriveConstants.MAX_COLLECTION_X_METERS;
+        // Intake at back: extends toward the wall when cos(heading) > 0.
+        double intakeBackwardExtent = Math.max(0.0,
+                (GamePieceAutoDriveConstants.ROBOT_HALF_WIDTH + GamePieceAutoDriveConstants.INTAKE_REACH)
+                        * heading.getCos());
 
-        return error != null &&
-                robotIsSafe &&
-                error.getNorm() > AutonomousConstants.AUTO_COLLECTION_INTAKE_OPEN_CHECK_DISTANCE_METERS;
+        return getEffectiveWallX() + Math.max(bodyBackwardExtent, intakeBackwardExtent);
     }
 
-    // --- Vision Logic (Unchanged) ---
+    /**
+     * Robot-frame translation error to the effective (wall-clamped) target.
+     */
+    private Translation2d getTranslationError() {
+        Translation2d effectiveTarget = getEffectiveTarget();
+        if (effectiveTarget == null) return null;
+
+        Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
+        Translation2d fieldRelativeError = effectiveTarget.minus(robotPose.getTranslation());
+        return fieldRelativeError.rotateBy(robotPose.getRotation().unaryMinus());
+    }
+
+    // =========================================================================
+    // VISION LOGIC
+    // =========================================================================
+
     private GamePieceCluster findBestCluster() {
         List<Translation2d> allObjects = OBJECT_POSE_ESTIMATOR.getObjectsOnField();
         Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
@@ -134,6 +347,7 @@ private boolean isOutOfBounds(Translation2d piece) {
     }
 
     private double calculateClusterScore(int count, double distance) {
-        return (count * GamePieceAutoDriveConstants.SCORE_WEIGHT_COUNT) - (distance * GamePieceAutoDriveConstants.SCORE_PENALTY_DISTANCE);
+        return (count * GamePieceAutoDriveConstants.SCORE_WEIGHT_COUNT)
+                - (distance * GamePieceAutoDriveConstants.SCORE_PENALTY_DISTANCE);
     }
 }
\ No newline at end of file
diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveConstants.java b/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveConstants.java
index 578f9df1..6b286929 100644
--- a/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveConstants.java
+++ b/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveConstants.java
@@ -10,4 +10,15 @@ public class GamePieceAutoDriveConstants {
     static final double LINEARITY_THRESHOLD = 6.0;
     static final double BLEND_START_DISTANCE_METERS = 1.5;
     static final double BLEND_END_DISTANCE_METERS = 0.5;
+
+    // Intake slowdown
+    public static final double INTAKE_SLOWDOWN_DISTANCE_METERS      = 0.8;   // start ramping speed here
+    public static final double INTAKE_DRIVE_SPEED_SCALE              = 0.35;  // minimum speed multiplier at the piece
+
+    // Rotation rate limit while intaking
+    public static final double MAX_INTAKE_ROTATION_RATE_DEG_PER_SEC = 60.0;  // tune on-field
+
+    public static final double ROBOT_HALF_WIDTH      = 0.37;  // half-side of the robot square
+    public static final double INTAKE_REACH          = 0.4;  // how far the intake extends past the body
+    public static final double ALLIANCE_WALL_X_METERS = 0.0;  // field X coordinate of the alliance wall
 }
\ No newline at end of file
diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
deleted file mode 100644
index 64e59a83..00000000
--- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
+++ /dev/null
@@ -1,187 +0,0 @@
-package frc.trigon.robot.commands.commandfactories;
-
-import com.pathplanner.lib.commands.PathPlannerAuto;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj2.command.*;
-import frc.trigon.lib.utilities.flippable.FlippablePose2d;
-import frc.trigon.robot.RobotContainer;
-import frc.trigon.robot.commands.commandclasses.gamepieceautodrive.GamePieceAutoDriveCommand;
-import frc.trigon.robot.constants.AutonomousConstants;
-import frc.trigon.robot.constants.FieldConstants;
-import frc.trigon.robot.subsystems.intake.IntakeCommands;
-import frc.trigon.robot.subsystems.intake.IntakeConstants;
-import frc.trigon.robot.subsystems.swerve.SwerveCommands;
-import org.json.simple.parser.ParseException;
-
-import java.io.IOException;
-import java.util.function.Supplier;
-
-/**
- * A class that contains command factories for preparation commands and commands used during the 15-second autonomous period at the start of each match.
- */
-public class AutonomousCommands {
-    public static Command getAutonomousCommand() {
-        return new SequentialCommandGroup(
-                AutonomousConstants.FIRST_AUTONOMOUS_CHOOSER.get() == null ? new InstantCommand() : AutonomousConstants.FIRST_AUTONOMOUS_CHOOSER.get().get(),
-                AutonomousConstants.SECOND_AUTONOMOUS_CHOOSER.get() == null ? new InstantCommand() : AutonomousConstants.SECOND_AUTONOMOUS_CHOOSER.get().get(),
-                AutonomousConstants.THIRD_AUTONOMOUS_CHOOSER.get() == null ? new InstantCommand() : AutonomousConstants.THIRD_AUTONOMOUS_CHOOSER.get().get(),
-                getClimbCommand(AutonomousConstants.CLIMB_POSITION_CHOOSER.get()).onlyIf(() -> AutonomousConstants.CLIMB_POSITION_CHOOSER.get() != null)
-        );
-    }
-
-    public static Command getDeliveryCommand() {
-        return new ParallelCommandGroup(
-                new SequentialCommandGroup(
-                        getSafeDriveToPoseCommand(() -> isRight() ? FieldConstants.RIGHT_INTAKE_POSITION : FieldConstants.LEFT_INTAKE_POSITION, 3),
-                        new GamePieceAutoDriveCommand()
-                ),
-                IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.INTAKE),
-                GeneralCommands.getContinuousConditionalCommand(
-                        ShootingCommands.getShootAtHubCommand(),
-                        ShootingCommands.getDeliveryCommand(),
-                        AutonomousCommands::isInAllianceZone
-                )
-        ).withTimeout(AutonomousConstants.DELIVERY_TIMEOUT_SECONDS);
-    }
-
-    public static Command getCollectFromNeutralZoneCommand() {
-        return new ParallelDeadlineGroup(
-                new SequentialCommandGroup(
-                        getSafeDriveToPoseCommand(() -> isRight() ? FieldConstants.RIGHT_INTAKE_POSITION : FieldConstants.LEFT_INTAKE_POSITION, 3),
-                        new GamePieceAutoDriveCommand().withTimeout(AutonomousConstants.NEUTRAL_ZONE_COLLECTION_TIMEOUT_SECONDS)
-                ),
-                IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.INTAKE),
-                ShootingCommands.getShootAtHubCommand().onlyWhile(AutonomousCommands::isInAllianceZone)
-        );
-    }
-
-    public static Command getScoreCommand() {
-        return new ParallelCommandGroup(
-                getSafeDriveToPoseCommand(() -> isRight() ? FieldConstants.RIGHT_IDEAL_SHOOTING_POSITION : FieldConstants.LEFT_IDEAL_SHOOTING_POSITION),
-                ShootingCommands.getShootAtHubCommand().onlyWhile(AutonomousCommands::isInAllianceZone).repeatedly()
-        ).withTimeout(AutonomousConstants.SCORING_TIMEOUT_SECONDS);
-    }
-
-    public static Command getCollectFromDepotCommand() {
-        return new SequentialCommandGroup(
-                new ParallelCommandGroup(
-                        getSafeDriveToPoseCommand(() -> FieldConstants.DEPOT_POSITION),
-                        ShootingCommands.getShootAtHubCommand().onlyWhile(AutonomousCommands::isInAllianceZone).repeatedly()
-                ).until(() -> RobotContainer.SWERVE.atPose(FieldConstants.DEPOT_POSITION)),
-                new GamePieceAutoDriveCommand().alongWith(ShootingCommands.getShootAtHubCommand())
-        ).alongWith(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.INTAKE)).withTimeout(AutonomousConstants.DEPOT_COLLECTION_TIMEOUT_SECONDS);
-    }
-
-    public static Command getClimbCommand(FlippablePose2d climbPosition) {
-        return new SequentialCommandGroup(
-                getSafeDriveToPoseCommand(() -> climbPosition),
-                new InstantCommand() // TODO: Add climb command
-        );
-    }
-
-    private static Command getSafeDriveToPoseCommand(Supplier<FlippablePose2d> targetPose) {
-        return getSafeDriveToPoseCommand(targetPose, 0);
-    }
-
-    private static Command getSafeDriveToPoseCommand(Supplier<FlippablePose2d> targetPose, double endVelocity) {
-        return new ConditionalCommand(
-                getDriveThroughTrenchCommand(targetPose, endVelocity),
-                SwerveCommands.getDriveToPoseCommand(targetPose, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS, endVelocity),
-                () -> shouldDriveThroughTrench(targetPose.get())
-        );
-    }
-
-    private static Command getDriveThroughTrenchCommand(Supplier<FlippablePose2d> targetPose, double endVelocity) {
-        return new SequentialCommandGroup(
-                SwerveCommands.getDriveToPoseCommand(AutonomousCommands::getTrenchEntryPose, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS, 4),
-                SwerveCommands.getDriveToPoseCommand(() -> AutonomousCommands.getTrenchExitPose(targetPose.get()), AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS, 4),
-                SwerveCommands.getDriveToPoseCommand(targetPose, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS, endVelocity)
-        );
-    }
-
-    private static boolean shouldDriveThroughTrench(FlippablePose2d targetPose) {
-        return (!isInAllianceZone() && isPoseInAllianceZone(targetPose)) || (isInAllianceZone() && !isPoseInAllianceZone(targetPose));
-    }
-
-    private static FlippablePose2d getTrenchExitPose(FlippablePose2d targetPose) {
-        final FlippablePose2d targetTrenchExitPose = isRight() ?
-                getClosestPoseToPose(targetPose, FieldConstants.RIGHT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE, FieldConstants.RIGHT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE) :
-                getClosestPoseToPose(targetPose, FieldConstants.LEFT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE, FieldConstants.LEFT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE);
-        if (RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().getDegrees() > 90 || RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().getDegrees() < -90)
-            return new FlippablePose2d(targetTrenchExitPose.getBlueObject().getTranslation(), Math.PI, true);
-        return targetTrenchExitPose;
-    }
-
-    private static FlippablePose2d getTrenchEntryPose() {
-        final FlippablePose2d targetTrenchEntryPose = isRight() ?
-                isInAllianceZone() ? FieldConstants.RIGHT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE : FieldConstants.RIGHT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE :
-                isInAllianceZone() ? FieldConstants.LEFT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE : FieldConstants.LEFT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE;
-        if (RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().getDegrees() > 90)
-            return new FlippablePose2d(targetTrenchEntryPose.getBlueObject().getTranslation(), Math.PI, true);
-        return targetTrenchEntryPose;
-    }
-
-    private static FlippablePose2d getClosestPoseToPose(FlippablePose2d pose, FlippablePose2d... poses) {
-        FlippablePose2d closestPose = null;
-        double closestDistance = Double.MAX_VALUE;
-        for (FlippablePose2d candidatePose : poses) {
-            final double distance = pose.get().getTranslation().getDistance(candidatePose.get().getTranslation());
-            if (distance < closestDistance) {
-                closestDistance = distance;
-                closestPose = candidatePose;
-            }
-        }
-        return closestPose;
-    }
-
-    private static boolean isRight() {
-        return RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation().getY() < FieldConstants.FIELD_WIDTH_METERS / 2;
-    }
-
-    private static boolean isInAllianceZone() {
-        return isPoseInAllianceZone(new FlippablePose2d(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(), true));
-    }
-
-    private static boolean isPoseInAllianceZone(FlippablePose2d pose) {
-        return pose.get().getX() < FieldConstants.ALLIANCE_ZONE_LENGTH;
-    }
-
-    private static boolean shouldCollectGamePiecesFromNeutralZone() {
-        final Pose2d currentRobotPose = new FlippablePose2d(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(), true).get();
-        return currentRobotPose.getX() > FieldConstants.DELIVERY_ZONE_START_BLUE_X && RobotContainer.OBJECT_POSE_ESTIMATOR.hasObjects();
-    }
-
-    /**
-     * Creates a command that resets the pose estimator's pose to the starting pose of the given autonomous as long as the robot is not enabled.
-     *
-     * @param autoName the name of the autonomous
-     * @return a command that resets the robot's pose estimator pose to the start position of the given autonomous
-     */
-    public static Command getResetPoseToAutoPoseCommand(Supplier<String> autoName) {
-        return new InstantCommand(
-                () -> {
-                    if (DriverStation.isEnabled())
-                        return;
-                    RobotContainer.ROBOT_POSE_ESTIMATOR.resetPose(getAutoStartPose(autoName.get()));
-                }
-        ).ignoringDisable(true);
-    }
-
-    /**
-     * Gets the starting position of the target PathPlanner autonomous.
-     *
-     * @param autoName the name of the autonomous group
-     * @return the staring pose of the autonomous
-     */
-    public static Pose2d getAutoStartPose(String autoName) {
-        try {
-            final Pose2d nonFlippedAutoStartPose = PathPlannerAuto.getPathGroupFromAutoFile(autoName).get(0).getStartingHolonomicPose().get();
-            final FlippablePose2d flippedAutoStartPose = new FlippablePose2d(nonFlippedAutoStartPose, true);
-            return flippedAutoStartPose.get();
-        } catch (IOException | ParseException e) {
-            e.printStackTrace();
-            return new Pose2d();
-        }
-    }
-}
\ No newline at end of file
diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/ShootingCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/ShootingCommands.java
index 1df38877..0ca50317 100644
--- a/src/main/java/frc/trigon/robot/commands/commandfactories/ShootingCommands.java
+++ b/src/main/java/frc/trigon/robot/commands/commandfactories/ShootingCommands.java
@@ -96,7 +96,7 @@ private static Command getLoadFuelWhenReadyCommand(boolean isAutoShootingAtHub)
         return new SequentialCommandGroup(
                 new WaitUntilCommand(() -> canShoot(isAutoShootingAtHub)),
                 getLoadFuelCommand()
-                        .until(ShootingCommands::shouldStopShooting)
+                        .until(() -> ShootingCommands.shouldStopShooting(isAutoShootingAtHub))
         ).repeatedly();
     }
 
@@ -108,8 +108,11 @@ private static Command getLoadFuelCommand() {
     }
 
     private static boolean canShoot(boolean isShootingAtHub) {
-        return (!isShootingAtHub || canShootAtHub()) &&
-                RobotContainer.SHOOTER.atTargetVelocity()
+        if (isShootingAtHub) {
+            return canShootAtHub() && RobotContainer.SHOOTER.atTargetVelocity() && RobotContainer.HOOD.atTargetAngle() &&
+                    RobotContainer.TURRET.atTargetShootingCalculationsAngle(false);
+        }
+        return RobotContainer.SHOOTER.atTargetVelocity()
                 && RobotContainer.HOOD.atTargetAngle()
                 && RobotContainer.TURRET.atTargetAngle(false);
     }
@@ -118,8 +121,8 @@ private static boolean canShootAtHub() {
         return RobotContainer.SHOOTER.isAimingAtHub();
     }
 
-    private static boolean shouldStopShooting() {
-        return !RobotContainer.TURRET.atTargetAngle(true);
+    private static boolean shouldStopShooting(boolean isShootingAtHub) {
+        return isShootingAtHub ? !RobotContainer.TURRET.atTargetShootingCalculationsAngle(true) : !RobotContainer.TURRET.atTargetAngle(true);
     }
 
     private static void updateShootingCalculations() {
diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java
new file mode 100644
index 00000000..46d7957c
--- /dev/null
+++ b/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java
@@ -0,0 +1,146 @@
+package frc.trigon.robot.commands.commandfactories.autonomous;
+
+import com.pathplanner.lib.commands.PathPlannerAuto;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.*;
+import frc.trigon.lib.utilities.flippable.FlippablePose2d;
+import frc.trigon.robot.RobotContainer;
+import frc.trigon.robot.commands.commandclasses.gamepieceautodrive.GamePieceAutoDriveCommand;
+import frc.trigon.robot.commands.commandfactories.GeneralCommands;
+import frc.trigon.robot.commands.commandfactories.ShootingCommands;
+import frc.trigon.robot.constants.AutonomousConstants;
+import frc.trigon.robot.constants.FieldConstants;
+import frc.trigon.robot.subsystems.intake.IntakeCommands;
+import frc.trigon.robot.subsystems.intake.IntakeConstants;
+import org.json.simple.parser.ParseException;
+
+import java.io.IOException;
+import java.util.function.Supplier;
+
+/**
+ * A class that contains command factories for preparation commands and commands used during the 15-second autonomous period at the start of each match.
+ */
+public class GeneralAutonomousCommands {
+    public static Command getAutonomousCommand() {
+        return new SequentialCommandGroup(
+                AutonomousConstants.FIRST_AUTONOMOUS_CHOOSER.get() == null ? new InstantCommand() : AutonomousConstants.FIRST_AUTONOMOUS_CHOOSER.get().get(),
+                AutonomousConstants.SECOND_AUTONOMOUS_CHOOSER.get() == null ? new InstantCommand() : AutonomousConstants.SECOND_AUTONOMOUS_CHOOSER.get().get(),
+                AutonomousConstants.THIRD_AUTONOMOUS_CHOOSER.get() == null ? new InstantCommand() : AutonomousConstants.THIRD_AUTONOMOUS_CHOOSER.get().get(),
+                getClimbCommand(AutonomousConstants.CLIMB_POSITION_CHOOSER.get()).onlyIf(() -> AutonomousConstants.CLIMB_POSITION_CHOOSER.get() != null)
+        );
+    }
+
+    public static Command getDeliveryCommand(boolean shootPreload, double collectionTimeout) {
+        return new ParallelDeadlineGroup(
+                getDriveToFuelInNeutralZoneCommand(shootPreload, collectionTimeout),
+                IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.INTAKE),
+                GeneralCommands.getContinuousConditionalCommand(
+                        ShootingCommands.getShootAtHubCommand(),
+                        ShootingCommands.getDeliveryCommand(),
+                        SafeAutonomousDriveCommands::isInAllianceZone
+                )
+        );
+    }
+
+    public static Command getCollectFromNeutralZoneCommand(boolean shootPreload, double collectionTimeout) {
+        return new ParallelDeadlineGroup(
+                getDriveToFuelInNeutralZoneCommand(shootPreload, collectionTimeout),
+                IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.INTAKE),
+                ShootingCommands.getShootAtHubCommand().onlyWhile(SafeAutonomousDriveCommands::isInAllianceZone)
+        );
+    }
+
+    public static Command getScoreCommand(double timeout) {
+        return new ParallelCommandGroup(
+                SafeAutonomousDriveCommands.getSafeDriveToPoseCommand(
+                        () -> SafeAutonomousDriveCommands.isRight() ? FieldConstants.RIGHT_IDEAL_SHOOTING_POSITION : FieldConstants.LEFT_IDEAL_SHOOTING_POSITION,
+                        AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS,
+                        0,
+                        AutonomousConstants.DRIVE_SLOWLY_IN_AUTONOMOUS_CONSTRAINTS,
+                        1000
+                ),
+                ShootingCommands.getShootAtHubCommand().onlyWhile(SafeAutonomousDriveCommands::isInAllianceZone).repeatedly()
+        ).withTimeout(timeout);
+    }
+
+    public static Command getCollectFromDepotCommand(boolean shootWhileDriving, double collectionTimeout) {
+        return new SequentialCommandGroup(
+                new ParallelCommandGroup(
+                        SafeAutonomousDriveCommands.getSafeDriveToPoseCommand(
+                                () -> FieldConstants.DEPOT_POSITION,
+                                AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS,
+                                0,
+                                AutonomousConstants.DRIVE_SLOWLY_IN_AUTONOMOUS_CONSTRAINTS,
+                                shootWhileDriving ? 1000 : 0
+                        ),
+                        ShootingCommands.getShootAtHubCommand().onlyWhile(SafeAutonomousDriveCommands::isInAllianceZone).repeatedly()
+                ).until(() -> RobotContainer.SWERVE.atPose(FieldConstants.DEPOT_POSITION)),
+                new GamePieceAutoDriveCommand(true).alongWith(ShootingCommands.getShootAtHubCommand()).withTimeout(collectionTimeout)
+        ).alongWith(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.INTAKE));
+    }
+
+    public static Command getClimbCommand(FlippablePose2d climbPosition) {
+        return new SequentialCommandGroup(
+                SafeAutonomousDriveCommands.getSafeDriveToPoseCommand(
+                        () -> climbPosition,
+                        AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS,
+                        0,
+                        AutonomousConstants.DRIVE_SLOWLY_IN_AUTONOMOUS_CONSTRAINTS,
+                        1000
+                ),
+                new InstantCommand() // TODO: Add climb command
+        );
+    }
+
+    private static Command getDriveToFuelInNeutralZoneCommand(boolean shootPreload, double timeout) {
+        return new SequentialCommandGroup(
+                SafeAutonomousDriveCommands.getSafeDriveToPoseCommand(
+                        () -> SafeAutonomousDriveCommands.isRight() ? FieldConstants.RIGHT_INTAKE_POSITION : FieldConstants.LEFT_INTAKE_POSITION,
+                        AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS,
+                        3,
+                        AutonomousConstants.SHOOT_PRELOAD_BEFORE_NEUTRAL_ZONE_DRIVE_CONSTRAINTS,
+                        shootPreload ? AutonomousConstants.SHOOT_PRELOAD_BEFORE_NEUTRAL_ZONE_DRIVE_TIME : 0
+                ),
+                new GamePieceAutoDriveCommand(false).withTimeout(timeout)
+        );
+    }
+
+    private static boolean shouldCollectGamePiecesFromNeutralZone() {
+        final Pose2d currentRobotPose = new FlippablePose2d(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(), true).get();
+        return currentRobotPose.getX() > FieldConstants.DELIVERY_ZONE_START_BLUE_X && RobotContainer.OBJECT_POSE_ESTIMATOR.hasObjects();
+    }
+
+    /**
+     * Creates a command that resets the pose estimator's pose to the starting pose of the given autonomous as long as the robot is not enabled.
+     *
+     * @param autoName the name of the autonomous
+     * @return a command that resets the robot's pose estimator pose to the start position of the given autonomous
+     */
+    public static Command getResetPoseToAutoPoseCommand(Supplier<String> autoName) {
+        return new InstantCommand(
+                () -> {
+                    if (DriverStation.isEnabled())
+                        return;
+                    RobotContainer.ROBOT_POSE_ESTIMATOR.resetPose(getAutoStartPose(autoName.get()));
+                }
+        ).ignoringDisable(true);
+    }
+
+    /**
+     * Gets the starting position of the target PathPlanner autonomous.
+     *
+     * @param autoName the name of the autonomous group
+     * @return the staring pose of the autonomous
+     */
+    public static Pose2d getAutoStartPose(String autoName) {
+        try {
+            final Pose2d nonFlippedAutoStartPose = PathPlannerAuto.getPathGroupFromAutoFile(autoName).get(0).getStartingHolonomicPose().get();
+            final FlippablePose2d flippedAutoStartPose = new FlippablePose2d(nonFlippedAutoStartPose, true);
+            return flippedAutoStartPose.get();
+        } catch (IOException | ParseException e) {
+            e.printStackTrace();
+            return new Pose2d();
+        }
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/SafeAutonomousDriveCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/SafeAutonomousDriveCommands.java
new file mode 100644
index 00000000..bbf92c2c
--- /dev/null
+++ b/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/SafeAutonomousDriveCommands.java
@@ -0,0 +1,107 @@
+package frc.trigon.robot.commands.commandfactories.autonomous;
+
+import com.pathplanner.lib.path.PathConstraints;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.trigon.lib.utilities.flippable.FlippablePose2d;
+import frc.trigon.robot.RobotContainer;
+import frc.trigon.robot.constants.AutonomousConstants;
+import frc.trigon.robot.constants.FieldConstants;
+import frc.trigon.robot.subsystems.swerve.SwerveCommands;
+
+import java.util.function.Supplier;
+
+public class SafeAutonomousDriveCommands {
+    public static Command getSafeDriveToPoseCommand(Supplier<FlippablePose2d> targetPose, PathConstraints normalPathConstrains) {
+        return getSafeDriveToPoseCommand(targetPose, normalPathConstrains, 0);
+    }
+
+    public static Command getSafeDriveToPoseCommand(Supplier<FlippablePose2d> targetPose, PathConstraints normalPathConstrains, double endVelocity) {
+        return getSafeDriveToPoseCommand(targetPose, normalPathConstrains, endVelocity, null, 0);
+    }
+
+    public static Command getSafeDriveToPoseCommand(
+            Supplier<FlippablePose2d> targetPose, PathConstraints normalPathConstrains,
+            double endVelocity, PathConstraints driveSlowlyInAllianceZoneConstraints, double driveSlowlyInAllianceZoneTime
+    ) {
+        return new ConditionalCommand(
+                getDriveThroughTrenchCommand(targetPose, normalPathConstrains, endVelocity, driveSlowlyInAllianceZoneConstraints, driveSlowlyInAllianceZoneTime),
+                getDriveSlowlyInAllianceZoneCommand(targetPose, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS, endVelocity, driveSlowlyInAllianceZoneConstraints, driveSlowlyInAllianceZoneTime),
+                () -> shouldDriveThroughTrench(targetPose.get())
+        );
+    }
+
+    private static Command getDriveThroughTrenchCommand(
+            Supplier<FlippablePose2d> targetPose, PathConstraints normalPathConstrains,
+            double endVelocity, PathConstraints driveSlowlyInAllianceZoneConstraints, double driveSlowlyInAllianceZoneTime
+    ) {
+        return new SequentialCommandGroup(
+                getDriveSlowlyInAllianceZoneCommand(SafeAutonomousDriveCommands::getTrenchEntryPose, normalPathConstrains, 4, driveSlowlyInAllianceZoneConstraints, driveSlowlyInAllianceZoneTime),
+                getDriveSlowlyInAllianceZoneCommand(() -> SafeAutonomousDriveCommands.getTrenchExitPose(targetPose.get()), normalPathConstrains, 4, driveSlowlyInAllianceZoneConstraints, driveSlowlyInAllianceZoneTime),
+                getDriveSlowlyInAllianceZoneCommand(targetPose, normalPathConstrains, endVelocity, driveSlowlyInAllianceZoneConstraints, driveSlowlyInAllianceZoneTime)
+        );
+    }
+
+    private static Command getDriveSlowlyInAllianceZoneCommand(
+            Supplier<FlippablePose2d> targetPose, PathConstraints normalPathConstrains,
+            double endVelocity, PathConstraints driveSlowlyInAllianceZoneConstraints, double driveSlowlyInAllianceZoneTime
+    ) {
+        if (driveSlowlyInAllianceZoneTime == 0)
+            return SwerveCommands.getDriveToPoseCommand(targetPose, normalPathConstrains, endVelocity);
+
+        return new SequentialCommandGroup(
+                SwerveCommands.getDriveToPoseCommand(targetPose, normalPathConstrains, endVelocity).until(SafeAutonomousDriveCommands::isInAllianceZone),
+                SwerveCommands.getDriveToPoseCommand(targetPose, driveSlowlyInAllianceZoneConstraints, endVelocity).withTimeout(driveSlowlyInAllianceZoneTime).onlyWhile(SafeAutonomousDriveCommands::isInAllianceZone),
+                SwerveCommands.getDriveToPoseCommand(targetPose, normalPathConstrains, endVelocity)
+        );
+    }
+
+    private static boolean shouldDriveThroughTrench(FlippablePose2d targetPose) {
+        return (!isInAllianceZone() && isPoseInAllianceZone(targetPose)) || (isInAllianceZone() && !isPoseInAllianceZone(targetPose));
+    }
+
+    private static FlippablePose2d getTrenchExitPose(FlippablePose2d targetPose) {
+        final FlippablePose2d targetTrenchExitPose = isRight() ?
+                getClosestPoseToPose(targetPose, FieldConstants.RIGHT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE, FieldConstants.RIGHT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE) :
+                getClosestPoseToPose(targetPose, FieldConstants.LEFT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE, FieldConstants.LEFT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE);
+        if (RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().getDegrees() > 90 || RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().getDegrees() < -90)
+            return new FlippablePose2d(targetTrenchExitPose.getBlueObject().getTranslation(), Math.PI, true);
+        return targetTrenchExitPose;
+    }
+
+    private static FlippablePose2d getTrenchEntryPose() {
+        final FlippablePose2d targetTrenchEntryPose = isRight() ?
+                isInAllianceZone() ? FieldConstants.RIGHT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE : FieldConstants.RIGHT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE :
+                isInAllianceZone() ? FieldConstants.LEFT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE : FieldConstants.LEFT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE;
+        if (RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().getDegrees() > 90)
+            return new FlippablePose2d(targetTrenchEntryPose.getBlueObject().getTranslation(), Math.PI, true);
+        return targetTrenchEntryPose;
+    }
+
+    private static FlippablePose2d getClosestPoseToPose(FlippablePose2d pose, FlippablePose2d... poses) {
+        FlippablePose2d closestPose = null;
+        double closestDistance = Double.MAX_VALUE;
+        for (FlippablePose2d candidatePose : poses) {
+            final double distance = pose.get().getTranslation().getDistance(candidatePose.get().getTranslation());
+            if (distance < closestDistance) {
+                closestDistance = distance;
+                closestPose = candidatePose;
+            }
+        }
+        return closestPose;
+    }
+
+    public static boolean isRight() {
+        return RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation().getY() < FieldConstants.FIELD_WIDTH_METERS / 2;
+    }
+
+    public static boolean isInAllianceZone() {
+        return isPoseInAllianceZone(new FlippablePose2d(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(), true));
+    }
+
+    private static boolean isPoseInAllianceZone(FlippablePose2d pose) {
+        return pose.get().getX() < FieldConstants.ALLIANCE_ZONE_LENGTH;
+    }
+
+}
diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
index 496dfe71..b98cb629 100644
--- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
+++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
@@ -17,7 +17,7 @@
 import frc.trigon.lib.utilities.flippable.Flippable;
 import frc.trigon.lib.utilities.flippable.FlippablePose2d;
 import frc.trigon.robot.RobotContainer;
-import frc.trigon.robot.commands.commandfactories.AutonomousCommands;
+import frc.trigon.robot.commands.commandfactories.autonomous.GeneralAutonomousCommands;
 import org.json.simple.parser.ParseException;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
@@ -32,8 +32,13 @@ public class AutonomousConstants {
     public static final RobotConfig ROBOT_CONFIG = getRobotConfig();
     public static final double FEEDFORWARD_SCALAR = 0.7;//TODO: Calibrate
     public static final PathConstraints
-            DRIVE_IN_AUTONOMOUS_CONSTRAINTS = new PathConstraints(3.3, 3, Units.degreesToRadians(200), Units.degreesToRadians(200)),
-            DRIVE_SLOWLY_IN_AUTONOMOUS_CONSTRAINTS = new PathConstraints(1.5, 3.0, Units.degreesToRadians(300), Units.degreesToRadians(600));
+            DRIVE_IN_AUTONOMOUS_CONSTRAINTS = new PathConstraints(4, 7, Units.degreesToRadians(100), Units.degreesToRadians(100)),
+            SHOOT_PRELOAD_BEFORE_NEUTRAL_ZONE_DRIVE_CONSTRAINTS = new PathConstraints(0.2, 0.5, Units.degreesToRadians(100), Units.degreesToRadians(100)),
+            SHOOT_PRELOAD_BEFORE_COLLECTING_FROM_DEPOT_CONSTRAINTS = new PathConstraints(1.5, 3.0, Units.degreesToRadians(100), Units.degreesToRadians(100)),
+            DRIVE_SLOWLY_IN_AUTONOMOUS_CONSTRAINTS = new PathConstraints(2.5, 2, Units.degreesToRadians(100), Units.degreesToRadians(100));
+    public static final double
+            SHOOT_PRELOAD_BEFORE_NEUTRAL_ZONE_DRIVE_TIME = 1,
+            SHOOT_PRELOAD_BEFORE_COLLECTING_FROM_DEPOT_TIME = 2;
     public static LoggedDashboardChooser<Supplier<Command>>
             FIRST_AUTONOMOUS_CHOOSER = new LoggedDashboardChooser<>("FirstAutonomousChooser", new SendableChooser<>()),
             SECOND_AUTONOMOUS_CHOOSER = new LoggedDashboardChooser<>("SecondAutonomousChooser", new SendableChooser<>()),
@@ -108,10 +113,10 @@ private static void initializeAutoChoosers() {
     }
 
     private static void configureAutonomousPositionChooser(LoggedDashboardChooser<Supplier<Command>> firstAutonomousChooser) {
-        firstAutonomousChooser.addOption("Depot", AutonomousCommands::getCollectFromDepotCommand);
-        firstAutonomousChooser.addOption("Score", AutonomousCommands::getScoreCommand);
-        firstAutonomousChooser.addOption("CollectFromNeutralZone", AutonomousCommands::getCollectFromNeutralZoneCommand);
-        firstAutonomousChooser.addOption("Delivery", AutonomousCommands::getDeliveryCommand);
+        firstAutonomousChooser.addOption("Depot", () -> GeneralAutonomousCommands.getCollectFromDepotCommand(true, 6));
+        firstAutonomousChooser.addOption("Score", () -> GeneralAutonomousCommands.getScoreCommand(4));
+        firstAutonomousChooser.addOption("CollectFromNeutralZone", () -> GeneralAutonomousCommands.getCollectFromNeutralZoneCommand(true, 1.5));
+        firstAutonomousChooser.addOption("Delivery", () -> GeneralAutonomousCommands.getDeliveryCommand(true, 6));
         firstAutonomousChooser.addDefaultOption("Nothing", null);
     }
 
diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java
index 3148add9..fcc51d4f 100644
--- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java
+++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java
@@ -33,8 +33,8 @@ public class FieldConstants {
             LEFT_CLIMB_POSITION = new FlippablePose2d(1.45, 4.25, Rotation2d.fromDegrees(0), true),
             RIGHT_CLIMB_POSITION = new FlippablePose2d(LEFT_CLIMB_POSITION.getBlueObject().getX(), 3.28, Rotation2d.fromDegrees(0), true),
             CENTER_CLIMB_POSITION = new FlippablePose2d((LEFT_CLIMB_POSITION.getBlueObject().getX() + LEFT_CLIMB_POSITION.getBlueObject().getX()) / 2, LEFT_CLIMB_POSITION.getBlueObject().getY(), Rotation2d.fromDegrees(0), true),
-            DEPOT_POSITION = new FlippablePose2d(0.9, 6, Rotation2d.fromDegrees(180), true),
-            LEFT_INTAKE_POSITION = new FlippablePose2d(7, 7.3, Rotation2d.fromDegrees(-40), true),
+            DEPOT_POSITION = new FlippablePose2d(0.45, 7, Rotation2d.fromDegrees(-90), true),
+            LEFT_INTAKE_POSITION = new FlippablePose2d(7.4, 7.3, Rotation2d.fromDegrees(-75), true),
             RIGHT_INTAKE_POSITION = new FlippablePose2d(LEFT_INTAKE_POSITION.getBlueObject().getX(), FIELD_WIDTH_METERS - LEFT_INTAKE_POSITION.getBlueObject().getY(), Rotation2d.fromDegrees(40), true),
             LEFT_IDEAL_SHOOTING_POSITION = new FlippablePose2d(2.7, 5.8, Rotation2d.fromDegrees(0), true),
             RIGHT_IDEAL_SHOOTING_POSITION = new FlippablePose2d(LEFT_IDEAL_SHOOTING_POSITION.getBlueObject().getX(), FIELD_WIDTH_METERS - LEFT_IDEAL_SHOOTING_POSITION.getBlueObject().getY(), Rotation2d.fromDegrees(0), true),
@@ -50,7 +50,7 @@ public class FieldConstants {
             RIGHT_DELIVERY_POSITION = new FlippableTranslation2d(BLUE_RELATIVE_DELIVERY_POSITION_X, (FIELD_WIDTH_METERS / 2) - DELIVERY_POSITION_Y_OFFSET_FROM_CENTER_METERS, true),
             LEFT_DELIVERY_POSITION = new FlippableTranslation2d(BLUE_RELATIVE_DELIVERY_POSITION_X, (FIELD_WIDTH_METERS / 2) + DELIVERY_POSITION_Y_OFFSET_FROM_CENTER_METERS, true);
     public static final double
-            ALLIANCE_ZONE_LENGTH = 4,
+            ALLIANCE_ZONE_LENGTH = 4.5,
             DELIVERY_ZONE_START_BLUE_X = ALLIANCE_ZONE_LENGTH + 1;
 
     private static AprilTagFieldLayout createAprilTagFieldLayout() {
diff --git a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java
index db021ed0..277bed2e 100644
--- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java
+++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java
@@ -45,6 +45,10 @@ public class SimulatedGamePieceConstants {
             STARTING_FUEL_Y_POSITION_METERS = 1.724406,
             STARTING_FUEL_SPACING_METERS = 0.16;
 
+    private static final int
+            DEPOT_FUEL_ROWS = 6,
+            DEPOT_FUEL_COLUMNS = 4;
+
     public static final double
             EJECTION_FROM_HUB_MINIMUM_VELOCITY_METERS_PER_SECOND = 4,
             EJECTION_FROM_HUB_MAXIMUM_VELOCITY_METERS_PER_SECOND = 15;
@@ -63,6 +67,27 @@ private static void initializeFuel() {
                 );
             }
         }
+
+        for (int i = 0; i <8; i++) {
+            final SimulatedGamePiece currentHeldFuel = new SimulatedGamePiece(0, 0);
+            SimulationFieldHandler.addHeldFuel(currentHeldFuel);
+        }
+
+        initializeDepotFuel(0.31, 5.96);
+    }
+
+    private static void initializeDepotFuel(double depotCenterX, double depotCenterY) {
+        double startX = depotCenterX - (DEPOT_FUEL_COLUMNS - 1) * STARTING_FUEL_SPACING_METERS / 2.0;
+        double startY = depotCenterY - (DEPOT_FUEL_ROWS - 1) * STARTING_FUEL_SPACING_METERS / 2.0;
+
+        for (int row = 0; row < DEPOT_FUEL_ROWS; row++) {
+            for (int col = 0; col < DEPOT_FUEL_COLUMNS; col++) {
+                new SimulatedGamePiece(
+                        startX + col * STARTING_FUEL_SPACING_METERS,
+                        startY + row * STARTING_FUEL_SPACING_METERS
+                );
+            }
+        }
     }
 
     public enum GamePieceType {
diff --git a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java
index 3a82cb79..e84c825a 100644
--- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java
+++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java
@@ -43,12 +43,16 @@ private static void updateCollection() {
                 if (HELD_FUEL.size() >= SimulatedGamePieceConstants.MAXIMUM_HELD_FUEL)
                     return;
 
-                HELD_FUEL.add(fuel);
-                fuel.resetIndexing();
+                addHeldFuel(fuel);
             }
         }
     }
 
+    public static void addHeldFuel(SimulatedGamePiece fuel) {
+        HELD_FUEL.add(fuel);
+        fuel.resetIndexing();
+    }
+
     /**
      * Gets the fuel object that is being collected.
      *
diff --git a/src/main/java/frc/trigon/robot/subsystems/hood/HoodConstants.java b/src/main/java/frc/trigon/robot/subsystems/hood/HoodConstants.java
index 24083906..01cd2f0d 100644
--- a/src/main/java/frc/trigon/robot/subsystems/hood/HoodConstants.java
+++ b/src/main/java/frc/trigon/robot/subsystems/hood/HoodConstants.java
@@ -68,7 +68,7 @@ public class HoodConstants {
             null
     );
 
-    static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(0.5);
+    static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(1);
     static final Rotation2d
             REST_ANGLE = Rotation2d.fromDegrees(87),
             DELIVERY_ANGLE = Rotation2d.fromDegrees(50),
diff --git a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java
index 40e5d08e..520923d4 100644
--- a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java
+++ b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java
@@ -49,7 +49,7 @@ public class ShooterConstants {
     );
 
     static final double EJECTION_VELOCITY_METERS_PER_SECOND = 3;
-    static final double VELOCITY_TOLERANCE_METERS_PER_SECOND = 0.1;
+    static final double VELOCITY_TOLERANCE_METERS_PER_SECOND = 0.4;
     static final double WHEEL_SLIPPAGE_COMPENSATION_VELOCITY_MULTIPLIER = RobotHardwareStats.isSimulation() ? 1 : 1.05;
     static final double
             DELIVERY_VELOCITY_SLOPE = 0.9,
diff --git a/src/main/java/frc/trigon/robot/subsystems/spindexer/SpindexerConstants.java b/src/main/java/frc/trigon/robot/subsystems/spindexer/SpindexerConstants.java
index f7fa1f8c..a2adb04c 100644
--- a/src/main/java/frc/trigon/robot/subsystems/spindexer/SpindexerConstants.java
+++ b/src/main/java/frc/trigon/robot/subsystems/spindexer/SpindexerConstants.java
@@ -84,7 +84,7 @@ public class SpindexerConstants {
     }
 
     public enum SpindexerState {
-        LOAD_TO_TURRET(1),
+        LOAD_TO_TURRET(2),
         STOP(0);
 
         public final double targetVelocityRotationsPerSecond;
diff --git a/src/main/java/frc/trigon/robot/subsystems/turret/Turret.java b/src/main/java/frc/trigon/robot/subsystems/turret/Turret.java
index a8a09cd7..ba3b505c 100644
--- a/src/main/java/frc/trigon/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/trigon/robot/subsystems/turret/Turret.java
@@ -106,9 +106,24 @@ public Rotation2d getCurrentFieldRelativeAngle() {
         return getCurrentSelfRelativeAngle().plus(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation());
     }
 
+    public boolean atTargetShootingCalculationsAngle(boolean useWideTolerance) {
+        return atFieldRelativeAngle(shootingCalculations.getTargetShootingState().targetFieldRelativeYaw(), useWideTolerance);
+    }
+
     public boolean atTargetAngle(boolean useWideTolerance) {
-        return Math.abs(targetSelfRelativeAngle.minus(getCurrentSelfRelativeAngle()).getRadians())
-                < (useWideTolerance ? TurretConstants.WIDE_TOLERANCE.getRadians() : TurretConstants.NORMAL_TOLERANCE.getRadians());
+        return atSelfRelativeAngle(targetSelfRelativeAngle, useWideTolerance);
+    }
+
+    public boolean atFieldRelativeAngle(Rotation2d fieldRelativeAngle, boolean useWideTolerance) {
+        final double differenceRadians = Math.abs(fieldRelativeAngle.minus(getCurrentFieldRelativeAngle()).getRadians());
+        final double toleranceRadians = useWideTolerance ? TurretConstants.WIDE_TOLERANCE.getRadians() : TurretConstants.NORMAL_TOLERANCE.getRadians();
+        return differenceRadians < toleranceRadians;
+    }
+
+    public boolean atSelfRelativeAngle(Rotation2d selfRelativeAngle, boolean useWideTolerance) {
+        final double differenceRadians = Math.abs(selfRelativeAngle.minus(getCurrentSelfRelativeAngle()).getRadians());
+        final double toleranceRadians = useWideTolerance ? TurretConstants.WIDE_TOLERANCE.getRadians() : TurretConstants.NORMAL_TOLERANCE.getRadians();
+        return differenceRadians < toleranceRadians;
     }
 
     public Rotation2d getCurrentSelfRelativeAngle() {
diff --git a/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java
index e59e5c6f..b1fde27d 100644
--- a/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java
+++ b/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java
@@ -67,11 +67,11 @@ public class TurretConstants {
     );
 
     static final Rotation2d
-            MAXIMUM_ANGLE = Rotation2d.fromDegrees(179.5),
-            MINIMUM_ANGLE = Rotation2d.fromDegrees(-179.5),
+            MAXIMUM_ANGLE = Rotation2d.fromDegrees(370),
+            MINIMUM_ANGLE = Rotation2d.fromDegrees(-10),
             TOTAL_ANGULAR_RANGE = MAXIMUM_ANGLE.minus(MINIMUM_ANGLE);
     static final Rotation2d
-            NORMAL_TOLERANCE = Rotation2d.fromDegrees(2),
+            NORMAL_TOLERANCE = Rotation2d.fromDegrees(3),
             WIDE_TOLERANCE = Rotation2d.fromDegrees(15);
     static final double ROBOT_VELOCITY_TO_FUTURE_ANGLE_SECONDS = 0.2;
     static final double RESIST_Y_MOVEMENT_FOR_DELIVERY_COEFFICIENT = 10;
@@ -96,9 +96,9 @@ private static void configureMasterMotor() {
         config.ClosedLoopGeneral.GainSchedKpBehavior = GainSchedKpBehaviorValue.Continuous;
         config.ClosedLoopGeneral.GainSchedErrorThreshold = 0.007;
 
-        config.Slot0.kP = RobotHardwareStats.isSimulation() ? 75 : 0;
+        config.Slot0.kP = RobotHardwareStats.isSimulation() ? 270 : 0;
         config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0;
-        config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 0;
+        config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.6 : 0;
         config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.01 : 0;
         config.Slot0.kV = RobotHardwareStats.isSimulation() ? 7.5 : 0;
         config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.0005 : 0;
@@ -113,8 +113,8 @@ private static void configureMasterMotor() {
         config.Slot1.kA = 0;
         config.Slot1.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign;
 
-        config.MotionMagic.MotionMagicCruiseVelocity = RobotHardwareStats.isSimulation() ? 1 : 5;
-        config.MotionMagic.MotionMagicAcceleration = RobotHardwareStats.isSimulation() ? 70 : 5;
+        config.MotionMagic.MotionMagicCruiseVelocity = RobotHardwareStats.isSimulation() ? 1.4 : 5;
+        config.MotionMagic.MotionMagicAcceleration = RobotHardwareStats.isSimulation() ? 90 : 5;
         config.MotionMagic.MotionMagicJerk = 0;
 
         config.CurrentLimits.StatorCurrentLimitEnable = true;