From 60dd55fd5e28801508d6cec3b35e1d3bb41cacbd Mon Sep 17 00:00:00 2001
From: Mihir Patankar <mpatankar06@gmail.com>
Date: Mon, 15 Apr 2024 00:58:08 -0400
Subject: [PATCH] remove sensor multithreading (dumb idea)

---
 .../frc/robot/feeder/FeederIOTalonFX.java     |  9 +----
 .../java/frc/robot/feeder/SensorThread.java   | 40 -------------------
 2 files changed, 1 insertion(+), 48 deletions(-)
 delete 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 9456be3..ebbc916 100644
--- a/src/main/java/frc/robot/feeder/FeederIOTalonFX.java
+++ b/src/main/java/frc/robot/feeder/FeederIOTalonFX.java
@@ -15,7 +15,6 @@ public class FeederIOTalonFX implements FeederIO {
   private final TalonFX m_feederMotor;
   private final DigitalInput m_noteSensor;
   private final DigitalGlitchFilter m_glitchFilter;
-  private final SensorThread m_sensorThread;
 
   private final StatusSignal<Double> m_rollerPositionSignal,
       m_rollerVelocitySignal,
@@ -28,7 +27,6 @@ public FeederIOTalonFX() {
     m_glitchFilter = new DigitalGlitchFilter();
     m_glitchFilter.setPeriodNanoSeconds(Duration.ofMillis(1).toNanos());
     m_glitchFilter.add(m_noteSensor);
-    m_sensorThread = new SensorThread(m_noteSensor);
 
     final TalonFXConfiguration feederMotorConfig = new TalonFXConfiguration();
     m_feederMotor.getConfigurator().apply(feederMotorConfig);
@@ -49,8 +47,6 @@ public FeederIOTalonFX() {
         m_rollerAppliedVoltageSignal,
         m_rollerCurrentSignal);
     m_feederMotor.optimizeBusUtilization();
-
-    if (kUseHighFrequencySensorPolling) m_sensorThread.start();
   }
 
   @Override
@@ -67,10 +63,7 @@ public void updateInputs(FeederIOInputs inputs) {
     inputs.rollerVelocityRps = m_rollerVelocitySignal.getValue();
     inputs.rollerAppliedVolts = m_rollerAppliedVoltageSignal.getValue();
     inputs.rollerCurrentAmps = m_rollerCurrentSignal.getValue();
-    inputs.topNoteSensorTripped =
-        kUseHighFrequencySensorPolling
-            ? m_sensorThread.getSensorTripped.getAsBoolean()
-            : m_noteSensor.get();
+    inputs.topNoteSensorTripped = m_noteSensor.get();
   }
 
   @Override
diff --git a/src/main/java/frc/robot/feeder/SensorThread.java b/src/main/java/frc/robot/feeder/SensorThread.java
deleted file mode 100644
index ba5f810..0000000
--- a/src/main/java/frc/robot/feeder/SensorThread.java
+++ /dev/null
@@ -1,40 +0,0 @@
-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;
-  }
-}