refactor: use units wherever possible (#28)

:3

simgui-ds.json

@@ -0,0 +1,97 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "Keyboard0"
+    }
+  ]
+}

simgui.json

@@ -0,0 +1,23 @@
+{
+  "NTProvider": {
+    "types": {
+      "/AdvantageKit/RealOutputs//Arm/Mechanism": "Mechanism2d",
+      "/AdvantageKit/RealOutputs/Indexer Angle": "Mechanism2d",
+      "/AdvantageKit/RealOutputs/Intake Angle": "Mechanism2d",
+      "/FMSInfo": "FMSInfo",
+      "/Shuffleboard/Teleoperated/Auto Chooser": "String Chooser",
+      "/Shuffleboard/Teleoperated/Field": "Field2d",
+      "/SmartDashboard/Intake Angle": "Mechanism2d"
+    },
+    "windows": {
+      "/SmartDashboard/Intake Angle": {
+        "window": {
+          "visible": true
+        }
+      }
+    }
+  },
+  "NetworkTables Info": {
+    "visible": true
+  }
+}

src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Module.kt

@@ -2,18 +2,25 @@ package com.frcteam3636.frc2024.subsystems.drivetrain
 
 import com.ctre.phoenix6.configs.Slot0Configs
 import com.ctre.phoenix6.configs.TorqueCurrentConfigs
+import com.ctre.phoenix6.controls.TorqueCurrentFOC
 import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC
 import com.frcteam3636.frc2024.*
 import com.frcteam3636.frc2024.utils.math.*
+import com.frcteam3636.frc2024.utils.swerve.speed
 import com.revrobotics.CANSparkBase
 import com.revrobotics.CANSparkLowLevel
 import com.revrobotics.SparkAbsoluteEncoder
 import edu.wpi.first.math.geometry.Rotation2d
 import edu.wpi.first.math.kinematics.SwerveModulePosition
 import edu.wpi.first.math.kinematics.SwerveModuleState
 import edu.wpi.first.math.system.plant.DCMotor
-import edu.wpi.first.math.util.Units
+import edu.wpi.first.units.Angle
+import edu.wpi.first.units.Distance
+import edu.wpi.first.units.Measure
+import edu.wpi.first.units.Units.*
+import edu.wpi.first.units.Velocity
 import edu.wpi.first.wpilibj.simulation.DCMotorSim
+import org.littletonrobotics.junction.Logger
 import kotlin.math.roundToInt
 
 interface SwerveModule {
@@ -77,7 +84,7 @@ class MAXSwerveModule(
 
     override val state: SwerveModuleState
         get() = SwerveModuleState(
-            drivingMotor.velocity, Rotation2d.fromRadians(turningEncoder.position) + chassisAngle
+            drivingMotor.velocity.`in`(MetersPerSecond), Rotation2d.fromRadians(turningEncoder.position) + chassisAngle
         )
 
     override val position: SwerveModulePosition
@@ -94,19 +101,20 @@ class MAXSwerveModule(
                 corrected, Rotation2d.fromRadians(turningEncoder.position)
             )
 
-            drivingMotor.velocity = optimized.speedMetersPerSecond
+            drivingMotor.velocity = optimized.speed
 
             turningPIDController.setReference(
                 optimized.angle.radians, CANSparkBase.ControlType.kPosition
             )
 
+
             field = optimized
         }
 }
 
 interface DrivingMotor {
-    val position: Double
-    var velocity: Double
+    val position: Measure<Distance>
+    var velocity: Measure<Velocity<Distance>>
 }
 
 class DrivingTalon(id: CTREDeviceId) : DrivingMotor {
@@ -121,26 +129,25 @@ class DrivingTalon(id: CTREDeviceId) : DrivingMotor {
             TorqueCurrentConfigs().apply {
                 withPeakForwardTorqueCurrent(DRIVING_CURRENT_LIMIT)
                 withPeakReverseTorqueCurrent(-DRIVING_CURRENT_LIMIT)
-            }
-        )
+            })
 
     }
 
     init {
         Robot.statusSignals[id.name] = inner.version
     }
 
-    override val position: Double
-        get() = inner.position.value * DRIVING_GEAR_RATIO_TALON * WHEEL_CIRCUMFERENCE
+    override val position: Measure<Distance>
+        get() = Meters.of(inner.position.value * DRIVING_GEAR_RATIO_TALON * WHEEL_CIRCUMFERENCE.`in`(Meters))
 
-    override var velocity: Double
-        get() = inner.velocity.value
+    override var velocity: Measure<Velocity<Distance>>
+        get() = MetersPerSecond.of(inner.velocity.value * DRIVING_GEAR_RATIO_TALON * WHEEL_CIRCUMFERENCE.`in`(Meters))
         set(value) {
-            inner.setControl(VelocityTorqueCurrentFOC(value / DRIVING_GEAR_RATIO_TALON / WHEEL_CIRCUMFERENCE).withSlot(0))
+            inner.setControl(VelocityTorqueCurrentFOC(value.`in`(MetersPerSecond) / DRIVING_GEAR_RATIO_TALON / WHEEL_CIRCUMFERENCE.`in`(Meters)))
         }
 }
 
-class DrivingSparkMAX(id: REVMotorControllerId) : DrivingMotor {
+class DrivingSparkMAX(val id: REVMotorControllerId) : DrivingMotor {
     private val inner = CANSparkMax(id, CANSparkLowLevel.MotorType.kBrushless).apply {
         restoreFactoryDefaults()
 
@@ -153,8 +160,8 @@ class DrivingSparkMAX(id: REVMotorControllerId) : DrivingMotor {
     init {
         inner.encoder.apply {
             // convert native units of rotations and RPM to meters and meters per second
-            positionConversionFactor = DRIVING_GEAR_RATIO_NEO * WHEEL_CIRCUMFERENCE
-            velocityConversionFactor = DRIVING_GEAR_RATIO_NEO * WHEEL_CIRCUMFERENCE / 60
+            positionConversionFactor =  WHEEL_CIRCUMFERENCE.`in`(Meters) / DRIVING_GEAR_RATIO_NEO
+            velocityConversionFactor = WHEEL_CIRCUMFERENCE.`in`(Meters) / DRIVING_GEAR_RATIO_NEO  / 60
         }
 
         inner.pidController.apply {
@@ -165,13 +172,14 @@ class DrivingSparkMAX(id: REVMotorControllerId) : DrivingMotor {
         }
     }
 
-    override val position: Double
-        get() = inner.encoder.position
+    override val position: Measure<Distance>
+        get() = Meters.of(inner.encoder.position)
 
-    override var velocity: Double
-        get() = inner.encoder.velocity
+    override var velocity: Measure<Velocity<Distance>>
+        get() = MetersPerSecond.of(inner.encoder.velocity)
         set(value) {
-            inner.set(value)
+            Logger.recordOutput("/Drivetrain/$id/OutputVel", value)
+            inner.pidController.setReference(value.`in`(MetersPerSecond), CANSparkBase.ControlType.kVelocity)
         }
 }
 
@@ -220,20 +228,23 @@ class SimSwerveModule : SwerveModule {
 
 // take the known wheel diameter, divide it by two to get the radius, then get the
 // circumference
-internal val WHEEL_RADIUS = Units.inchesToMeters(3.0) / 2
-internal val WHEEL_CIRCUMFERENCE = WHEEL_RADIUS * TAU
+internal val WHEEL_RADIUS = Inches.of(3.0).`in`(Meters) / 2
+internal val WHEEL_CIRCUMFERENCE = Meters.of(WHEEL_RADIUS * TAU)
 
-internal const val NEO_FREE_SPEED_RPM = 5676.0
-internal const val NEO_FREE_SPEED_RPS: Double = NEO_FREE_SPEED_RPM / 60
+internal val NEO_FREE_SPEED = RPM.of(5676.0)
+
+private const val DRIVING_MOTOR_PINION_TEETH = 14
 
 internal const val DRIVING_GEAR_RATIO_TALON = 1.0 / 3.56
-internal const val DRIVING_GEAR_RATIO_NEO = 0.0
+internal const val DRIVING_GEAR_RATIO_NEO = (45.0 * 22.0) / (DRIVING_MOTOR_PINION_TEETH * 15.0)
+
+internal val NEO_DRIVING_FREE_SPEED = (NEO_FREE_SPEED.`in`(RotationsPerSecond) * WHEEL_CIRCUMFERENCE.`in`(Meters)) / DRIVING_GEAR_RATIO_NEO
 
 internal val DRIVING_PID_GAINS_TALON: PIDGains = PIDGains(4.0, 0.0, 0.1)
 internal val DRIVING_PID_GAINS_NEO: PIDGains = PIDGains(0.04, 0.0, 0.0)
 internal val DRIVING_FF_GAINS_TALON: MotorFFGains = MotorFFGains(5.75, 0.0)
 internal val DRIVING_FF_GAINS_NEO: MotorFFGains =
-    MotorFFGains(0.0, 1 / NEO_FREE_SPEED_RPS, 0.0) // TODO: ensure this is right
+    MotorFFGains(0.0, 1 / NEO_DRIVING_FREE_SPEED, 0.0) // TODO: ensure this is right
 
 internal val TURNING_PID_GAINS: PIDGains = PIDGains(1.7, 0.0, 0.125)
 internal const val DRIVING_CURRENT_LIMIT = 35.0