From 1e4e53a7a6d3556b814f016be45d0051b7ea00fc Mon Sep 17 00:00:00 2001
From: sabellad <s-donofrios@bsd405.org>
Date: Mon, 27 Jan 2025 19:12:30 -0800
Subject: [PATCH] added branch sensors (#15)

* added branch sensors

* created a generic get distance method for sensors, called it in get left and right sensor distance, should be ready to merge to main

* added branch sensors to Sensors file

* made a method only called in branchsensors private

---------

Co-authored-by: braelynandthefrogs <97708418+braelynandthefrogs@users.noreply.github.com>
Co-authored-by: braelynandthefrogs <siennafox@gmail.com>
---
 src/main/java/frc/robot/Hardware.java         |  6 ++-
 src/main/java/frc/robot/Sensors.java          |  8 ++++
 .../java/frc/robot/sensors/BranchSensors.java | 48 +++++++++++++++++++
 3 files changed, 61 insertions(+), 1 deletion(-)
 create mode 100644 src/main/java/frc/robot/sensors/BranchSensors.java

diff --git a/src/main/java/frc/robot/Hardware.java b/src/main/java/frc/robot/Hardware.java
index 8e44e5c..9c6840f 100644
--- a/src/main/java/frc/robot/Hardware.java
+++ b/src/main/java/frc/robot/Hardware.java
@@ -5,10 +5,14 @@ public class Hardware {
 
   // Swerve: 1-12
 
-  // elevator [20-29]
+  // elevator [20-24]
   public static final int ELEVATOR_MOTOR_ONE = 20;
   public static final int ELEVATOR_MOTOR_TWO = 21;
 
+  // branch sensors [25-29] (mounted on elevator)
+  public static final int BRANCH_SENSOR_LEFT = 25;
+  public static final int BRANCH_SENSOR_RIGHT = 26;
+
   // arm pivot [30-34]
   public static final int ARM_PIVOT_MOTOR_ID = 30;
 
diff --git a/src/main/java/frc/robot/Sensors.java b/src/main/java/frc/robot/Sensors.java
index 9936354..f6d7850 100644
--- a/src/main/java/frc/robot/Sensors.java
+++ b/src/main/java/frc/robot/Sensors.java
@@ -3,15 +3,18 @@
 import static frc.robot.Sensors.SensorConstants.*;
 
 import frc.robot.sensors.ArmSensor;
+import frc.robot.sensors.BranchSensors;
 
 public class Sensors {
   public static class SensorConstants {
     // <SENSOR>_ENABLED constants go here
     public static final boolean ARMSENSOR_ENABLED = true;
+    public static final boolean BRANCHSENSORS_ENABLED = true;
   }
 
   // Sensors go here
   public final ArmSensor armSensor;
+  public final BranchSensors branchSensors;
 
   public Sensors() {
     // Initialize subsystems here (don't forget to check if they're enabled!)
@@ -21,5 +24,10 @@ public Sensors() {
     } else {
       armSensor = null;
     }
+    if (BRANCHSENSORS_ENABLED) {
+      branchSensors = new BranchSensors();
+    } else {
+      branchSensors = null;
+    }
   }
 }
diff --git a/src/main/java/frc/robot/sensors/BranchSensors.java b/src/main/java/frc/robot/sensors/BranchSensors.java
new file mode 100644
index 0000000..0f19adc
--- /dev/null
+++ b/src/main/java/frc/robot/sensors/BranchSensors.java
@@ -0,0 +1,48 @@
+package frc.robot.sensors;
+
+import static edu.wpi.first.units.Units.Millimeter;
+
+import au.grapplerobotics.ConfigurationFailedException;
+import au.grapplerobotics.LaserCan;
+import edu.wpi.first.units.measure.Distance;
+import frc.robot.Hardware;
+
+public class BranchSensors {
+
+  private final LaserCan leftSensor;
+  private final LaserCan rightSensor;
+
+  public BranchSensors() {
+    leftSensor = new LaserCan(Hardware.BRANCH_SENSOR_LEFT);
+    rightSensor = new LaserCan(Hardware.BRANCH_SENSOR_RIGHT);
+    ConfigureSensor(leftSensor);
+    ConfigureSensor(rightSensor);
+  }
+
+  private void ConfigureSensor(LaserCan Sensor) {
+    try {
+      Sensor.setRangingMode(LaserCan.RangingMode.SHORT);
+      Sensor.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16));
+      Sensor.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS);
+    } catch (ConfigurationFailedException e) {
+      System.out.println("Configuration Failed! " + e);
+    }
+  }
+
+  private Distance getSensorDistance(LaserCan sensor) {
+    LaserCan.Measurement measurement = sensor.getMeasurement();
+    if (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) {
+      return Millimeter.of(measurement.distance_mm);
+    } else {
+      return Millimeter.of(10000);
+    }
+  }
+
+  public Distance getLeftSensorDistance() {
+    return getSensorDistance(leftSensor);
+  }
+
+  public Distance getRightSensorDistance() {
+    return getSensorDistance(rightSensor);
+  }
+}