Add swerve to starting auto position command

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -11,6 +11,7 @@
 import com.stuypulse.robot.commands.swerve.SwerveDriveDrive;
 import com.stuypulse.robot.commands.swerve.SwerveDriveDriveToScore;
 import com.stuypulse.robot.commands.swerve.SwerveDriveResetHeading;
+import com.stuypulse.robot.commands.swerve.SwerveDriveToAutoStart;
 import com.stuypulse.robot.commands.swerve.SwerveDriveToPose;
 import com.stuypulse.robot.commands.swerve.SwerveDriveToScore;
 import com.stuypulse.robot.commands.swerve.SwerveDriveWithAiming;
@@ -101,6 +102,9 @@ private void configureButtonBindings() {
         driver.getLeftButton().whileTrue(new SwerveDriveToScore());
         driver.getRightButton().whileTrue(new SwerveDriveDriveToScore(driver));
         // driver.getBottomButton().whileTrue(AutoBuilder.pathfindToPose(new Pose2d(), new PathConstraints(3, 4, Units.degreesToRadians(540), Units.degreesToRadians(720)), 0, 0));
+
+        driver.getStartButton()
+            .whileTrue(new SwerveDriveToAutoStart(() -> autonChooser.getSelected().getName()));
     }
 
     /**************/

src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToAutoStart.java

@@ -0,0 +1,92 @@
+/************************ PROJECT JON *************************/
+/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved.*/
+/* This work is licensed under the terms of the MIT license.  */
+/**************************************************************/
+
+package com.stuypulse.robot.commands.swerve;
+
+import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController;
+import com.stuypulse.stuylib.control.feedback.PIDController;
+import com.stuypulse.stuylib.streams.booleans.BStream;
+import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC;
+
+import static com.stuypulse.robot.constants.Settings.Alignment.*;
+
+import java.util.function.Supplier;
+
+import com.pathplanner.lib.commands.PathPlannerAuto;
+import com.stuypulse.robot.subsystems.odometry.AbstractOdometry;
+import com.stuypulse.robot.subsystems.odometry.Odometry;
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
+import com.stuypulse.robot.util.HolonomicController;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+
+
+public class SwerveDriveToAutoStart extends Command {
+    private final SwerveDrive swerve;
+    private Pose2d targetPose;
+
+    // Holonomic control
+    private final HolonomicController controller;
+    private final BStream aligned;
+
+    private final FieldObject2d targetPose2d;
+
+    private final Supplier<String> name;
+
+    public SwerveDriveToAutoStart(Supplier<String> name) {
+        this.swerve = SwerveDrive.getInstance();
+        this.name = name;
+
+        controller = new HolonomicController(
+            new PIDController(Translation.P,Translation.I,Translation.D),
+            new PIDController(Translation.P, Translation.I, Translation.D),
+            new AnglePIDController(Rotation.P, Rotation.I, Rotation.D));
+
+        SmartDashboard.putData("Alignment/Controller", controller);
+
+        aligned = BStream.create(this::isAligned).filtered(new BDebounceRC.Rising(DEBOUNCE_TIME));
+
+        targetPose2d = AbstractOdometry.getInstance().getField().getObject("Target Pose");
+        addRequirements(swerve);
+    }
+
+    private boolean isAligned() {
+        return controller.isDone(X_TOLERANCE.get(), Y_TOLERANCE.get(), ANGLE_TOLERANCE.get());
+    }
+
+    @Override
+    public void initialize() {
+        try {
+            targetPose = PathPlannerAuto.getStaringPoseFromAutoFile(name.get());
+        } catch (Exception e) {
+            targetPose = Odometry.getInstance().getPose();
+        }
+    }
+
+    @Override
+    public void execute() {
+        Pose2d currentPose = AbstractOdometry.getInstance().getPose();
+
+        targetPose2d.setPose(targetPose);
+
+        controller.update(targetPose, currentPose);
+        swerve.setChassisSpeeds(controller.getOutput());
+    }
+
+    @Override
+    public boolean isFinished(){
+        return aligned.get();
+    }
+
+    public void end(boolean interupted) {
+        swerve.stop();
+        targetPose2d.setPose(Double.NaN, Double.NaN, new Rotation2d(Double.NaN));
+    }
+
+}

src/main/java/com/stuypulse/robot/constants/Settings.java

@@ -158,9 +158,9 @@ public interface Intake {
     public interface Alignment {
 
         SmartNumber DEBOUNCE_TIME = new SmartNumber("Alignment/Debounce Time", 0.15);
-        SmartNumber X_TOLERANCE = new SmartNumber("Alignment/X Tolerance", 0.1);
-        SmartNumber Y_TOLERANCE = new SmartNumber("Alignment/Y Tolerance", 0.1);
-        SmartNumber ANGLE_TOLERANCE = new SmartNumber("Alignment/Angle Tolerance", 5);
+        SmartNumber X_TOLERANCE = new SmartNumber("Alignment/X Tolerance", 0.05);
+        SmartNumber Y_TOLERANCE = new SmartNumber("Alignment/Y Tolerance", 0.05);
+        SmartNumber ANGLE_TOLERANCE = new SmartNumber("Alignment/Angle Tolerance", 3);
 
         SmartNumber TARGET_DISTANCE_IN = new SmartNumber("Alignment/Target Distance (in)", 110);
         SmartNumber TAKEOVER_DISTANCE_IN = new SmartNumber("Alignment/Takeover Distance (in)", 50);