From 9a159fae297f1b07b15e564560e0ec6f5117b655 Mon Sep 17 00:00:00 2001
From: Zach Wilke <83995467+C4llSqin@users.noreply.github.com>
Date: Thu, 13 Mar 2025 20:17:11 -0600
Subject: [PATCH] End of practice day, autos are completely function

---
 .../deploy/pathplanner/paths/Bottom Feed to Reef 2.path   | 4 ++--
 src/main/java/frc4388/robot/Constants.java                | 2 +-
 src/main/java/frc4388/robot/RobotContainer.java           | 8 +++++---
 src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java | 2 +-
 src/main/java/frc4388/robot/subsystems/SwerveDrive.java   | 2 +-
 5 files changed, 10 insertions(+), 8 deletions(-)

diff --git a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path
index 35326f8..7118995 100644
--- a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path	
+++ b/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
 }
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index cd388bb..546e1b4 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/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;
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 9ff2715..0e97233 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/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)
-        //     .onTrue(new LidarAlign(m_robotSwerveDrive, m_reefLidar));
+        new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
+            .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());
diff --git a/src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java b/src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java
index 029d7a6..937f02c 100644
--- a/src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java
+++ b/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
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
index 4605f6a..8ba2b72 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+++ b/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;