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 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; + } +}