Implemented new 250 Hz odometry

src/main/kotlin/frc/team449/control/holonomic/OdometryThread.kt

@@ -0,0 +1,45 @@
+package frc.team449.control.holonomic
+
+import edu.wpi.first.wpilibj.Notifier
+import frc.team449.robot2024.constants.RobotConstants
+import java.util.Queue
+import java.util.concurrent.ArrayBlockingQueue
+import java.util.concurrent.locks.ReentrantLock
+import java.util.function.DoubleSupplier
+
+
+open class OdometryThread (
+  val odometryLock: ReentrantLock
+  ) {
+  private var signals = ArrayList<DoubleSupplier>()
+  private var queues = ArrayList<Queue<Double>>()
+  private var notifier = Notifier { this.periodic() }
+
+  init {
+    notifier.setName("OdometryThread")
+    notifier.startPeriodic(1.0 / RobotConstants.ODOMETRY_FREQUENCY)
+  }
+
+  open fun registerSignal(signal: DoubleSupplier): Queue<Double> {
+    val queue: Queue<Double> = ArrayBlockingQueue(100)
+    odometryLock.lock()
+    try {
+      signals.add(signal)
+      queues.add(queue)
+    } finally {
+      odometryLock.unlock()
+    }
+    return queue
+  }
+
+  private fun periodic() {
+    odometryLock.lock()
+    try {
+      for (i in signals.indices) {
+        queues[i].add(signals[i].asDouble)
+      }
+    } finally {
+      odometryLock.unlock()
+    }
+  }
+}

src/main/kotlin/frc/team449/control/holonomic/SwerveDrive.kt

@@ -75,6 +75,9 @@ open class SwerveDrive(
     VisionConstants.MULTI_TAG_TRUST
   )
 
+  /** Things to do with odometry */
+
+
   var desiredSpeeds: ChassisSpeeds = ChassisSpeeds()
 
   protected var maxSpeed: Double = 0.0

src/main/kotlin/frc/team449/robot2024/Robot.kt

@@ -12,14 +12,16 @@ import frc.team449.system.AHRS
 import frc.team449.system.light.Light
 import monologue.Annotations.Log
 import monologue.Logged
+import java.util.concurrent.locks.ReentrantLock
 
 class Robot : RobotBase(), Logged {
 
   val driveController = XboxController(0)
 
   val mechController = XboxController(1)
 
-  val ahrs = AHRS(SPI.Port.kMXP)
+  val odometryLock = ReentrantLock()
+  val ahrs = AHRS(SPI.Port.kMXP, odometryLock)
 
   // Instantiate/declare PDP and other stuff here
 

src/main/kotlin/frc/team449/robot2024/constants/RobotConstants.kt

@@ -10,7 +10,9 @@ import kotlin.math.PI
 
 object RobotConstants {
 
-  /** Other CAN ID */
+  const val ODOMETRY_FREQUENCY = 250.0
+
+    /** Other CAN ID */
   const val PDH_CAN = 1
 
   /** Controller Configurations */

src/main/kotlin/frc/team449/system/AHRS.kt

@@ -5,32 +5,49 @@ import edu.wpi.first.math.geometry.Rotation2d
 import edu.wpi.first.wpilibj.SPI
 import edu.wpi.first.wpilibj.Timer
 import edu.wpi.first.wpilibj.simulation.SimDeviceSim
+import frc.team449.control.holonomic.OdometryThread
 import frc.team449.util.simBooleanProp
 import frc.team449.util.simDoubleProp
+import java.util.concurrent.locks.ReentrantLock
 
 class AHRS(
-  private val navx: com.kauailabs.navx.frc.AHRS
+  private val navx: com.kauailabs.navx.frc.AHRS,
+  private val odometryLock: ReentrantLock
 ) {
 
   var prevPos = Double.NaN
   var prevTime = Double.NaN
+  val odometryThread = OdometryThread(odometryLock)
+  val pitchQueue = odometryThread.registerSignal { navx.pitch.toDouble() }
+  val rollQueue = odometryThread.registerSignal { navx.roll.toDouble() }
+  val headingQueue = odometryThread.registerSignal { navx.fusedHeading.toDouble() }
 
   private val filter = LinearFilter.movingAverage(5)
 
+
   /** The current reading of the gyro with the offset included */
   val heading: Rotation2d
     get() {
-      return -Rotation2d.fromDegrees(navx.fusedHeading.toDouble())
+      odometryLock.lock()
+      val output = -Rotation2d.fromDegrees(headingQueue.peek())
+      odometryLock.unlock()
+      return output
     }
 
   val pitch: Rotation2d
     get() {
-      return -Rotation2d.fromDegrees(navx.pitch.toDouble())
+      odometryLock.lock()
+      val output = -Rotation2d.fromDegrees(pitchQueue.peek())
+      odometryLock.unlock()
+      return output
     }
 
   val roll: Rotation2d
     get() {
-      return -Rotation2d.fromDegrees(navx.roll.toDouble())
+      odometryLock.lock()
+      val output = -Rotation2d.fromDegrees(rollQueue.peek())
+      odometryLock.unlock()
+      return output
     }
 
   fun angularXVel(): Double {
@@ -50,9 +67,11 @@ class AHRS(
   }
 
   constructor(
-    port: SPI.Port = SPI.Port.kMXP
+    port: SPI.Port = SPI.Port.kMXP,
+    lock: ReentrantLock
   ) : this(
-    com.kauailabs.navx.frc.AHRS(port)
+    com.kauailabs.navx.frc.AHRS(port),
+    lock
   )
 
   fun calibrated(): Boolean {