Lidar Align

src/main/deploy/pathplanner/autos/Robot Reveal Night.auto

@@ -10,6 +10,12 @@
             "name": "Robot Rev Up"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Retracted"
+          }
+        },
         {
           "type": "path",
           "data": {

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

@@ -101,10 +101,10 @@ 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,
+        // RobotIntakeDown,
-        new WaitCommand(1),
+        // new WaitCommand(1),
-        new InstantCommand(() -> System.out.println("Closest Ball: " + m_lidar.getClosestBall())),
+        // new InstantCommand(() -> System.out.println("Closest Ball: " + m_lidar.getClosestBall())),
-        new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(m_lidar.getClosestBall(), new Rotation2d(0)), true)
+        new AutoAlign(m_robotSwerveDrive, m_vision, m_lidar, true)
 
         // new RunCommand(
         // () -> m_robotSwerveDrive.driveWithInput(
@@ -123,14 +123,18 @@ public class RobotContainer {
         new InstantCommand(() -> m_robotShooter.setShooterReady(), m_robotShooter)
     );
 
+    private Command IntakeRetracted = new SequentialCommandGroup(
+        new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake)
+    );
+
     private Command RobotShoot = new SequentialCommandGroup(
         // TEST NEW AUTO ALIGN
         //new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION,  new Rotation2d(0)), false),
         new InstantCommand(()-> m_robotShooter.setShooterShoot(), m_robotShooter),
         new InstantCommand(()-> m_robotShooter.setShooterReady(), m_robotShooter),
-        new WaitCommand(3),
-        new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake),
         new WaitCommand(2),
+        IntakeRetracted,
+        new WaitCommand(3),
         new InstantCommand(()->m_robotShooter.setShooterNOTShoot(), m_robotShooter),
         new InstantCommand(() -> m_robotShooter.setShooterNotReady(), m_robotShooter)
     );
@@ -155,6 +159,7 @@ public class RobotContainer {
         }, true);
 
         NamedCommands.registerCommand("Robot Rev Up", RobotReadyToShoot);
+        NamedCommands.registerCommand("Intake Retracted", IntakeRetracted);
         NamedCommands.registerCommand("Robot Shoot", RobotShoot);
         NamedCommands.registerCommand("Lidar Intake", LidarIntake);
         NamedCommands.registerCommand("Robot Intake Down", RobotIntakeDown);

src/main/java/frc4388/robot/commands/alignment/AutoAlign.java

@@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc4388.robot.constants.Constants.AutoConstants;
+import frc4388.robot.subsystems.Lidar;
 import frc4388.robot.subsystems.swerve.SwerveDrive;
 import frc4388.robot.subsystems.vision.Vision;
 import frc4388.utility.compute.FieldPositions;
@@ -14,15 +15,19 @@ import frc4388.utility.structs.Gains;
 public class AutoAlign extends Command {
 
     private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0);
-    private Pose2d targetpos;
+    private Lidar lidar;
     private boolean isLidar;
+
+    private Pose2d targetpos;
+    private double targetRotation;
+
     SwerveDrive swerveDrive;
     Vision vision;
 
-    public AutoAlign(SwerveDrive swerveDrive, Vision vision, Pose2d targetpos, boolean isLidar) {
+    public AutoAlign(SwerveDrive swerveDrive, Vision vision, Lidar lidar, boolean isLidar) {
         this.swerveDrive = swerveDrive;
         this.vision = vision;
-        this.targetpos = targetpos;
+        this.lidar = lidar;
         this.isLidar = isLidar;
         addRequirements(swerveDrive);
     }
@@ -30,6 +35,7 @@ public class AutoAlign extends Command {
     @Override
     public void initialize() {
         rotPID.initialize();
+        this.targetRotation = swerveDrive.getPose2d().getRotation().getDegrees();
         //this.targetpos = new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0));
     }
     
@@ -40,36 +46,43 @@ public class AutoAlign extends Command {
 
     @Override
     public void execute() {
-        Rotation2d desiredHeading;
+        double desiredHeading;
-        Pose2d robotPose = vision.getPose2d();
+        // Pose2d robotPose = vision.getPose2d();
-        if (robotPose == null) return;
+        targetpos = new Pose2d(lidar.getClosestBall(), new Rotation2d(0));
+        // if (robotPose == null) return;
+        if (targetpos == null) return;
+        if (targetpos.getTranslation() == null) return;
 
-        double dx = targetpos.getX() - robotPose.getX();
+
-        double dy = targetpos.getY() - robotPose.getY();
+        double dx = targetpos.getX();// - robotPose.getX();
+        double dy = targetpos.getY();// - robotPose.getY();
 
         if (!isLidar){
-            desiredHeading = new Rotation2d(Math.atan2(dy, dx)+ Math.PI);
+            desiredHeading = (Math.atan2(dy, dx)+ Math.PI) * (180. / Math.PI) + 180;
         }else{
-            desiredHeading = new Rotation2d(Math.atan2(dx, dy));
+            desiredHeading = (Math.atan2(dx, dy)) * (180. / Math.PI);// + 180;
         }
 
 
-        roterr = desiredHeading.getDegrees() - robotPose.getRotation().getDegrees();
+        targetRotation = swerveDrive.getPose2d().getRotation().getDegrees() - desiredHeading;
-        if (roterr > 180) roterr -= 360;
-        if (roterr < -180) roterr += 360;
 
-        SmartDashboard.putNumber("PID Rot Error", roterr);
+        // roterr = desiredHeading.getDegrees() - robotPose.getRotation().getDegrees();
-        System.out.println("Target: " + targetpos + "Heading: " + desiredHeading + "Error: " + roterr);
+        // if (roterr > 180) roterr -= 360;
-        swerveDrive.driveRelativeAngle(new Translation2d(0.0, 0.0), desiredHeading);
+        // if (roterr < -180) roterr += 360;
+
+        SmartDashboard.putNumber("Target Rotation!", targetRotation);
+        // System.out.println("Target: " + targetpos + "Heading: " + desiredHeading + "Error: " + roterr);
+        swerveDrive.driveRelativeAngle(new Translation2d(0.0, 0.0), Rotation2d.fromDegrees(targetRotation));
     }
 
     @Override
     public final boolean isFinished() {
-        boolean finished = Math.abs(roterr) < AutoConstants.ROT_TOLERANCE;
+        // boolean finished = Math.abs(roterr) < AutoConstants.ROT_TOLERANCE;
-        if (finished) {
+        // if (finished) {
-            swerveDrive.softStop();
+        //     swerveDrive.softStop();
-        }
+        // }
-        return finished;
+        // return finished;
+        return false;
     }
 
     private class PID {

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 = 91;
+  public static final int GIT_REVISION = 96;
-  public static final String GIT_SHA = "b3b32b00fed827f8efe77d0f6710d36222a209ac";
+  public static final String GIT_SHA = "3745cc2b1869e5850c93507277539c7cfed606c1";
-  public static final String GIT_DATE = "2026-02-25 17:17:52 MST";
+  public static final String GIT_DATE = "2026-02-25 18:59:11 MST";
   public static final String GIT_BRANCH = "AutoTesting";
-  public static final String BUILD_DATE = "2026-02-25 17:44:49 MST";
+  public static final String BUILD_DATE = "2026-02-25 20:19:51 MST";
-  public static final long BUILD_UNIX_TIME = 1772066689466L;
+  public static final long BUILD_UNIX_TIME = 1772075991136L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java

@@ -45,13 +45,13 @@ public class IntakeConstants {
 
 
     public static final Slot0Configs ARM_PID = new Slot0Configs()
-        .withKP(0.1)
+        .withKP(0.03)
         .withKI(0.0)
         .withKD(0.0);
 
     
 
-    public static ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.05);
+    public static ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.03);
     public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KI", 0);
     public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KD", 0);