Auto Test

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

@@ -35,6 +35,7 @@ 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 +66,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);
@@ -97,7 +99,19 @@ public class RobotContainer {
     private Command RobotIntakeDown = new SequentialCommandGroup(
         new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Extended))
     );
-    
+
+    private Command LidarIntake = new SequentialCommandGroup(
+        new RunCommand(
+        () -> m_robotSwerveDrive.driveWithInputRotation(
+                m_lidar.getClosestBall(),
+                m_lidar.getLatestBallAngle()
+            ),
+            m_robotSwerveDrive
+        )
+        .withTimeout(10.0)
+        .andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive))
+    );
+
     private Command RobotShoot = new SequentialCommandGroup(
         new InstantCommand(() -> m_robotShooter.setShooterReady()),
         new InstantCommand(()->m_robotIntake.setMode(IntakeMode.Idle)),
@@ -143,6 +157,7 @@ public class RobotContainer {
         }, true);
 
         NamedCommands.registerCommand("Robot Shoot", RobotShoot);
+        NamedCommands.registerCommand("Lidar Intake", LidarIntake);
         NamedCommands.registerCommand("Robot Intake Down", RobotIntakeDown);
 
 

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 = 75;
-  public static final String GIT_SHA = "4907e0c8a0fc7dade91b2075d70a2b38213f9cab";
-  public static final String GIT_DATE = "2026-02-21 15:08:32 MST";
-  public static final String GIT_BRANCH = "operator-controls";
-  public static final String BUILD_DATE = "2026-02-21 15:52:02 MST";
-  public static final long BUILD_UNIX_TIME = 1771714322479L;
+  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 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 int DIRTY = 1;
 
   private BuildConstants(){}

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

@@ -0,0 +1,309 @@
+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.7);
+
+    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;
+    }
+    
+
+    @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(0.8);
+
+    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.006;
+
+        List<Translation2d> circlePoints = new ArrayList<>();
+
+        Mat mat = Mat.zeros(WIDTH, HEIGHT, CvType.CV_8UC3);
+
+        for(PolarPoint point_polar : scan) {
+            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){
+
+                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);
+
+                // System.out.println("Drive "+ Units.metersToInches(closestBall.x) + " inches forward and " + Units.metersToInches(closestBall.y) + "inches to the right");
+                latestBallAngleDeg = new Rotation2d(Math.atan((closestBall.getY())/(closestBall.getX()))/Math.PI*180);
+                System.out.println("!!" + latestBallAngleDeg);
+
+            } else {
+                Point scaledPoint = new Point( (WIDTH/2) + (closestBallPrior.getX() / scale), (WIDTH/2) + (closestBallPrior.getX() / 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/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;
+    }
+}