From 34a7ed050d2967603ebf12cd0bad0b7264aeae01 Mon Sep 17 00:00:00 2001
From: Shikhar <mj@mikovsky.com>
Date: Tue, 24 Feb 2026 17:49:26 -0700
Subject: [PATCH] Lidar and Auto testing

---
 .../pathplanner/autos/Tuesday Test.auto       | 12 ------------
 .../java/frc4388/robot/RobotContainer.java    | 19 ++++++++++++-------
 .../robot/constants/BuildConstants.java       | 10 +++++-----
 .../robot/subsystems/intake/Intake.java       |  4 ++++
 4 files changed, 21 insertions(+), 24 deletions(-)

diff --git a/src/main/deploy/pathplanner/autos/Tuesday Test.auto b/src/main/deploy/pathplanner/autos/Tuesday Test.auto
index 40d8425..0e1376a 100644
--- a/src/main/deploy/pathplanner/autos/Tuesday Test.auto	
+++ b/src/main/deploy/pathplanner/autos/Tuesday Test.auto	
@@ -4,18 +4,6 @@
     "type": "sequential",
     "data": {
       "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Quick Shoot"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "Robot Shoot"
-          }
-        },
         {
           "type": "named",
           "data": {
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 49e9ec1..20a0525 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -40,6 +40,7 @@ import frc4388.robot.subsystems.intake.Intake;
 import frc4388.robot.subsystems.intake.Intake.IntakeMode;
 import frc4388.robot.subsystems.shooter.Shooter;
 import frc4388.robot.subsystems.shooter.ShooterConstants;
+import frc4388.robot.subsystems.shooter.Shooter.ShooterMode;
 import frc4388.robot.subsystems.swerve.SwerveDrive;
 import frc4388.robot.subsystems.vision.Vision;
 import frc4388.utility.DeferredBlock;
@@ -103,24 +104,28 @@ public class RobotContainer {
     private Command LidarIntake = new SequentialCommandGroup(
         // Right now this will just go to the closest ball constantly updating - need to make it so it locks on one ball
         RobotIntakeDown,
+        new WaitCommand(1),
         new RunCommand(
-        () -> m_robotSwerveDrive.driveWithInputRotation(
+        () -> m_robotSwerveDrive.driveWithInput(
                 m_lidar.getClosestBall(),
-                m_lidar.getLatestBallAngle()
+                new Translation2d(m_lidar.getLatestBallAngle().getCos() * 0.1, 0),
+                false
             ),
             m_robotSwerveDrive
         )
-        .withTimeout(10.0)
+        .withTimeout(5.0)
         .andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive))
     );
 
     private Command RobotShoot = new SequentialCommandGroup(
+        new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted)),
         new InstantCommand(() -> m_robotShooter.setShooterReady()),
         new InstantCommand(()->m_robotIntake.setMode(IntakeMode.Idle)),
-        new WaitCommand(5),
+        new WaitCommand(3),
         new InstantCommand(()->m_robotShooter.setShooterShoot()),
-        new WaitCommand(10),
-        new InstantCommand(()->m_robotShooter.setShooterNOTShoot())
+        new WaitCommand(5),
+        new InstantCommand(()->m_robotShooter.setShooterNOTShoot()),
+        new InstantCommand(() -> m_robotShooter.setShooterNotReady())
     );
 
     // private Command RobotShoot = new SequentialCommandGroup(
@@ -365,7 +370,7 @@ public class RobotContainer {
 
         autoChooser.onChange((filename) -> {
             autoChooserUpdated = true;
-            // if (filename.equals("Taxi")) {
+            // if (filename.equals("Taxi%")) {
             //     autoCommand = new SequentialCommandGroup(
             //         new MoveForTimeCommand(m_robotSwerveDrive, 
             //             new Translation2d(0, -1), 
diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java
index 33fa8a8..30ca2ae 100644
--- a/src/main/java/frc4388/robot/constants/BuildConstants.java
+++ b/src/main/java/frc4388/robot/constants/BuildConstants.java
@@ -7,12 +7,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "2026KPopRobotHunters";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 79;
-  public static final String GIT_SHA = "ff0cff819cc7280a353d7ce86999efe16661f33b";
-  public static final String GIT_DATE = "2026-02-23 16:58:14 MST";
+  public static final int GIT_REVISION = 86;
+  public static final String GIT_SHA = "b3f3ae444324c4d339e4aa5dc59fe75bd3d56558";
+  public static final String GIT_DATE = "2026-02-23 23:28:25 MST";
   public static final String GIT_BRANCH = "AutoTesting";
-  public static final String BUILD_DATE = "2026-02-23 17:39:29 MST";
-  public static final long BUILD_UNIX_TIME = 1771893569353L;
+  public static final String BUILD_DATE = "2026-02-24 17:44:43 MST";
+  public static final long BUILD_UNIX_TIME = 1771980283653L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}
diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java
index fc05d3a..c1a90ad 100644
--- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java
+++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java
@@ -40,6 +40,10 @@ public class Intake extends SubsystemBase {
         return mode;
     }
 
+    public void rollerStop(){
+        io.setRollerOutput(state, 0);
+    }
+
 
     // public enum FieldZone {
     //     // The robot should aim at the hub