Merge branch 'reveal-night' into shikhar-op-controls

build.gradle

@@ -75,6 +75,9 @@ dependencies {
     testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
     testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
 
+    
+    implementation("com.fazecast:jSerialComm:2.4.1")
+
         def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
     annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version"
 }

src/main/deploy/pathplanner/autos/Lidar Intake.auto

@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Lidar Intake"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

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

@@ -7,7 +7,13 @@
         {
           "type": "named",
           "data": {
-            "name": "Robot Intake Down"
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Retracted"
           }
         },
         {

src/main/deploy/pathplanner/autos/Test- Robot Align Path.auto

@@ -0,0 +1,37 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Quick Shoot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Test - Robot Align"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/Quick Shoot.path

@@ -3,41 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 3.605428571428572,
+        "x": 3.6,
-        "y": 5.107378917378918
+        "y": 4.0
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.6073542083177803,
+        "x": 3.35,
-        "y": 5.107378917378918
+        "y": 4.0
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 2.2557261904761914,
+        "x": 2.5,
-        "y": 4.801630952380952
+        "y": 4.0
       },
       "prevControl": {
-        "x": 2.5688571428571443,
+        "x": 2.75,
-        "y": 5.4170952380952375
+        "y": 4.0
-      },
-      "nextControl": {
-        "x": 1.848588583599833,
-        "y": 4.00139496645156
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 2.2557261904761914,
-        "y": 4.035
-      },
-      "prevControl": {
-        "x": 2.2665238095238105,
-        "y": 4.304940476190477
       },
       "nextControl": null,
       "isLocked": false,
@@ -64,7 +48,7 @@
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 178.40885972880554
+    "rotation": 180.0
   },
   "useDefaultConstraints": false
 }

src/main/deploy/pathplanner/paths/Test - Robot Align.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.5,
+        "y": 4.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.526567580029336,
+        "y": 4.248584319077823
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.5,
+        "y": 4.7
+      },
+      "prevControl": {
+        "x": 2.5175324328959037,
+        "y": 4.450615530161258
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 0.5,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": false
+}

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

@@ -11,7 +11,6 @@ import java.io.File;
 
 import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.commands.PathPlannerAuto;
-import com.pathplanner.lib.trajectory.PathPlannerTrajectory;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -29,12 +28,13 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc4388.robot.commands.MoveForTimeCommand;
+import frc4388.robot.commands.alignment.AutoAlign;
 import frc4388.robot.constants.Constants;
 import frc4388.robot.constants.Constants.OIConstants;
 import frc4388.robot.constants.Constants.SimConstants.Mode;
 // Subsystems
 import frc4388.robot.subsystems.LED;
+import frc4388.robot.subsystems.Lidar;
 import frc4388.robot.subsystems.intake.Intake;
 import frc4388.robot.subsystems.intake.Intake.IntakeMode;
 import frc4388.robot.subsystems.shooter.Shooter;
@@ -65,6 +65,7 @@ public class RobotContainer {
     // public final DigitalInput m_armLimitSwitch = new DigitalInput(9);
 
     /* Subsystems */
+    // public final Lidar m_lidar = new Lidar();
     public final LED m_robotLED = new LED(Constants.LEDConstants.LED_SPARK_ID);
     //Testing of Colors
     public final Vision m_vision = new Vision(m_robotMap.rightCamera, m_robotMap.leftCamera);
@@ -95,43 +96,50 @@ public class RobotContainer {
 
 
     private Command RobotIntakeDown = new SequentialCommandGroup(
-        new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Extended))
+        new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Extended), m_robotIntake)
     );
-    
+
+    // 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 InstantCommand(() -> System.out.println("Closest Ball: " + m_lidar.getClosestBall())),
+    //     new AutoAlign(m_robotSwerveDrive, m_vision, m_lidar, true)
+
+    //     // new RunCommand(
+    //     // () -> m_robotSwerveDrive.driveWithInput(
+    //     //         m_lidar.getClosestBall(),
+    //     //         new Translation2d(m_lidar.getLatestBallAngle().getCos() * 0.1, 0),
+    //     //         false
+    //     //     ),
+    //     //     m_robotSwerveDrive
+    //     // )
+    //     // .withTimeout(5.0)
+    //     // .andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive))
+    // );
+
+    private Command RobotReadyToShoot = new SequentialCommandGroup(
+        new InstantCommand(()-> m_robotIntake.setMode(IntakeMode.Idle),  m_robotIntake),
+        new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter)
+    );
+
+    private Command IntakeRetracted = new SequentialCommandGroup(
+        new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake)
+    );
+
     private Command RobotShoot = new SequentialCommandGroup(
-        new InstantCommand(() -> m_robotShooter.spinUpShooting()),
+        // TEST NEW AUTO ALIGN
-        new RunCommand(
+        //new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION,  new Rotation2d(0)), false),
-                () -> {
+        new InstantCommand(()-> m_robotShooter.spinUpShooting(), m_robotShooter),
-                m_robotSwerveDrive.driveFacingPosition(
+        new InstantCommand(()-> m_robotShooter.allowShooting(), m_robotShooter),
-                    getDeadbandedDriverController().getLeft(),
+        new WaitCommand(2),
-                    FieldPositions.HUB_POSITION,
+        IntakeRetracted,
-                    ShooterConstants.AIM_LEAD_TIME.get()
+        new WaitCommand(3),
-                    );
+        new InstantCommand(()->m_robotShooter.spinUpIdle(), m_robotShooter),
-                }, m_robotSwerveDrive),
+        new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter)
-        new InstantCommand(()->m_robotIntake.setMode(IntakeMode.Idle)),
-        new WaitCommand(5),
-        new InstantCommand(()->m_robotShooter.allowShooting()),
-        new WaitCommand(10),
-        new InstantCommand(()->m_robotShooter.denyShooting())
     );
 
-    // private Command RobotShoot = new SequentialCommandGroup(
+    
-    //     new InstantCommand(() -> System.out.println(m_robotLED.getMode())),
-    //     new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.PARTY_TWINKLES), m_robotLED),
-    //     new InstantCommand(() -> System.out.println(m_robotLED.getMode())),
-    //     new WaitCommand(5),
-    //     new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_RED), m_robotLED),
-    //     new InstantCommand(() -> System.out.println(m_robotLED.getMode()))
-    // );
-
-    // private Command RobotShoot = new SequentialCommandGroup(
-    //     new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Active), m_robotShooter)
-    // );
-
-    // private Command RobotIntake = new SequentialCommandGroup(
-    //     new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Down), m_robotIntake)
-    // );
-
     public RobotContainer() {
         
         configureButtonBindings();
@@ -150,7 +158,10 @@ public class RobotContainer {
             m_robotShooter.io.updateGains();
         }, 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);
 
 
@@ -364,7 +375,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), 

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

@@ -0,0 +1,119 @@
+package frc4388.robot.commands.alignment;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+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;
+import frc4388.utility.structs.Gains;
+
+public class AutoAlign extends Command {
+
+    private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0);
+    private Lidar lidar;
+    private boolean isLidar;
+
+    private Pose2d targetpos;
+    private double targetRotation;
+
+    SwerveDrive swerveDrive;
+    Vision vision;
+
+    public AutoAlign(SwerveDrive swerveDrive, Vision vision, Lidar lidar, boolean isLidar) {
+        this.swerveDrive = swerveDrive;
+        this.vision = vision;
+        this.lidar = lidar;
+        this.isLidar = isLidar;
+        addRequirements(swerveDrive);
+    }
+
+    @Override
+    public void initialize() {
+        rotPID.initialize();
+        this.targetRotation = swerveDrive.getPose2d().getRotation().getDegrees();
+        //this.targetpos = new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0));
+    }
+    
+
+    double roterr;
+
+    double rotoutput;
+
+    @Override
+    public void execute() {
+        double desiredHeading;
+        // Pose2d robotPose = vision.getPose2d();
+        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();
+
+        if (!isLidar){
+            desiredHeading = (Math.atan2(dy, dx)+ Math.PI) * (180. / Math.PI) + 180;
+        }else{
+            desiredHeading = (Math.atan2(dx, dy)) * (180. / Math.PI);// + 180;
+        }
+
+
+        targetRotation = swerveDrive.getPose2d().getRotation().getDegrees() - desiredHeading;
+
+        // roterr = desiredHeading.getDegrees() - robotPose.getRotation().getDegrees();
+        // if (roterr > 180) roterr -= 360;
+        // 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;
+        // if (finished) {
+        //     swerveDrive.softStop();
+        // }
+        // return finished;
+        return false;
+    }
+
+    private class PID {
+        protected Gains  gains;
+        private   double output    = 0;
+
+
+        /** Creates a new PelvicInflammatoryDisease. */
+        public PID(Gains gains, double tolerance) {
+            this.gains     = gains;
+        }
+
+        // Called when the command is initially scheduled.
+        public final void initialize() {
+            output = 0;
+        }
+
+        private double prevError, cumError = 0;
+        
+        // Called every time the scheduler runs while the command is scheduled.
+        public double update(double error) {
+            cumError += error * .02; // 20 ms
+            double delta = error - prevError;
+
+            output = error * gains.kP;
+            output += cumError * gains.kI;
+            output += delta * gains.kD;
+            output += gains.kF;
+
+            return output;
+        }
+    }
+    
+}

src/main/java/frc4388/robot/constants/BuildConstants.java

@@ -5,7 +5,7 @@ package frc4388.robot.constants;
  */
 public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
-  public static final String MAVEN_NAME = "2026KPopRobotHunters";
+  public static final String MAVEN_NAME = "2026KPopRobotHunters-new";
   public static final String VERSION = "unspecified";
   public static final int GIT_REVISION = 85;
   public static final String GIT_SHA = "3d22607e5a8805f6bafa42d0e8ae41a719741842";

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

@@ -47,7 +47,7 @@ public final class Constants {
 
     public static final class AutoConstants {
         // public static final Gains XY_GAINS = new Gains(5,0.6,0.0);
-        public static final Gains XY_GAINS = new Gains(8,0,0.0);
+        public static final Gains ROT_GAINS = new Gains(8,0,0.0);
         // public static final ConfigurableDouble P_XY_GAINS = new ConfigurableDouble("P_XY_GAINS", XY_GAINS.kP);
         // public static final ConfigurableDouble I_XY_GAINS = new ConfigurableDouble("I_XY_GAINS", XY_GAINS.kI);
         // public static final ConfigurableDouble D_XY_GAINS = new ConfigurableDouble("D_XY_GAINS", XY_GAINS.kD);
@@ -61,7 +61,7 @@ public final class Constants {
         public static final Trim Y_OFFSET_TRIM =        new Trim("Y Offset Trim",        Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0);
          
         public static final double XY_TOLERANCE = 0.07; // Meters
-        public static final double ROT_TOLERANCE = 5; // Degrees
+        public static final double ROT_TOLERANCE = 10; // Degrees
 
         public static final double MIN_XY_PID_OUTPUT = 0.0;
         public static final double MIN_ROT_PID_OUTPUT = 0.0;

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

@@ -0,0 +1,313 @@
+package frc4388.robot.subsystems;
+
+import java.util.ArrayList;
+import java.util.LinkedList;
+import java.util.List;
+import java.util.Queue;
+
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.opencv.core.CvType;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.core.Scalar;
+import org.opencv.highgui.HighGui;
+import org.opencv.imgproc.Imgproc;
+
+import edu.wpi.first.cscore.OpenCvLoader;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.subsystems.RPLidarA1.PolarPoint;
+import frc4388.robot.subsystems.RPLidarA1.ScanListener;
+import frc4388.utility.configurable.ConfigurableDouble;
+import frc4388.utility.status.FaultA1M8;
+
+
+
+public class Lidar extends SubsystemBase implements ScanListener {
+    // private final Spark m_motor;
+    private final RPLidarA1 lidar;
+
+    private ConfigurableDouble speed = new ConfigurableDouble("LiDAR speed", 0.2);
+
+    static
+    {
+        // This is so libopencv_javaVERSION.so (where version is the 3-digit opencv
+        // version) gets loaded.
+        try {
+            OpenCvLoader.forceLoad();
+        }
+        catch (Exception e) {
+            e.printStackTrace();
+        }
+    }
+
+    // private static final double m_Scan = 0.1;
+    public Lidar() {
+        // Spark motor = new Spark(0);
+        // this.m_motor = motor;
+        this.lidar = new RPLidarA1();
+        this.lidar.setListener(this);
+
+
+        // Thread processThread = new Thread(this::pointLoop);
+        // processThread.setDaemon(true);
+        // processThread.setName("RPLidar-Calc");
+        // processThread.start();
+
+        FaultA1M8.addDevice(lidar, "A1M8");
+    }
+
+    public Rotation2d getLatestBallAngle() {
+        return latestBallAngleDeg;
+    }
+
+    public boolean outOfBounds(Translation2d closestBall){
+        // Make sure robot doesn't go off the earth
+        return true;
+    }
+    
+
+    @Override
+    public void periodic() {
+        this.lidar.setSpeed(speed.get());
+        SmartDashboard.putString("lidar state", this.lidar.getStatus().toString());
+    }
+
+    // Detection constriants: cluster detection
+    private static final double ANG_MAX_GAP = 3.; // Degrees
+    private static final double DIST_MAX_GAP = 0.04; // Meters
+
+    // Detection constraints: Circle detection 
+    private static final double RADIUS_X_COEFF = Units.inchesToMeters(0);
+    private static final double RADIUS_Y_COEFF = Units.inchesToMeters(0);
+    private static final double RADIUS_OFFSET = Units.inchesToMeters(3);
+    private static final double RADIUS_TOLERANCE = Units.inchesToMeters(3);
+
+    private static boolean radiusInTolerance(double x, double y, double radius) {
+        double rad_at_position = RADIUS_X_COEFF*x + RADIUS_Y_COEFF*y + RADIUS_OFFSET;
+
+        return Math.abs(rad_at_position - radius) <= RADIUS_TOLERANCE;
+    }
+
+    // Window constants
+    private static final int WIDTH = 512;
+    private static final int HEIGHT = 512;
+    private static final int POINT_RAD = 2;
+
+
+    Translation2d closestBall;
+    Translation2d closestBallPrior = null;    
+
+    @AutoLogOutput
+    public Translation2d getClosestBall() {
+        return closestBall;
+    }
+
+   
+
+    private List<Point> point_group = new ArrayList<>();
+    private double last_ang = 0;
+    private double last_dist = 0;
+    private Rotation2d latestBallAngleDeg= new Rotation2d();
+    private boolean last_color = false;
+    
+    Point LIDAR = new Point(WIDTH/2,WIDTH/2);
+
+    @Override
+    public void onScanComplete(List<PolarPoint> scan) {
+
+        System.out.println("SCAN: " + scan.size());
+
+        double scale = 0.009;
+
+        List<Translation2d> circlePoints = new ArrayList<>();
+
+        // Mat mat = Mat.zeros(WIDTH, HEIGHT, CvType.CV_8UC3);
+
+        for(PolarPoint point_polar : scan) {
+            if(!(point_polar.angle < 30 || point_polar.angle > 330)) {
+                continue;
+            }
+
+            double ang_rad = Math.toRadians(point_polar.angle); 
+            double x = point_polar.distance * Math.cos(ang_rad);
+            double y = point_polar.distance * Math.sin(ang_rad);
+
+            // Point point_xy = new Point((WIDTH/2) + (x/scale), (HEIGHT/2) + (y/scale));
+            Point point_xy = new Point(x, y);
+            
+            if(
+                Math.abs(last_ang - point_polar.angle) > ANG_MAX_GAP ||
+                Math.abs(last_dist - point_polar.distance) > DIST_MAX_GAP
+            ) {
+                last_color = !last_color;
+
+                if (
+                    point_group.size() >= 3
+                    // point_group.size() <= POINT_MAX.get()
+                ) {
+                    // Get points
+                    Point p1 = point_group.get(0);
+                    Point p2 = point_group.get(point_group.size()/2);
+                    Point p3 = point_group.get(point_group.size()-1);
+
+                    // Simplify to var
+                    double dx23 = p2.x - p3.x;
+                    double dy23 = p2.y - p3.y;
+
+                    double dx13 = p1.x - p3.x;
+                    double dy13 = p1.y - p3.y;
+
+                    double dx12 = p1.x - p2.x;
+                    double dy12 = p1.y - p2.y;
+
+                    // Calc Determinite
+                    double D = p1.x*dy23 - p1.y*dx23 + (p2.x*p3.y - p3.x*p2.y);
+
+                    // The points are in a straight line.
+                    if(D == 0) {
+                        continue;
+                    }
+
+                    // Square distances between each set of 2 points
+                    double a_sq = dx23*dx23 + dy23*dy23;
+                    double b_sq = dx13*dx13 + dy13*dy13;
+                    double c_sq = dx12*dx12 + dy12*dy12;
+
+                    // Calculate the radius
+                    double radius = Math.sqrt(a_sq*b_sq*c_sq) / (2 * Math.abs(D));
+
+                    // Square distances between each point and origin
+                    double d1 = p1.x*p1.x + p1.y*p1.y;
+                    double d2 = p2.x*p2.x + p2.y*p2.y;
+                    double d3 = p3.x*p3.x + p3.y*p3.y;
+
+                    // Calculate X and Y
+                    double cx = (d1*dy23 - d2*dy13 + d3*dy12)/(2*D);
+                    double cy = -(d1*dx23 - d2*dx13 + d3*dx12)/(2*D);
+
+
+
+                    if(radiusInTolerance(cx, cy, radius)) {
+                        circlePoints.add(new Translation2d(cx, cy));
+                    }
+
+
+                    
+
+                    // FitResult result = TaubinCircleFitter.fit(point_group, Lidar::getRadius);
+
+                    // if(result.rmsError < ERROR_BOUND.get()) {
+                    //     circlePoints.add(result.center);
+                    //     Imgproc.circle(mat, result.center, (int) (result.radius/scale), new Scalar(255,255,255));
+                    // }
+                }
+                
+                point_group.clear();
+            }
+
+            point_group.add(point_xy);
+
+            last_ang = point_polar.angle;
+            last_dist = point_polar.distance;
+
+            // Point scaledPoint = new Point((WIDTH/2) + (point_xy.x / scale), (WIDTH/2) + (point_xy.y / scale));
+            // Imgproc.circle(mat, scaledPoint, POINT_RAD, new Scalar(127,127,127));
+        }
+
+        // for(Translation2d circle : circlePoints) {
+        //     Point scaledPoint = new Point( (WIDTH/2) + (circle.getX() / scale), (WIDTH/2) + (circle.getY() / scale));
+        //     Imgproc.circle(mat, scaledPoint, (int) (RADIUS_OFFSET / scale), new Scalar(0,255,255));
+        //     // System.out.println(circle.x + " - " + circle.y);
+        // }
+        
+    
+
+        closestBall = new Translation2d();
+
+        if (circlePoints.isEmpty()) {
+            closestBall = new Translation2d(Double.NaN, Double.NaN);
+        } else {
+            double minDist = Double.POSITIVE_INFINITY;
+            Translation2d best = null;
+            for (Translation2d circle : circlePoints) {
+                double dist = circle.getSquaredNorm(); // distance from 0,0
+                if (dist < minDist) {
+                        minDist = dist;
+                        best = circle;
+                    }
+                }
+
+            closestBall = best;
+        }
+
+        if (closestBallPrior != null) {
+            if (closestBall.getDistance(closestBallPrior) < 0.1 && outOfBounds(closestBall)){
+
+                // Point scaledPoint = new Point( (WIDTH/2) + (closestBall.getX() / scale), (WIDTH/2) + (closestBall.getY() / scale));
+                // Imgproc.circle(mat, scaledPoint, (int) (RADIUS_OFFSET / scale), new Scalar(200, 25, 52), -1);
+                latestBallAngleDeg = new Rotation2d(Math.atan((closestBall.getY())/(closestBall.getX()))/Math.PI*180);
+            } else {
+                // Point scaledPoint = new Point( (WIDTH/2) + (closestBallPrior.getX() / scale), (WIDTH/2) + (closestBallPrior.getY() / scale));
+                // Imgproc.circle(mat, scaledPoint, (int) (RADIUS_OFFSET / scale), new Scalar(200, 25, 52), -1);
+            }
+        }
+
+        closestBallPrior = closestBall;
+
+
+
+        // Imgproc.circle(mat, LIDAR, (int) (RADIUS_OFFSET / scale), new Scalar(255,255,255), -1);
+        
+        // System.o
+        
+
+        // showWindow(mat);
+    }
+
+    private static void showWindow(Mat img) {
+        // Display the image in a window titled "Original Image"
+        HighGui.imshow("Original Image", img);
+
+
+        // Wait for a key press to close the window
+        HighGui.waitKey(1);
+    }
+
+    // XYZ Position of the LiDAR on the robot 
+    private static final Translation2d LiDAR_POS = new Translation2d(1, 0);
+
+    // Angle of the lidar unit 
+    private static final double LiDAR_PITCH = 0; // Radians
+    private static final double LiDAR_ROLL = 0; // Radians
+
+    // Convert a LiDAR ball position to a field position
+    public static Translation2d lidarPosToField(Translation2d p, Pose2d pose) {
+        // Project the point tilted plane on to the XY plane
+        // Point should be relative to the XY plane, with (0,0) centered at the centerpoint of the lidar
+        double x = p.getX() * Math.cos(LiDAR_ROLL) + p.getY() * Math.sin(LiDAR_PITCH) * Math.sin(LiDAR_ROLL);
+        double y = p.getY() * Math.cos(LiDAR_PITCH);
+
+        // Translate the ball position to relative to the center of the robot
+        // Point should be relative to robot, wth (0,0) centered at center of robot
+        x -= LiDAR_POS.getX();
+        y -= LiDAR_POS.getY();
+
+        // Rotate the point by the robot's rotation
+        // Point should now be relative to robot, but rotated relative to the field.
+        double ang = -pose.getRotation().getRadians();
+        x = x*Math.cos(ang) - y*Math.sin(ang);
+        y = x*Math.sin(ang) + y*Math.cos(ang);
+
+        // Translate the point to the robot's field position
+        // Point should be relative to field. (0,0) should be relative to the field.
+        x += pose.getX();
+        y += pose.getY();
+
+        return new Translation2d(x, y);
+    }
+}

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

@@ -0,0 +1,431 @@
+package frc4388.robot.subsystems;
+
+import com.fazecast.jSerialComm.SerialPort;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
+import java.io.InputStream;
+import java.io.OutputStream;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+import java.util.concurrent.atomic.AtomicReference;
+
+/**
+ * Robust RPLidar A1 / R1M8 Driver for FRC.
+ * Implements standard protocol with auto-reconnection and state monitoring.
+ */
+public class RPLidarA1 {
+
+    // --- Data Types ---
+
+    public static class PolarPoint {
+        public final double angle;    // Degrees 0-360
+        public final double distance; // Meters
+
+        public PolarPoint(double angle, double distance) {
+            this.angle = angle;
+            this.distance = distance;
+        }
+    }
+
+    @FunctionalInterface
+    public interface ScanListener {
+        void onScanComplete(List<PolarPoint> scan);
+    }
+
+    public enum ConnectionStatus {
+        DISCONNECTED,      // Port not found or closed
+        CONNECTING,        // Attempting to open serial port
+        CONNECTED_IDLE,    // Port open, but scan not started / no data yet
+        CONNECTED_DISABLED,// Robot is disabled, but sensor is connected
+        RECEIVING_DATA,    // Actively receiving valid scan points
+        ERROR              // Communication failure or timeout
+    }
+
+    // --- Protocol Constants ---
+    private static final byte SYNC_BYTE = (byte) 0xA5;
+    private static final byte SYNC_BYTE2 = (byte) 0x5A;
+    private static final byte CMD_STOP = (byte) 0x25;
+    private static final byte CMD_RESET = (byte) 0x40;
+    private static final byte CMD_SCAN = (byte) 0x20;
+    private static final byte CMD_GET_HEALTH = (byte) 0x52;
+
+    private static final int DESCRIPTOR_LEN = 7;
+    private static final int SCAN_PACKET_LEN = 5;
+
+    // --- Settings ---
+    private static final String PORT_DESC = "CP2102 USB to UART Bridge Controller";
+    private static final double WATCHDOG_TIMEOUT = 2.5; // Seconds before assuming link is dead
+
+    // --- Members ---
+    private final AtomicReference<ConnectionStatus> mStatus = new AtomicReference<>(ConnectionStatus.DISCONNECTED);
+    private SerialPort mSerialPort;
+    private InputStream mIn;
+    private OutputStream mOut;
+    private ScanListener mListener;
+    
+    private final List<PolarPoint> mCurrentScan = new ArrayList<>();
+    private double mLastDataTimestamp = 0;
+    // private boolean mScanningActive = false;
+
+    public RPLidarA1() {
+        Thread driverThread = new Thread(this::runLoop);
+        driverThread.setDaemon(true);
+        driverThread.setName("RPLidar-Driver-Thread");
+        driverThread.start();
+
+        Thread pwmThread = new Thread(this::funnyDTR_PWM);
+        pwmThread.setDaemon(true);
+        pwmThread.setName("RPLidar-Driver-PWM");
+        pwmThread.start();
+    }
+
+    /** Sets the function to call whenever a full 360-degree rotation is parsed. */
+    public void setListener(ScanListener listener) {
+        this.mListener = listener;
+    }
+
+    // Set Speed of motor between 0 - 1
+    public void setSpeed(double speed) {
+        this.motor_percentage = speed;
+    }
+
+    public ConnectionStatus getStatus() {
+        return mStatus.get();
+    }
+
+    /** Signals the Lidar to stop the motor and laser. */
+    private void stop_motor() {
+        sendCmd(CMD_RESET);
+        Timer.delay(0.02);
+        sendCmd(CMD_STOP);
+        mSerialPort.setDTR();
+    }
+
+
+    private final static double TOGGLE_DELAY = 10;
+    private double motor_percentage = 0.5;
+    private boolean is_dtr = false;
+
+    // Control the speed of the motor like a PWM through the DTR serial pin
+    // This is "PWM", like we control the speed through the percentage.
+    // The rate of toggles is the resolution
+    private void funnyDTR_PWM() {
+        while (!Thread.interrupted()) {
+            try {
+                ConnectionStatus status = mStatus.get();
+                if (status == ConnectionStatus.RECEIVING_DATA) {
+
+                    // If the motor is at full speed
+                    if (motor_percentage >= 1) {
+                        // Set the motor to on
+                        mSerialPort.clearDTR();
+                        // check again in a little bit
+                        Thread.sleep(100);
+                    }
+
+                    
+                    // If the motor is at zero speed
+                    if (motor_percentage <= 0) {
+                        // Set the motor to on
+                        mSerialPort.setDTR();
+                        // check again in a little bit
+                        Thread.sleep(100);
+                    }
+
+                    if (is_dtr) {
+                        mSerialPort.clearDTR();
+                        // Sleep for main part of motor pulse
+                        Thread.sleep((long) (TOGGLE_DELAY * motor_percentage));
+                    } else {
+                        mSerialPort.setDTR();
+                        // Sleep for gap of motor pulse
+                        Thread.sleep((long) (TOGGLE_DELAY * (1 - motor_percentage)));
+                    }
+
+                    is_dtr = !is_dtr;
+
+                } else if(status == ConnectionStatus.CONNECTED_DISABLED) {
+                    // Stop the motor
+                    mSerialPort.setDTR();
+
+                
+                    // Sleep until we can check again
+                    Thread.sleep(100);
+
+                } else { // When the motor is not ready
+                    // Sleep until we can check again
+                    Thread.sleep(100);
+                }
+            } catch (Exception e) {
+                continue;
+            }
+        }
+
+    }
+
+    private void runLoop() {
+        while (!Thread.interrupted()) {
+
+            try {
+            ConnectionStatus current = mStatus.get();
+
+            boolean robotEnabled = DriverStation.isEnabled();
+            
+
+            switch (current) {
+                case DISCONNECTED:
+                case ERROR:
+                    attemptConnection();
+                    break;
+
+                case CONNECTING:
+                    // Handled by attemptConnection
+                    break;
+                
+                case CONNECTED_DISABLED:
+                    if (robotEnabled) {
+                        mStatus.set(ConnectionStatus.CONNECTED_IDLE);
+                        // On enable, set the last data time to now to avoid watchdog error
+                        mLastDataTimestamp = Timer.getFPGATimestamp();
+                        break;
+                    }
+
+                    // We have to check the health seperately because 
+                    // the connection check only ever occurs when
+                    // the robot is recieving data
+                    if (!getHealth()) {
+                        mStatus.set(ConnectionStatus.ERROR);
+                    }
+
+                    break;
+
+                case CONNECTED_IDLE:
+                    if (!robotEnabled) {
+                        mStatus.set(ConnectionStatus.CONNECTED_DISABLED);
+                        // On enable, set the last data time to now to avoid watchdog error
+                        mLastDataTimestamp = Timer.getFPGATimestamp();
+                        break;
+                    }
+
+                    if (initiateScanMode()) {
+                        mStatus.set(ConnectionStatus.RECEIVING_DATA);
+                        mLastDataTimestamp = Timer.getFPGATimestamp();
+                    } else {
+
+                        mStatus.set(ConnectionStatus.ERROR);
+                    }
+
+                    break;
+
+                case RECEIVING_DATA:
+                    if (!robotEnabled) {
+                        mStatus.set(ConnectionStatus.CONNECTED_DISABLED);
+                        break;
+                    }
+
+                    processIncomingData();
+                    
+
+                    checkWatchdog();
+                    break;
+            }
+
+            Thread.sleep(200);
+
+            } catch (Exception e) {
+                continue;
+            }
+        }
+    }
+
+    private void attemptConnection() {
+        if (mSerialPort != null && mSerialPort.isOpen()) {
+            mSerialPort.closePort();
+        }
+
+        mStatus.set(ConnectionStatus.CONNECTING);
+        
+        SerialPort[] ports = SerialPort.getCommPorts();
+        for (SerialPort p : ports) {
+            if (p.getPortDescription().contains(PORT_DESC)) {
+                mSerialPort = p;
+                break;
+            }
+        }
+
+        if (mSerialPort != null) {
+            mSerialPort.setComPortParameters(115200, 8, SerialPort.ONE_STOP_BIT, SerialPort.NO_PARITY);
+            mSerialPort.setFlowControl(SerialPort.FLOW_CONTROL_DISABLED);
+            if (mSerialPort.openPort()) {
+                mIn = mSerialPort.getInputStream();
+                mOut = mSerialPort.getOutputStream();
+
+                if (DriverStation.isEnabled()) {
+                    mStatus.set(ConnectionStatus.CONNECTED_IDLE);
+                    // On start, set the last data time to now to avoid watchdog error
+                    mLastDataTimestamp = Timer.getFPGATimestamp();
+                } else {
+                    mStatus.set(ConnectionStatus.CONNECTED_DISABLED);
+                    stop_motor();
+                }
+
+                mStatus.set(ConnectionStatus.CONNECTED_IDLE);
+                
+                // For A1: DTR False starts motor, DTR True stops it.
+                // mSerialPort.setDTR(); 
+                return;
+            }
+        }
+        
+        mStatus.set(ConnectionStatus.DISCONNECTED);
+        Timer.delay(1.0); // Wait before retry
+    }
+
+    private boolean initiateScanMode() {
+        try {
+            // Clear buffer before starting
+            while (mIn.available() > 0) mIn.read();
+
+            mSerialPort.clearDTR(); // Start Motor
+
+            Thread.sleep(100);
+
+            sendCmd(CMD_SCAN);
+            
+            // Wait for 7-byte descriptor
+            byte[] descriptor = new byte[DESCRIPTOR_LEN];
+            long start = System.currentTimeMillis();
+            while (mIn.available() < DESCRIPTOR_LEN) {
+                if (System.currentTimeMillis() - start > 1000) return false;
+                Timer.delay(0.01);
+            }
+            
+            mIn.read(descriptor);
+
+            return descriptor[0] == SYNC_BYTE && descriptor[1] == SYNC_BYTE2;
+        } catch (Exception e) {
+            return false;
+        }
+    }
+
+    private void processIncomingData() {
+        try {
+            while (mIn.available() >= SCAN_PACKET_LEN) {
+                byte[] packet = new byte[SCAN_PACKET_LEN];
+                mIn.read(packet);
+
+                // Protocol validation based on provided Python logic
+                boolean newScan = (packet[0] & 0x1) != 0;
+                boolean invNewScan = ((packet[0] >> 1) & 0x1) != 0;
+                int checkBit = (packet[1] & 0x1);
+
+                if (newScan == invNewScan || checkBit != 1) {
+                    // Out of sync - skip one byte to try and find sync again
+                    return; 
+                }
+
+                mLastDataTimestamp = Timer.getFPGATimestamp();
+
+                // Python logic: ((raw[1] >> 1) + (raw[2] << 7)) / 64.
+                int angleRaw = ((packet[1] & 0xFF) >> 1) + ((packet[2] & 0xFF) << 7);
+                double angle = angleRaw / 64.0;
+
+                // Python logic: (raw[3] + (raw[4] << 8)) / 4. (in mm)
+                int distRaw = (packet[3] & 0xFF) + ((packet[4] & 0xFF) << 8);
+                double distanceMeters = distRaw / 4000.0;
+
+                if (newScan && !mCurrentScan.isEmpty()) {
+                    if (mListener != null) {
+                        mListener.onScanComplete(new ArrayList<>(mCurrentScan));
+                    }
+                    mCurrentScan.clear();
+                }
+
+                if (distanceMeters > 0) {
+                    mCurrentScan.add(new PolarPoint(angle, distanceMeters));
+                }
+            }
+        } catch (Exception e) {
+            mStatus.set(ConnectionStatus.ERROR);
+        }
+    }
+
+    private void checkWatchdog() {
+        if (Timer.getFPGATimestamp() - mLastDataTimestamp > WATCHDOG_TIMEOUT) {
+
+            // //
+            // stop_motor();
+
+            mStatus.set(ConnectionStatus.CONNECTED_IDLE);
+
+
+            // We have to check the health seperately because 
+            // the connection check only ever occurs when
+            // the robot is recieving data
+            // if (!getHealth()) {
+
+            //     DriverStation.reportWarning("RPLidar A1: Data timeout. Reconnecting...", false);
+            //     mStatus.set(ConnectionStatus.ERROR);
+
+            // }
+        }
+    }
+
+    private void sendCmd(byte cmd) {
+        try {
+            if (mOut != null) {
+                mOut.write(new byte[]{SYNC_BYTE, cmd});
+                mOut.flush();
+            }
+        } catch (Exception e) {
+            mStatus.set(ConnectionStatus.ERROR);
+        }
+    }
+
+    /**
+     * Queries the device health status. 
+     * @return true if the device is connected and returns a 'Good' health status, false otherwise.
+     */
+    public boolean getHealth() {
+        if (mStatus.get() == ConnectionStatus.DISCONNECTED || mOut == null || mIn == null) {
+            return false;
+        }
+
+        try {
+            // Ensure the buffer is clear before sending request
+            while (mIn.available() > 0) mIn.read();
+
+            sendCmd(CMD_GET_HEALTH);
+
+            // Read 7-byte Descriptor
+            byte[] descriptor = new byte[DESCRIPTOR_LEN];
+            long startTime = System.currentTimeMillis();
+            while (mIn.available() < DESCRIPTOR_LEN) {
+                if (System.currentTimeMillis() - startTime > 500) return false;
+                Timer.delay(0.01);
+            }
+            mIn.read(descriptor);
+
+            return true;
+
+            // // Check if descriptor is valid and data type matches HEALTH (0x06)
+            // if (descriptor[0] != SYNC_BYTE || descriptor[1] != SYNC_BYTE2 || descriptor[6] != 0x06) {
+            //     return false;
+            // }
+
+            // // Read 3-byte Health Payload
+            // byte[] healthPayload = new byte[3];
+            // while (mIn.available() < 3) {
+            //     if (System.currentTimeMillis() - startTime > 1000) return false;
+            //     Timer.delay(0.01);
+            // }
+            // mIn.read(healthPayload);
+
+            // Byte 0 is status: 0x00 = Good, 0x01 = Warning, 0x02 = Error
+            // return healthPayload[0] == 0;
+        } catch (Exception e) {
+            return false;
+        }
+    }
+}

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

@@ -1,244 +0,0 @@
-package frc4388.robot.subsystems;
-
-import static edu.wpi.first.units.Units.RotationsPerSecond;
-
-import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
-import com.ctre.phoenix6.configs.MotorOutputConfigs;
-import com.ctre.phoenix6.configs.Slot0Configs;
-import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.controls.PositionDutyCycle;
-import com.ctre.phoenix6.controls.VelocityDutyCycle;
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.NeutralModeValue;
-
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc4388.utility.configurable.ConfigurableDouble;
-
-public class TestRobot extends SubsystemBase {
-
-    // TalonFX m_intakeMotor;
-    // TalonFX m_armMotor;
-    // TalonFX m_storageMotor;
-    TalonFX m_outerShooter;
-    TalonFX m_innerShooter;
-
-    ConfigurableDouble intakeRate = new ConfigurableDouble("Intake Rate", 0);
-
-
-    ConfigurableDouble armUpPosition = new ConfigurableDouble("Arm Up Position", 0);
-    ConfigurableDouble armDownPosition = new ConfigurableDouble("Arm Down Position", 0);
-
-    ConfigurableDouble storageRate = new ConfigurableDouble("Storage Rate", 0);
-    ConfigurableDouble outerRate = new ConfigurableDouble("Outer Rate", 0);
-    ConfigurableDouble innerRate = new ConfigurableDouble("Inner Rate", 0);
-
-    VelocityDutyCycle outerVelocity = new VelocityDutyCycle(0);
-    VelocityDutyCycle innerVelocity = new VelocityDutyCycle(0);
-
-    public static final double MAX_OUTER_VELOCITY = 1200; // Rots per second
-
-    public enum RobotStare {
-        IntakeDown,
-        Idle,
-        Shoot
-    }
-
-    public RobotStare robotStare = RobotStare.Idle;
-
-
-    public TestRobot(
-        // TalonFX intakeMotor,
-        // TalonFX armMotor,
-        // TalonFX storageMotor,
-        TalonFX outerShooter,
-        TalonFX innerShooter
-    ) {
-        // m_intakeMotor = intakeMotor;
-        // m_armMotor = armMotor;
-        // m_storageMotor = storageMotor;
-        m_outerShooter = outerShooter;
-        m_innerShooter = innerShooter;
-
-        // m_intakeMotor.getConfigurator().apply(INTAKE_MOTOR_CONFIG);
-        // m_armMotor.getConfigurator().apply(ARM_MOTOR_CONFIG);
-        // m_storageMotor.getConfigurator().apply(STORAGE_MOTOR_CONFIG);
-        m_outerShooter.getConfigurator().apply(OUTER_MOTOR_CONFIG);
-        m_innerShooter.getConfigurator().apply(INNER_MOTOR_CONFIG);
-
-        m_outerShooter.getConfigurator().apply(SHOOTER_PID);
-        m_innerShooter.getConfigurator().apply(SHOOTER_PID);
-
-        outerVelocity.Slot = 0;
-        innerVelocity.Slot = 0;
-    }
-
-    // public static final TalonFXConfiguration INTAKE_MOTOR_CONFIG = new TalonFXConfiguration()
-    //     .withCurrentLimits(
-    //         new CurrentLimitsConfigs()
-    //             .withStatorCurrentLimit(40) // TODO: tune???
-    //             .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means
-    //         ).withMotorOutput(
-    //             new MotorOutputConfigs()
-    //                 .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
-    //                 .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
-    // );
-    // public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration()
-    //     .withCurrentLimits(
-    //         new CurrentLimitsConfigs()
-    //             .withStatorCurrentLimit(40) // TODO: tune???
-    //             .withStatorCurrentLimitEnable(true)
-    //         ).withMotorOutput(
-    //             new MotorOutputConfigs()
-    //                 .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate
-    //                 .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
-    // );
-
-    // public static final TalonFXConfiguration STORAGE_MOTOR_CONFIG = new TalonFXConfiguration()
-    //     .withCurrentLimits(
-    //         new CurrentLimitsConfigs()
-    //             .withStatorCurrentLimit(40) // TODO: tune???
-    //             .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means
-    //         ).withMotorOutput(
-    //             new MotorOutputConfigs()
-    //                 .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
-    //                 .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
-    // );
-    public static final TalonFXConfiguration OUTER_MOTOR_CONFIG = new TalonFXConfiguration()
-        .withCurrentLimits(
-            new CurrentLimitsConfigs()
-                .withStatorCurrentLimit(40) // TODO: tune???
-                .withStatorCurrentLimitEnable(true)
-            ).withMotorOutput(
-                new MotorOutputConfigs()
-                    .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
-                    .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
-    );
-    public static final TalonFXConfiguration INNER_MOTOR_CONFIG = new TalonFXConfiguration()
-        .withCurrentLimits(
-            new CurrentLimitsConfigs()
-                .withStatorCurrentLimit(40) // TODO: tune???
-                .withStatorCurrentLimitEnable(true)
-            ).withMotorOutput(
-                new MotorOutputConfigs()
-                    .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
-                    .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
-                    
-    );
-
-    public static Slot0Configs SHOOTER_PID;
-
-    static {
-        SHOOTER_PID = new Slot0Configs();
-        SHOOTER_PID.kV = 0.12;
-        // SHOOTER_PID.kP = 0.11;
-        // SHOOTER_PID.kI = 0.48;
-        // SHOOTER_PID.kD = 0.01;
-        SHOOTER_PID.kP = 0.3;
-        SHOOTER_PID.kI = 0.0;
-        SHOOTER_PID.kD = 0.0;
-    }
-
-    
-    ConfigurableDouble kV = new ConfigurableDouble("kV", 0.12);
-    ConfigurableDouble kP = new ConfigurableDouble("kP", 0.11);
-    ConfigurableDouble kI = new ConfigurableDouble("kI", 0.48);
-    ConfigurableDouble kD = new ConfigurableDouble("kD", 0.01);
-
-    @Override
-    public void periodic() {
-        
-        SHOOTER_PID = new Slot0Configs();
-        SHOOTER_PID.kV = kV.get();
-        SHOOTER_PID.kP = kP.get();
-        SHOOTER_PID.kI = kI.get();
-        SHOOTER_PID.kD = kD.get();
-
-        m_outerShooter.getConfigurator().apply(SHOOTER_PID);
-        m_innerShooter.getConfigurator().apply(SHOOTER_PID);
-
-        
-        SmartDashboard.putNumber("Outer Velocity", m_outerShooter.getVelocity().getValue().in(RotationsPerSecond));
-        SmartDashboard.putNumber("Inner Velocity", m_innerShooter.getVelocity().getValue().in(RotationsPerSecond));
-
-
-        switch (robotStare) {
-            case Idle:
-            
-                // Move the arm to the up position
-                PositionDutyCycle armPosition = new PositionDutyCycle(armUpPosition.get());
-                // m_armMotor.setControl(armPosition);
-            
-
-                // // Stop the intake motor
-                // m_intakeMotor.set(0);
-
-                // // Stop the storage motor
-                // m_storageMotor.set(0);
-
-                
-                // Stop the outer shooter motor
-                m_outerShooter.set(0);
-                // Stop the inner shooter motor
-                m_innerShooter.set(0);
-
-                break;
-            case IntakeDown:
-                // Move the arm to the down sposition
-                PositionDutyCycle armPosition1 = new PositionDutyCycle(armDownPosition.get());
-                // m_armMotor.setControl(armPosition1);
-
-                // Move balls into the robot because the arm is down
-                VelocityDutyCycle intakeVelocity = new VelocityDutyCycle(intakeRate.get());
-                // m_intakeMotor.setControl(intakeVelocity);
-
-                // Move balls into the main box
-                VelocityDutyCycle storageVelocity = new VelocityDutyCycle(storageRate.get());
-                // m_storageMotor.setControl(storageVelocity);
-
-                
-                // Stop the outer shooter motor
-                m_outerShooter.set(0);
-                // Stop the inner shooter motor
-                m_innerShooter.set(0);
-
-                break;
-            case Shoot:
-
-                // Move the arm to the up position
-                PositionDutyCycle armPosition2 = new PositionDutyCycle(armUpPosition.get());
-                // m_armMotor.setControl(armPosition2);
-
-                // // Stop the intake motor
-                // m_intakeMotor.set(0);
-
-                // // Move the balls into the 
-                VelocityDutyCycle storageVelocity1 = new VelocityDutyCycle(-storageRate.get());
-                // m_storageMotor.setControl(storageVelocity1);
-                        
-                // outerVelocity.
-                // m_outerShooter.setControl(outerVelocity);
-                double outerRPM = outerRate.get();
-                m_outerShooter.setControl(outerVelocity.withVelocity(outerRPM));//.withFeedForward(outerRPM/MAX_OUTER_VELOCITY));
-
-
-                
-                // m_innerShooter.setControl(innerVelocity);
-                // m_innerShooter.set(innerRate.get());
-
-                double innerRPM = innerRate.get();
-                m_innerShooter.setControl(innerVelocity.withVelocity(innerRPM));//.withFeedForward(outerRPM/MAX_OUTER_VELOCITY));
-
-                
-                // SmartDashboard.putNumber("Test", outerRate.get());
-
-                break;
-            default:
-                break;
-        }
-
-
-        
-    }
-    
-}

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);
     

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

@@ -411,20 +411,13 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
     }
 
     public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) {
-        // if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the
+    
-        // swerve drive is still going:
-        // stopModules(); // stop the swerve
-
-        // if (leftStick.getNorm() < 0.05) //if no imput
-        // return; // don't bother doing swerve drive math and return early.
-
         leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
 
         io.setControl(new SwerveRequest.FieldCentricFacingAngle()
                 .withVelocityX(leftStick.getX() * -speedAdjust)
                 .withVelocityY(leftStick.getY() * speedAdjust)
                 .withTargetDirection(rot));
-        // double
     }
 
     public double getGyroAngle() {

src/main/java/frc4388/utility/status/FaultA1M8.java

@@ -0,0 +1,39 @@
+package frc4388.utility.status;
+
+import frc4388.robot.subsystems.RPLidarA1;
+import frc4388.robot.subsystems.RPLidarA1.ConnectionStatus;
+import frc4388.utility.status.Status.ReportLevel;
+
+// Fault reporter for the RPLidar A1M8 Revolving lidar sensor
+public class FaultA1M8 implements Queryable {
+    private String name;
+    private RPLidarA1 cam;
+
+    public static void addDevice(RPLidarA1 cam, String name) {
+        FaultA1M8 p = new FaultA1M8();
+
+        p.name = name;
+        p.cam = cam;
+
+        FaultReporter.register(p);
+    }
+
+    @Override
+    public String getName() {
+        return name;
+    }
+
+    @Override
+    public Status diagnosticStatus() {
+        Status s = new Status();
+
+        ConnectionStatus cam_ConnectionStatus = cam.getStatus();
+
+        if(cam_ConnectionStatus != ConnectionStatus.RECEIVING_DATA)
+            s.addReport(ReportLevel.ERROR, "Not Connected! Status = " + cam_ConnectionStatus);
+
+        s.addReport(ReportLevel.INFO, cam.getStatus().toString());
+
+        return s;
+    }
+}