From 88508ef9c41dcd12b2626bf1a9adbf98db21eeea Mon Sep 17 00:00:00 2001
From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com>
Date: Sat, 18 Jan 2025 15:08:23 -0700
Subject: [PATCH] Add Lidar subsystem

---
 src/main/java/frc4388/robot/Constants.java    |  4 +
 .../java/frc4388/robot/RobotContainer.java    |  3 +-
 .../java/frc4388/robot/subsystems/Lidar.java  | 80 +++++++++++++++++++
 .../frc4388/robot/subsystems/SwerveDrive.java |  1 +
 4 files changed, 87 insertions(+), 1 deletion(-)
 create mode 100644 src/main/java/frc4388/robot/subsystems/Lidar.java

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 643bdfb..cd7cb9c 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -235,6 +235,10 @@ public final class Constants {
 
             public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.05, 0);
 
+            public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole
+            public static final int LIDAR_DIO_CHANNEL = 0;
+            public static final int LIDAR_MICROS_TO_CM = 10;
+            public static final int SECONDS_TO_MICROS = 1000000;
                     
             public static final double XY_TOLERANCE = 0.05;
             public static final double ROT_TOLERANCE = 1;
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 00689c6..a51c2af 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -39,6 +39,7 @@ import com.pathplanner.lib.commands.PathPlannerAuto;
 // Subsystems
 // import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.Vision;
+import frc4388.robot.subsystems.Lidar;
 import frc4388.robot.subsystems.SwerveDrive;
 
 // Utilites
@@ -62,7 +63,7 @@ public class RobotContainer {
     /* Subsystems */
     // private final LED m_robotLED = new LED();
     public final Vision m_vision = new Vision(m_robotMap.camera);
-
+    public final Lidar m_lidar = new Lidar();
     public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
     // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
 
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..130efc3
--- /dev/null
+++ b/src/main/java/frc4388/robot/subsystems/Lidar.java
@@ -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;
+    }
+    
+}
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
index 17cf81e..f0410e9 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
@@ -130,6 +130,7 @@ public class SwerveDrive extends Subsystem {
             return; // don't bother doing swerve drive math and return early.
 
         leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
+		
         if (SwerveDriveConstants.INVERT_X) leftStick = new Translation2d(-leftStick.getX(), leftStick.getY());
         if (SwerveDriveConstants.INVERT_Y) leftStick = new Translation2d(leftStick.getX(), -leftStick.getY());
         if (SwerveDriveConstants.INVERT_ROTATION) rightStick.times(-1);