finish multithreading

build.gradle

@@ -83,7 +83,7 @@ dependencies {
 	simulationRelease wpi.sim.enableRelease()
 
 	implementation 'org.jetbrains.kotlin:kotlin-test-junit5'
-	implementation 'com.github.team4099:FalconUtils:1.1.26'
+	implementation 'com.github.team4099:FalconUtils:1.1.27r2'
 	implementation 'org.apache.commons:commons-collections4:4.0'
 	implementation 'com.google.code.gson:gson:2.10.1'
 	implementation "io.javalin:javalin:5.3.2"

src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt

@@ -31,6 +31,8 @@ object DrivetrainConstants {
   const val FOC_ENABLED = false
   const val MINIMIZE_SKEW = false
 
+  const val OMOMETRY_UPDATE_FREQUENCY = 250.0
+
   const val WHEEL_COUNT = 4
   val WHEEL_DIAMETER = 3.827.inches
   val DRIVETRAIN_LENGTH = 22.750.inches

src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt

@@ -46,6 +46,8 @@ import org.team4099.lib.units.derived.inRotation2ds
 import org.team4099.lib.units.derived.radians
 import org.team4099.lib.units.inMetersPerSecond
 import org.team4099.lib.units.perSecond
+import java.util.concurrent.locks.Lock
+import java.util.concurrent.locks.ReentrantLock
 import java.util.function.Supplier
 
 class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemBase() {
@@ -58,6 +60,17 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
   val gyroInputs = GyroIO.GyroIOInputs()
   val swerveModules = swerveModuleIOs.getSwerveModules() // FL, FR, BL, BR
 
+  companion object {
+    var odometryLock: Lock = ReentrantLock()
+    fun setOdometryLock(Locked: Boolean) {
+      if (Locked) {
+        odometryLock.lock()
+      } else {
+        odometryLock.unlock()
+      }
+    }
+  }
+
   var gyroYawOffset = 0.0.radians
 
   val closestAlignmentAngle: Angle
@@ -181,8 +194,15 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
 
   override fun periodic() {
     val startTime = Clock.realTimestamp
-    gyroNotConnectedAlert.set(!gyroInputs.gyroConnected)
+
+    odometryLock.lock(); // Prevents odometry updates while reading data
+
     gyroIO.updateInputs(gyroInputs)
+    swerveModules.forEach { it.updateInputs() }
+
+    odometryLock.unlock()
+
+    gyroNotConnectedAlert.set(!gyroInputs.gyroConnected)
 
     swerveModules.forEach { it.periodic() }
 
@@ -272,35 +292,31 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
   }
 
   private fun updateOdometry() {
-    val wheelDeltas =
-      mutableListOf(
-        SwerveModulePosition(),
-        SwerveModulePosition(),
-        SwerveModulePosition(),
-        SwerveModulePosition()
-      )
-    for (i in 0 until 4) {
-      wheelDeltas[i] =
-        SwerveModulePosition(
-          swerveModules[i].inputs.drivePosition.inMeters -
-            lastModulePositions[i].distanceMeters,
-          swerveModules[i].inputs.steeringPosition.inRotation2ds
-        )
-      lastModulePositions[i] =
-        SwerveModulePosition(
-          swerveModules[i].inputs.drivePosition.inMeters,
-          swerveModules[i].inputs.steeringPosition.inRotation2ds
-        )
+
+    var deltaCount =
+      if (gyroInputs.gyroConnected) gyroInputs.odometryYawPositions.size else Integer.MAX_VALUE
+    for (i in 0..4) {
+      deltaCount = Math.min(deltaCount, swerveModules[i].positionDeltas.size)
     }
+    for (deltaIndex in 0..deltaCount) {
+      // Read wheel deltas from each module
+      val wheelDeltas = arrayOfNulls<SwerveModulePosition>(4)
+      for (moduleIndex in 0..4) {
+        wheelDeltas[moduleIndex] = swerveModules[moduleIndex].positionDeltas[deltaIndex]
+      }
 
-    var driveTwist = swerveDriveKinematics.toTwist2d(*wheelDeltas.toTypedArray())
+      var driveTwist = swerveDriveKinematics.toTwist2d(*wheelDeltas)
 
-    if (gyroInputs.gyroConnected) {
-      driveTwist =
-        edu.wpi.first.math.geometry.Twist2d(
-          driveTwist.dx, driveTwist.dy, (gyroInputs.rawGyroYaw - lastGyroYaw).inRadians
-        )
-      lastGyroYaw = gyroInputs.rawGyroYaw
+      if (gyroInputs.gyroConnected) {
+        driveTwist =
+          edu.wpi.first.math.geometry.Twist2d(
+            driveTwist.dx, driveTwist.dy, (gyroInputs.rawGyroYaw - lastGyroYaw).inRadians
+          )
+        lastGyroYaw = gyroInputs.rawGyroYaw
+      }
+
+      swerveDrivePoseEstimator.addDriveData(Clock.fpgaTime.inSeconds, Twist2d(driveTwist))
+      odometryPose = odometryPose.exp(Twist2d(driveTwist))
     }
 
     // reversing the drift to store the ground truth pose
@@ -323,8 +339,6 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
 
       Logger.getInstance().recordOutput(VisionConstants.SIM_POSE_TOPIC_NAME, undriftedPose.pose2d)
     }
-
-    swerveDrivePoseEstimator.addDriveData(Clock.fpgaTime.inSeconds, Twist2d(driveTwist))
   }
 
   /**

src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIO.kt

@@ -19,6 +19,8 @@ interface GyroIO {
     var gyroPitchRate = 0.0.radians.perSecond
     var gyroRollRate = 0.0.radians.perSecond
 
+    var odometryYawPositions = arrayOf<Angle>()
+
     var gyroConnected = false
 
     override fun toLog(table: LogTable?) {

src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIOPigeon2.kt

@@ -5,6 +5,8 @@ import com.ctre.phoenix6.hardware.Pigeon2
 import com.team4099.robot2023.config.constants.Constants
 import com.team4099.robot2023.config.constants.DrivetrainConstants
 import com.team4099.robot2023.config.constants.GyroConstants
+import com.team4099.robot2023.subsystems.drivetrain.swervemodule.threads.PhoenixOdometryThread
+import com.team4099.robot2023.subsystems.drivetrain.swervemodule.threads.SparkMaxOdometryThread
 import org.littletonrobotics.junction.Logger
 import org.team4099.lib.units.AngularVelocity
 import org.team4099.lib.units.derived.Angle
@@ -13,17 +15,22 @@ import org.team4099.lib.units.derived.inDegrees
 import org.team4099.lib.units.derived.inRadians
 import org.team4099.lib.units.inDegreesPerSecond
 import org.team4099.lib.units.perSecond
+import java.util.Queue
 import kotlin.math.IEEErem
 
 object GyroIOPigeon2 : GyroIO {
   private var pigeon2 = Pigeon2(Constants.Gyro.PIGEON_2_ID, Constants.Universal.CANIVORE_NAME)
 
   private val isConnected: Boolean = pigeon2.upTime.value > 0.0
 
+  val phoenixDrive = true
+
   var gyroYawOffset: Angle = 0.0.degrees
   var gyroPitchOffset: Angle = 0.0.degrees
   var gyroRollOffset: Angle = 0.0.degrees
 
+  val yawPositionQueue: Queue<Double>
+
   val rawGyro: Angle = 0.0.degrees
 
   /** The current angle of the drivetrain. */
@@ -91,6 +98,15 @@ object GyroIOPigeon2 : GyroIO {
     pigeon2Configuration.MountPose.MountPoseYaw = GyroConstants.mountYaw.inRadians
     pigeon2Configuration.MountPose.MountPoseRoll = GyroConstants.mountRoll.inRadians
 
+    yawPositionQueue =
+      if (phoenixDrive) {
+        PhoenixOdometryThread.getInstance().registerSignal(pigeon2, pigeon2.getYaw())
+      } else {
+        SparkMaxOdometryThread.getInstance().registerSignal {
+          pigeon2.getYaw().getValueAsDouble()
+        }
+      }
+
     // TODO look into more pigeon configuration stuff
     pigeon2.configurator.apply(pigeon2Configuration)
   }
@@ -109,6 +125,10 @@ object GyroIOPigeon2 : GyroIO {
     inputs.gyroPitchRate = gyroPitchRate
     inputs.gyroRollRate = gyroRollRate
 
+    inputs.odometryYawPositions =
+      yawPositionQueue.stream().map { value: Double -> value.degrees }.toArray() as Array<Angle>
+    yawPositionQueue.clear()
+
     Logger.getInstance().recordOutput("Gyro/rawYawDegrees", pigeon2.yaw.value)
   }
 

src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt

@@ -43,11 +43,15 @@ class SwerveModule(val io: SwerveModuleIO) {
 
   var modulePosition = SwerveModulePosition()
 
+  var positionDeltas: Array<SwerveModulePosition?>
+
   private var speedSetPoint: LinearVelocity = 0.feet.perSecond
   private var accelerationSetPoint: LinearAcceleration = 0.feet.perSecond.perSecond
 
   private var steeringSetPoint: Angle = 0.degrees
 
+  private var lastDrivePosition = 0.meters
+
   private var shouldInvert = false
 
   private val steeringkP =
@@ -110,11 +114,30 @@ class SwerveModule(val io: SwerveModuleIO) {
       drivekI.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KI)
       drivekD.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KD)
     }
+
+    positionDeltas = arrayOf()
+  }
+
+  fun updateInputs() {
+    io.updateInputs(inputs)
   }
 
   fun periodic() {
     io.updateInputs(inputs)
 
+    val deltaCount =
+      Math.min(inputs.odometryDrivePositions.size, inputs.odometrySteeringPositions.size)
+    positionDeltas = arrayOfNulls(deltaCount)
+    for (i in 0..deltaCount) {
+      val newDrivePosition = inputs.odometryDrivePositions[i]
+      val newSteeringAngle = inputs.odometrySteeringPositions[i]
+      positionDeltas[i] =
+        SwerveModulePosition(
+          (newDrivePosition - lastDrivePosition).inMeters, newSteeringAngle.inRotation2ds
+        )
+      lastDrivePosition = newDrivePosition
+    }
+
     // Updating SwerveModulePosition every loop cycle
     modulePosition.distanceMeters = inputs.drivePosition.inMeters
     modulePosition.angle = inputs.steeringPosition.inRotation2ds

src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt

@@ -7,6 +7,7 @@ import org.team4099.lib.units.AngularVelocity
 import org.team4099.lib.units.LinearAcceleration
 import org.team4099.lib.units.LinearVelocity
 import org.team4099.lib.units.Velocity
+import org.team4099.lib.units.base.Length
 import org.team4099.lib.units.base.Meter
 import org.team4099.lib.units.base.amps
 import org.team4099.lib.units.base.celsius
@@ -48,6 +49,9 @@ interface SwerveModuleIO {
     var driveTemp = 0.0.celsius
     var steeringTemp = 0.0.celsius
 
+    var odometryDrivePositions = arrayOf<Length>()
+    var odometrySteeringPositions = arrayOf<Angle>()
+
     var potentiometerOutputRaw = 0.0
     var potentiometerOutputRadians = 0.0.radians