fix review changes

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

@@ -72,6 +72,14 @@ object Constants {
   }
 
   object Drivetrain {
+
+    enum class DrivetrainType {
+      PHOENIX_TALON,
+      REV_NEO
+    }
+
+    val DRIVETRAIN_TYPE = DrivetrainType.PHOENIX_TALON
+
     const val FRONT_LEFT_DRIVE_ID = 11
     const val FRONT_LEFT_STEERING_ID = 21
     const val FRONT_LEFT_ANALOG_POTENTIOMETER = 1

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

@@ -23,8 +23,6 @@ object GyroIOPigeon2 : GyroIO {
 
   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
@@ -99,7 +97,7 @@ object GyroIOPigeon2 : GyroIO {
     pigeon2Configuration.MountPose.MountPoseRoll = GyroConstants.mountRoll.inRadians
 
     yawPositionQueue =
-      if (phoenixDrive) {
+      if (Constants.Drivetrain.DRIVETRAIN_TYPE == Constants.Drivetrain.DrivetrainType.PHOENIX_TALON) {
         PhoenixOdometryThread.getInstance().registerSignal(pigeon2, pigeon2.getYaw())
       } else {
         SparkMaxOdometryThread.getInstance().registerSignal {

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

@@ -43,7 +43,7 @@ class SwerveModule(val io: SwerveModuleIO) {
 
   var modulePosition = SwerveModulePosition()
 
-  var positionDeltas: Array<SwerveModulePosition?>
+  var positionDeltas = mutableListOf<SwerveModulePosition>()
 
   private var speedSetPoint: LinearVelocity = 0.feet.perSecond
   private var accelerationSetPoint: LinearAcceleration = 0.feet.perSecond.perSecond
@@ -115,7 +115,6 @@ class SwerveModule(val io: SwerveModuleIO) {
       drivekD.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KD)
     }
 
-    positionDeltas = arrayOf()
   }
 
   fun updateInputs() {
@@ -127,14 +126,15 @@ class SwerveModule(val io: SwerveModuleIO) {
 
     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] =
+      positionDeltas.add(
         SwerveModulePosition(
           (newDrivePosition - lastDrivePosition).inMeters, newSteeringAngle.inRotation2ds
         )
+      )
       lastDrivePosition = newDrivePosition
     }
 

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

@@ -42,6 +42,7 @@ import org.team4099.lib.units.derived.Volt
 import org.team4099.lib.units.derived.inRadians
 import org.team4099.lib.units.derived.inVolts
 import org.team4099.lib.units.derived.radians
+import org.team4099.lib.units.derived.rotations
 import org.team4099.lib.units.derived.volts
 import org.team4099.lib.units.perSecond
 import java.lang.Math.PI
@@ -260,9 +261,9 @@ class SwerveModuleIOFalcon(
       (steeringFalcon.get() * RobotController.getBatteryVoltage()).volts
 
     inputs.driveStatorCurrent = driveStatorCurrentSignal.value.amps
-    inputs.driveSupplyCurrent = driveFalcon.supplyCurrent.value.amps
-    inputs.steeringStatorCurrent = steeringFalcon.statorCurrent.value.amps
-    inputs.steeringSupplyCurrent = steeringFalcon.statorCurrent.value.amps
+    inputs.driveSupplyCurrent = driveSupplyCurrentSignal.value.amps
+    inputs.steeringStatorCurrent = steeringStatorCurrentSignal.value.amps
+    inputs.steeringSupplyCurrent = steeringSupplyCurrentSignal.value.amps
 
     inputs.drivePosition = driveSensor.position
     inputs.steeringPosition = steeringSensor.position
@@ -271,8 +272,8 @@ class SwerveModuleIOFalcon(
     inputs.steeringVelocity = steeringSensor.velocity
 
     // processor temp is also something we may want to log ?
-    inputs.driveTemp = driveFalcon.deviceTemp.value.celsius
-    inputs.steeringTemp = steeringFalcon.deviceTemp.value.celsius
+    inputs.driveTemp = driveTempSignal.value.celsius
+    inputs.steeringTemp = steeringTempSignal.value.celsius
 
     inputs.odometryDrivePositions =
       drivePositionQueue
@@ -289,8 +290,7 @@ class SwerveModuleIOFalcon(
       steeringPositionQueue
       .stream()
       .map { value: Double ->
-        Rotation2d.fromRotations(value / DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO)
-          .radians
+        (value / DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO).rotations
       }
       .toArray() as
       Array<Angle>
@@ -334,7 +334,7 @@ class SwerveModuleIOFalcon(
         driveSensor.velocityToRawUnits(speed),
         driveSensor.accelerationToRawUnits(acceleration),
         DrivetrainConstants.FOC_ENABLED,
-        feedforward.inVolts / 12.0,
+        feedforward.inVolts / DrivetrainConstants.DRIVE_COMPENSATION_VOLTAGE.inVolts,
         0,
         false
       )
@@ -370,13 +370,18 @@ class SwerveModuleIOFalcon(
         else (2 * PI).radians - (potentiometerOutput.radians - zeroOffset)
       )
     )
+
+    drivePositionQueue.clear()
+    steeringPositionQueue.clear()
     Logger.getInstance()
       .recordOutput("$label/zeroPositionRadians", steeringSensor.position.inRadians)
     println("Loading Zero for Module $label (${steeringFalcon.position.value})")
   }
 
   override fun zeroDrive() {
     driveFalcon.setPosition(0.0)
+    drivePositionQueue.clear()
+    steeringPositionQueue.clear()
   }
 
   override fun configureDrivePID(

src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/threads/PhoenixOdometryThread.java

@@ -103,7 +103,7 @@ public void run() {
                     queues.get(i).offer(signals[i].getValueAsDouble());
                 }
             } finally {
-                Drivetrain.Companion.setOdometryLock(true);
+                Drivetrain.Companion.setOdometryLock(false);
             }
         }
     }

src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/threads/SparkMaxOdometryThread.java

@@ -61,7 +61,7 @@ public Queue<Double> registerSignal(DoubleSupplier signal) {
     }
 
     private void periodic() {
-        Drivetrain.Companion.setOdometryLock(false);
+        Drivetrain.Companion.setOdometryLock(true);
         try {
             for (int i = 0; i < signals.size(); i++) {
                 queues.get(i).offer(signals.get(i).getAsDouble());