From bd91fc5141e96afae2c907b822959b29a5a7bb5b Mon Sep 17 00:00:00 2001 From: Michael Mikovsky Date: Mon, 23 Feb 2026 17:50:20 -0800 Subject: [PATCH] Auto Test --- build.gradle | 3 + .../java/frc4388/robot/RobotContainer.java | 17 +- .../robot/constants/BuildConstants.java | 12 +- .../java/frc4388/robot/subsystems/Lidar.java | 309 +++++++++++++ .../frc4388/robot/subsystems/RPLidarA1.java | 431 ++++++++++++++++++ .../frc4388/utility/status/FaultA1M8.java | 39 ++ 6 files changed, 804 insertions(+), 7 deletions(-) create mode 100644 src/main/java/frc4388/robot/subsystems/Lidar.java create mode 100644 src/main/java/frc4388/robot/subsystems/RPLidarA1.java create mode 100644 src/main/java/frc4388/utility/status/FaultA1M8.java diff --git a/build.gradle b/build.gradle index bdf2b90..489a591 100644 --- a/build.gradle +++ b/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" } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c65108e..186b168 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index ebec1b4..33fa8a8 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 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(){} diff --git a/src/main/java/frc4388/robot/subsystems/Lidar.java b/src/main/java/frc4388/robot/subsystems/Lidar.java new file mode 100644 index 0000000..d6685d9 --- /dev/null +++ b/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_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 scan) { + + System.out.println("SCAN: " + scan.size()); + + double scale = 0.006; + + List 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); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/RPLidarA1.java b/src/main/java/frc4388/robot/subsystems/RPLidarA1.java new file mode 100644 index 0000000..fa5aa32 --- /dev/null +++ b/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 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 mStatus = new AtomicReference<>(ConnectionStatus.DISCONNECTED); + private SerialPort mSerialPort; + private InputStream mIn; + private OutputStream mOut; + private ScanListener mListener; + + private final List 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; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/utility/status/FaultA1M8.java b/src/main/java/frc4388/utility/status/FaultA1M8.java new file mode 100644 index 0000000..aedb866 --- /dev/null +++ b/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; + } +}