From b23c2ba65a5b3ec56e7e3c0adab97fff081fc24d Mon Sep 17 00:00:00 2001
From: Vignesh Balasubramaniam <vigneshb.hp@gmail.com>
Date: Wed, 18 Sep 2024 19:32:15 -0500
Subject: [PATCH] Run input thread for IMUs

---
 .../lasarobotics/hardware/ctre/Pigeon2.java   | 24 ++++++++++++++++++-
 .../hardware/kauailabs/NavX2.java             |  7 +++++-
 2 files changed, 29 insertions(+), 2 deletions(-)

diff --git a/src/main/java/org/lasarobotics/hardware/ctre/Pigeon2.java b/src/main/java/org/lasarobotics/hardware/ctre/Pigeon2.java
index 33f3132d..7187fea5 100644
--- a/src/main/java/org/lasarobotics/hardware/ctre/Pigeon2.java
+++ b/src/main/java/org/lasarobotics/hardware/ctre/Pigeon2.java
@@ -17,8 +17,10 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.units.Angle;
 import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Time;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.Velocity;
+import edu.wpi.first.wpilibj.Notifier;
 
 /** CTRE Pigeon 2.0 */
 public class Pigeon2 extends LoggableHardware {
@@ -60,6 +62,11 @@ public static class Pigeon2Inputs {
     public Rotation2d rotation2d = GlobalConstants.ROTATION_ZERO;
   }
 
+  private int DEFAULT_THREAD_PERIOD = 10;
+
+  private Notifier m_inputThread;
+  private Measure<Time> m_inputThreadPeriod = Units.Milliseconds.of(DEFAULT_THREAD_PERIOD);
+
   private com.ctre.phoenix6.hardware.Pigeon2 m_pigeon;
 
   private ID m_id;
@@ -69,6 +76,10 @@ public Pigeon2(ID id) {
     this.m_id = id;
     this.m_pigeon = new com.ctre.phoenix6.hardware.Pigeon2(id.deviceID, id.bus.name);
     this.m_inputs = new Pigeon2InputsAutoLogged();
+    this.m_inputThread = new Notifier(this::updateInputs);
+
+    // Start input thread
+    m_inputThread.startPeriodic(m_inputThreadPeriod.in(Units.Seconds));
 
     // Update inputs on init
     periodic();
@@ -155,7 +166,6 @@ private void updateInputs() {
    */
   @Override
   protected void periodic() {
-    updateInputs();
     Logger.processInputs(m_id.name, m_inputs);
   }
 
@@ -176,6 +186,18 @@ public ID getID() {
     return m_id;
   }
 
+  /**
+   * Set input thread period
+   * <p>
+   * Defaults to 10ms
+   * @param period Period between getting sensor updates
+   */
+  public void setPeriod(Measure<Time> period) {
+    m_inputThreadPeriod = period;
+    m_inputThread.stop();
+    m_inputThread.startPeriodic(m_inputThreadPeriod.in(Units.Seconds));
+  }
+
   /**
    * Configures all persistent settings to defaults (overloaded so timeoutMs is 50 ms).
    *
diff --git a/src/main/java/org/lasarobotics/hardware/kauailabs/NavX2.java b/src/main/java/org/lasarobotics/hardware/kauailabs/NavX2.java
index 773b9498..f55142f4 100644
--- a/src/main/java/org/lasarobotics/hardware/kauailabs/NavX2.java
+++ b/src/main/java/org/lasarobotics/hardware/kauailabs/NavX2.java
@@ -20,6 +20,7 @@
 import edu.wpi.first.units.Measure;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.Velocity;
+import edu.wpi.first.wpilibj.Notifier;
 import edu.wpi.first.wpilibj.SPI;
 
 /** NavX2 */
@@ -53,6 +54,7 @@ public static class NavX2Inputs {
 
   private AHRS m_navx;
   private SimDouble m_simNavXYaw;
+  private Notifier m_inputThread;
 
   private String m_name;
   private NavX2InputsAutoLogged m_inputs;
@@ -66,9 +68,13 @@ public NavX2(ID id, int updateRate) {
     this.m_name = id.name;
     this.m_navx = new AHRS(SPI.Port.kMXP, (byte)updateRate);
     this.m_inputs = new NavX2InputsAutoLogged();
+    this.m_inputThread = new Notifier(this::updateInputs);
     this.m_simNavXYaw = new SimDouble(SimDeviceDataJNI.getSimValueHandle(SimDeviceDataJNI.getSimDeviceHandle("navX-Sensor[0]"), "Yaw"));
     System.out.println();
 
+    // Start input thread
+    m_inputThread.startPeriodic(1.0 / updateRate);
+
     // Update inputs on init
     periodic();
 
@@ -187,7 +193,6 @@ private void updateInputs() {
    */
   @Override
   protected void periodic() {
-    updateInputs();
     Logger.processInputs(m_name, m_inputs);
   }