From 58f2694b4582bf24a995bfcab4f7729ea715953f Mon Sep 17 00:00:00 2001
From: Mihir Patankar <mpatankar06@gmail.com>
Date: Sat, 13 Apr 2024 17:43:04 -0400
Subject: [PATCH] Add (experimental) multithreading of sensor reading

---
 .../frc/robot/feeder/FeederIOTalonFX.java     | 22 +++++++---
 .../java/frc/robot/feeder/SensorThread.java   | 40 +++++++++++++++++++
 2 files changed, 57 insertions(+), 5 deletions(-)
 create mode 100644 src/main/java/frc/robot/feeder/SensorThread.java

diff --git a/src/main/java/frc/robot/feeder/FeederIOTalonFX.java b/src/main/java/frc/robot/feeder/FeederIOTalonFX.java
index e19d0a2..9456be3 100644
--- a/src/main/java/frc/robot/feeder/FeederIOTalonFX.java
+++ b/src/main/java/frc/robot/feeder/FeederIOTalonFX.java
@@ -11,9 +11,11 @@
 import java.time.Duration;
 
 public class FeederIOTalonFX implements FeederIO {
+  private final boolean kUseHighFrequencySensorPolling = true;
   private final TalonFX m_feederMotor;
-  private final DigitalInput m_topNoteSensor;
+  private final DigitalInput m_noteSensor;
   private final DigitalGlitchFilter m_glitchFilter;
+  private final SensorThread m_sensorThread;
 
   private final StatusSignal<Double> m_rollerPositionSignal,
       m_rollerVelocitySignal,
@@ -22,10 +24,11 @@ public class FeederIOTalonFX implements FeederIO {
 
   public FeederIOTalonFX() {
     m_feederMotor = new TalonFX(19, Constants.kSuperstructureCanBus);
-    m_topNoteSensor = new DigitalInput(0);
+    m_noteSensor = new DigitalInput(0);
     m_glitchFilter = new DigitalGlitchFilter();
     m_glitchFilter.setPeriodNanoSeconds(Duration.ofMillis(1).toNanos());
-    m_glitchFilter.add(m_topNoteSensor);
+    m_glitchFilter.add(m_noteSensor);
+    m_sensorThread = new SensorThread(m_noteSensor);
 
     final TalonFXConfiguration feederMotorConfig = new TalonFXConfiguration();
     m_feederMotor.getConfigurator().apply(feederMotorConfig);
@@ -40,8 +43,14 @@ public FeederIOTalonFX() {
     m_rollerAppliedVoltageSignal = m_feederMotor.getMotorVoltage();
     m_rollerCurrentSignal = m_feederMotor.getSupplyCurrent();
     BaseStatusSignal.setUpdateFrequencyForAll(
-        50.0, m_rollerVelocitySignal, m_rollerAppliedVoltageSignal, m_rollerCurrentSignal);
+        50.0,
+        m_rollerPositionSignal,
+        m_rollerVelocitySignal,
+        m_rollerAppliedVoltageSignal,
+        m_rollerCurrentSignal);
     m_feederMotor.optimizeBusUtilization();
+
+    if (kUseHighFrequencySensorPolling) m_sensorThread.start();
   }
 
   @Override
@@ -58,7 +67,10 @@ public void updateInputs(FeederIOInputs inputs) {
     inputs.rollerVelocityRps = m_rollerVelocitySignal.getValue();
     inputs.rollerAppliedVolts = m_rollerAppliedVoltageSignal.getValue();
     inputs.rollerCurrentAmps = m_rollerCurrentSignal.getValue();
-    inputs.topNoteSensorTripped = m_topNoteSensor.get();
+    inputs.topNoteSensorTripped =
+        kUseHighFrequencySensorPolling
+            ? m_sensorThread.getSensorTripped.getAsBoolean()
+            : m_noteSensor.get();
   }
 
   @Override
diff --git a/src/main/java/frc/robot/feeder/SensorThread.java b/src/main/java/frc/robot/feeder/SensorThread.java
new file mode 100644
index 0000000..ba5f810
--- /dev/null
+++ b/src/main/java/frc/robot/feeder/SensorThread.java
@@ -0,0 +1,40 @@
+package frc.robot.feeder;
+
+import edu.wpi.first.wpilibj.DigitalSource;
+import edu.wpi.first.wpilibj.Notifier;
+import edu.wpi.first.wpilibj.SynchronousInterrupt;
+import java.util.function.BooleanSupplier;
+
+public class SensorThread {
+  private final double kUpdateFrequencyHz = 200.0;
+  private final Notifier m_thread;
+  private final SynchronousInterrupt m_synchronousInterrupt;
+  private double m_lastRisingEdgeTimestamp = 0.0;
+  private double m_lastFallingEdgeTimestamp = 0.0;
+  private boolean m_sensorTripped = false;
+  public final BooleanSupplier getSensorTripped = () -> m_sensorTripped;
+
+  public SensorThread(DigitalSource digitalSource) {
+    m_thread = new Notifier(this::periodic);
+    m_thread.setName("SensorThread");
+    m_synchronousInterrupt = new SynchronousInterrupt(digitalSource);
+    m_synchronousInterrupt.setInterruptEdges(true, true);
+  }
+
+  public void start() {
+    m_thread.startPeriodic(1.0 / kUpdateFrequencyHz);
+  }
+
+  private void periodic() {
+    final double latestRisingEdgeTimestamp = m_synchronousInterrupt.getRisingTimestamp();
+    final double latestFallingEdgeTimestamp = m_synchronousInterrupt.getFallingTimestamp();
+    final boolean newRisingEdge = latestRisingEdgeTimestamp > m_lastRisingEdgeTimestamp;
+    if (newRisingEdge) m_lastRisingEdgeTimestamp = latestRisingEdgeTimestamp;
+    final boolean newFallingEdge = latestFallingEdgeTimestamp > m_lastFallingEdgeTimestamp;
+    if (newFallingEdge) m_lastFallingEdgeTimestamp = latestFallingEdgeTimestamp;
+    if (latestFallingEdgeTimestamp > latestRisingEdgeTimestamp && newFallingEdge)
+      m_sensorTripped = true;
+    else if (latestRisingEdgeTimestamp > latestFallingEdgeTimestamp && newRisingEdge)
+      m_sensorTripped = false;
+  }
+}