2025 Robot Essentials

Copied 2025 repository and removed reefscape or not reusable code.

src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java

@@ -0,0 +1,58 @@
+package frc4388.robot.subsystems.lidar;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.constants.Constants.LiDARConstants;
+import frc4388.utility.status.Status;
+import frc4388.utility.status.FaultReporter;
+import frc4388.utility.status.Queryable;
+import frc4388.utility.status.Status.ReportLevel;
+
+public class LiDAR extends SubsystemBase implements Queryable {
+    LidarIO io;
+    LidarStateAutoLogged state = new LidarStateAutoLogged();
+
+    private String name = "Lidar";
+
+    public LiDAR(LidarIO device, String name) {
+        FaultReporter.register(this);
+
+        this.io = device;
+        this.name = name;
+    }
+
+    @Override
+    public void periodic() {
+        io.updateInputs(state);
+        Logger.processInputs("LiDAR/"+name, state);
+    }
+
+    // @AutoLogOutput(key = "Lidar/{name}")
+    public double getDistance(){
+        return state.distance;
+    }
+
+    public boolean withinDistance(){
+        if(state.distance == -1) return false;
+        return state.distance < LiDARConstants.LIDAR_DETECT_DISTANCE;
+    }
+
+    @Override
+    public String getName() {
+        return "Lidar " + name;
+    }
+
+    @Override
+    public Status diagnosticStatus() {
+        Status s = new Status();
+
+        if(state.distance == -1)
+            s.addReport(ReportLevel.ERROR, "LiDAR DISCONNECTED");
+        
+        s.addReport(ReportLevel.INFO, "LiDAR Distance: " + state.distance);
+
+        return s;
+    }
+    
+}

src/main/java/frc4388/robot/subsystems/lidar/LidarIO.java

@@ -0,0 +1,13 @@
+package frc4388.robot.subsystems.lidar;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface LidarIO {
+    @AutoLog
+    public class LidarState {
+        public boolean connected;
+        public double distance;
+    }
+    
+    public default void updateInputs(LidarState state) {}
+}

src/main/java/frc4388/robot/subsystems/lidar/LidarReal.java

@@ -0,0 +1,27 @@
+package frc4388.robot.subsystems.lidar;
+
+import edu.wpi.first.wpilibj.Counter;
+import frc4388.robot.constants.Constants.LiDARConstants;
+
+// https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface
+public class LidarReal implements LidarIO {
+
+    
+    private Counter LidarPWM;
+
+    public LidarReal(int port) {
+        LidarPWM = new Counter(port);
+        LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured
+        LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement
+        LidarPWM.reset();
+    }
+
+    @Override
+    public void updateInputs(LidarState state) {
+        
+        if(LidarPWM.get() < 1)
+            state.distance = -1;
+        else
+            state.distance = (LidarPWM.getPeriod() * LiDARConstants.SECONDS_TO_MICROS) / LiDARConstants.LIDAR_MICROS_TO_CM;
+    }
+}