From f53b8fb41927d9dfa0921990a66c14c1d080f6d7 Mon Sep 17 00:00:00 2001
From: MatthewChoulas <matthewchoulas@gmail.com>
Date: Sun, 19 Jan 2025 19:26:45 -0500
Subject: [PATCH] add acceleration units

---
 .../org/team4099/lib/units/MechanismUnits.kt    | 17 +++++++++++++++++
 1 file changed, 17 insertions(+)

diff --git a/src/main/kotlin/org/team4099/lib/units/MechanismUnits.kt b/src/main/kotlin/org/team4099/lib/units/MechanismUnits.kt
index 8e21841..ee33257 100644
--- a/src/main/kotlin/org/team4099/lib/units/MechanismUnits.kt
+++ b/src/main/kotlin/org/team4099/lib/units/MechanismUnits.kt
@@ -11,6 +11,7 @@ import org.team4099.lib.units.base.inSeconds
 import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.base.minutes
 import org.team4099.lib.units.base.seconds
+import org.team4099.lib.units.derived.AccelerationFeedforward
 import org.team4099.lib.units.derived.DerivativeGain
 import org.team4099.lib.units.derived.ElectricalPotential
 import org.team4099.lib.units.derived.IntegralGain
@@ -75,6 +76,8 @@ interface MechanismSensor<U : UnitKey> {
   ): Double
 
   fun velocityFeedforwardToRawUnits(velocityFeedforward: VelocityFeedforward<U, Volt>): Double
+
+  fun accelerationFeedforwardToRawUnits(accelerationFeedforward: AccelerationFeedforward<U, Volt>): Double
 }
 
 class LinearMechanismSensor(
@@ -173,6 +176,13 @@ class LinearMechanismSensor(
       velocityToRawUnits(1.0.meters.perSecond) /
       compensationVoltage.inVolts * fullPowerThrottle
   }
+
+  override inline fun accelerationFeedforwardToRawUnits(
+    accelerationFeedforward: AccelerationFeedforward<Meter, Volt>): Double {
+    return accelerationFeedforward.value /
+            accelerationToRawUnits(1.0.meters.perSecond.perSecond) /
+            compensationVoltage.inVolts * fullPowerThrottle
+  }
 }
 
 class AngularMechanismSensor(
@@ -268,6 +278,13 @@ class AngularMechanismSensor(
       velocityToRawUnits(1.0.radians.perSecond) /
       compensationVoltage.inVolts * fullPowerThrottle
   }
+
+  override inline fun accelerationFeedforwardToRawUnits(
+    accelerationFeedforward: AccelerationFeedforward<Radian, Volt>): Double {
+    return accelerationFeedforward.value /
+            accelerationToRawUnits(1.0.radians.perSecond.perSecond) /
+            compensationVoltage.inVolts * fullPowerThrottle
+  }
 }
 
 fun ctreAngularMechanismSensor(