Add Lidar subsystem

2026-06-08 16:28:04 -06:00 · 2025-01-18 15:08:23 -07:00
parent e4ed00b401
commit 88508ef9c4
4 changed files with 87 additions and 1 deletions
@@ -0,0 +1,80 @@
+package frc4388.robot.subsystems;
+
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.wpilibj.PWM;
+import edu.wpi.first.wpilibj.Counter;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
+import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
+import frc4388.utility.Status;
+import frc4388.utility.Subsystem;
+import frc4388.utility.Status.ReportLevel;
+
+// https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface
+public class Lidar extends Subsystem {
+
+    private double distance = -1;
+    Counter LidarPWM = new Counter(AutoConstants.LIDAR_DIO_CHANNEL);
+
+    public Lidar() {
+        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 periodic() {
+        if(LidarPWM.get() < 1)
+            distance = -1;
+        else
+            distance = (LidarPWM.getPeriod() * AutoConstants.SECONDS_TO_MICROS) / AutoConstants.LIDAR_MICROS_TO_CM;
+    }
+
+    public double getDistance(){
+        return distance;
+    }
+
+    public boolean withinDistance(){
+        if(distance == -1) return false;
+        return distance < AutoConstants.LIDAR_DETECT_DISTANCE;
+    }
+
+    ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
+            .getLayout(getSubsystemName(), BuiltInLayouts.kList)
+            .withSize(2, 2);
+
+    GenericEntry sbDistance = subsystemLayout
+            .add("Distance", 0)
+            .withWidget(BuiltInWidgets.kGraph)
+            .getEntry();
+
+    GenericEntry sbWithinDistance = subsystemLayout
+            .add("Within Distance", 0)
+            .withWidget(BuiltInWidgets.kBooleanBox)
+            .getEntry();
+
+    @Override
+    public String getSubsystemName() {
+        return "Lidar";
+    }
+
+    @Override
+    public void queryStatus() {
+        sbDistance.setDouble(distance);
+        sbWithinDistance.setBoolean(withinDistance());
+    }
+
+    @Override
+    public Status diagnosticStatus() {
+        Status s = new Status();
+        if(distance == -1){
+            s.addReport(ReportLevel.ERROR, "LIDAR DISCONNECTED");
+        }else{
+            s.addReport(ReportLevel.ERROR, "LIDAR CONNECTED");
+        }
+        return s;
+    }
+    
+}