End of practice day, autos are completely function

src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path

@@ -20,7 +20,7 @@
         "y": 2.3675
       },
       "prevControl": {
-        "x": 2.5831884107032717,
+        "x": 2.583188410703272,
         "y": 1.782750079374932
       },
       "nextControl": null,
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": 53.426969021480645
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

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

@@ -290,7 +290,7 @@ public final class Constants {
         public static final int REEF_LIDAR_DIO_CHANNEL = 7;
         public static final int REVERSE_LIDAR_DIO_CHANNEL = 4;
 
-        public static final int HUMAN_PLAYER_STATION_DISTANCE = 25;
+        public static final int HUMAN_PLAYER_STATION_DISTANCE = 40;
 
         public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole
         public static final int LIDAR_MICROS_TO_CM = 10;

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

@@ -398,7 +398,8 @@ public class RobotContainer {
         NamedCommands.registerCommand("await-coral", new waitFeedCoral(m_robotElevator));
 
         NamedCommands.registerCommand("feed-driveback", new DriveUntilLiDAR(m_robotSwerveDrive, 
-        new Translation2d(0,1), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, false));
+        new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true));
+        // NamedCommands.registerCommand("feed-driveback", Commands.none());
         NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.lastOdomSpeed < AutoConstants.STOP_VELOCITY));
 
         NamedCommands.registerCommand("align-feed", new SequentialCommandGroup(
@@ -537,8 +538,9 @@ public class RobotContainer {
                 ), m_robotSwerveDrive))
             .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
         
-        // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
+        new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
-        //     .onTrue(new LidarAlign(m_robotSwerveDrive, m_reefLidar));
+            .onTrue(new DriveUntilLiDAR(m_robotSwerveDrive, 
+            new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true));
 
         new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
             .onTrue(thrustIntake.asProxy());

src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java

@@ -40,7 +40,7 @@ public class DriveUntilLiDAR extends Command {
 
     @Override
     public void execute() {
-        swerveDrive.driveFine(leftStick, rightStick, 0.1);
+        swerveDrive.driveFine(leftStick, rightStick, 0.3);
     }
 
     @Override

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -341,7 +341,7 @@ public class SwerveDrive extends Subsystem {
         vision.setLastOdomPose(curpose);
         setLastOdomSpeed(curpose, lastPose, freq);
 
-        if (vision.isTag() || true) {
+        if (vision.isTag()) {
             Pose2d pose = vision.getPose2d();
             if (!robotKnowsWhereItIs) {
                 robotKnowsWhereItIs = true;