make arm ff and elevator ff compatible w generic ff update

src/main/kotlin/org/team4099/lib/controller/ArmFeedforward.kt

@@ -9,6 +9,7 @@ import org.team4099.lib.units.derived.ElectricalPotential
 import org.team4099.lib.units.derived.Radian
 import org.team4099.lib.units.derived.StaticFeedforward
 import org.team4099.lib.units.derived.VelocityFeedforward
+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.inVoltsPerRadian
@@ -23,10 +24,10 @@ import org.team4099.lib.units.perSecond
 import edu.wpi.first.math.controller.ArmFeedforward as WPIArmFeedforward
 
 class ArmFeedforward(
-  kS: StaticFeedforward,
+  kS: StaticFeedforward<Volt>,
   kG: AngularGravityFeedforward,
-  kV: VelocityFeedforward<Radian>,
-  kA: AccelerationFeedforward<Radian>
+  kV: VelocityFeedforward<Radian, Volt>,
+  kA: AccelerationFeedforward<Radian, Volt>
 ) {
   private val feedforward =
     WPIArmFeedforward(
@@ -37,9 +38,9 @@ class ArmFeedforward(
     )
 
   constructor(
-    kS: StaticFeedforward,
+    kS: StaticFeedforward<Volt>,
     kG: AngularGravityFeedforward,
-    kV: VelocityFeedforward<Radian>
+    kV: VelocityFeedforward<Radian, Volt>
   ) : this(kS, kG, kV, 0.volts.perRadianPerSecondPerSecond)
 
   fun calculate(

src/main/kotlin/org/team4099/lib/controller/ElevatorFeedforward.kt

@@ -9,6 +9,7 @@ import org.team4099.lib.units.derived.ElectricalPotential
 import org.team4099.lib.units.derived.LinearGravityFeedforward
 import org.team4099.lib.units.derived.StaticFeedforward
 import org.team4099.lib.units.derived.VelocityFeedforward
+import org.team4099.lib.units.derived.Volt
 import org.team4099.lib.units.derived.inVolts
 import org.team4099.lib.units.derived.inVoltsPerMeterPerSecond
 import org.team4099.lib.units.derived.inVoltsPerMeterPerSecondPerSecond
@@ -20,20 +21,20 @@ import org.team4099.lib.units.perSecond
 import edu.wpi.first.math.controller.ElevatorFeedforward as WPIElevatorFeedforward
 
 class ElevatorFeedforward(
-  kS: StaticFeedforward,
+  kS: StaticFeedforward<Volt>,
   kG: LinearGravityFeedforward,
-  kV: VelocityFeedforward<Meter>,
-  kA: AccelerationFeedforward<Meter>
+  kV: VelocityFeedforward<Meter, Volt>,
+  kA: AccelerationFeedforward<Meter, Volt>
 ) {
   private val feedforward =
     WPIElevatorFeedforward(
       kS.inVolts, kG.inVolts, kV.inVoltsPerMeterPerSecond, kA.inVoltsPerMeterPerSecondPerSecond
     )
 
   constructor(
-    kS: StaticFeedforward,
+    kS: StaticFeedforward<Volt>,
     kG: LinearGravityFeedforward,
-    kV: VelocityFeedforward<Meter>
+    kV: VelocityFeedforward<Meter, Volt>
   ) : this(kS, kG, kV, 0.0.volts.perMeterPerSecondPerSecond)
 
   fun calculate(velocity: LinearVelocity, acceleration: LinearAcceleration): ElectricalPotential {

src/main/kotlin/org/team4099/lib/controller/SimpleMotorFeedforward.kt

@@ -7,72 +7,67 @@ import org.team4099.lib.units.Velocity
 import org.team4099.lib.units.base.Time
 import org.team4099.lib.units.base.inSeconds
 import org.team4099.lib.units.derived.AccelerationFeedforward
-import org.team4099.lib.units.derived.ElectricalPotential
 import org.team4099.lib.units.derived.StaticFeedforward
 import org.team4099.lib.units.derived.VelocityFeedforward
-import org.team4099.lib.units.derived.volts
 import edu.wpi.first.math.controller.SimpleMotorFeedforward as WPISimpleFeedforward
 
-class SimpleMotorFeedforward<U : UnitKey, T : UnitKey>(
-  val kS: StaticFeedforward<U>,
-  val kV: VelocityFeedforward<U, T>,
-  val kA: AccelerationFeedforward<U, T>
+class SimpleMotorFeedforward<E : UnitKey, O : UnitKey>(
+  val kS: StaticFeedforward<O>,
+  val kV: VelocityFeedforward<E, O>,
+  val kA: AccelerationFeedforward<E, O>
 ) {
   private val feedforward: WPISimpleFeedforward = WPISimpleFeedforward(kS.value, kV.value, kA.value)
 
   constructor(
-    kS: StaticFeedforward<U>,
-    kV: VelocityFeedforward<U, T>
+    kS: StaticFeedforward<O>,
+    kV: VelocityFeedforward<E, O>
   ) : this(kS, kV, AccelerationFeedforward(0.0))
 
-  fun calculate(
-    velocity: Value<Velocity<U>>,
-    acceleration: Value<Acceleration<U>>
-  ): ElectricalPotential {
-    return feedforward.calculate(velocity.value, acceleration.value).volts
+  fun calculate(velocity: Value<Velocity<E>>, acceleration: Value<Acceleration<E>>): Value<O> {
+    return Value(feedforward.calculate(velocity.value, acceleration.value))
   }
 
   fun calculate(
-    currentVelocitySetpoint: Value<Velocity<U>>,
-    nextVelocitySetpoint: Value<Velocity<U>>,
+    currentVelocitySetpoint: Value<Velocity<E>>,
+    nextVelocitySetpoint: Value<Velocity<E>>,
     dT: Time
-  ): Value<U> {
+  ): Value<O> {
     return Value(
       feedforward.calculate(
         currentVelocitySetpoint.value, nextVelocitySetpoint.value, dT.inSeconds
       )
     )
   }
 
-  fun calculate(velocity: Value<Velocity<T>>): Value<U> {
+  fun calculate(velocity: Value<Velocity<E>>): Value<O> {
     return Value(feedforward.calculate(velocity.value))
   }
 
   fun maxAchievableVelocity(
-    maxControlOutput: Value<U>,
-    acceleration: Value<Acceleration<T>>
-  ): Value<Velocity<T>> {
+    maxControlOutput: Value<O>,
+    acceleration: Value<Acceleration<E>>
+  ): Value<Velocity<E>> {
     return Value(feedforward.maxAchievableVelocity(maxControlOutput.value, acceleration.value))
   }
 
   fun minAchievableVelocity(
-    maxControlOutput: Value<U>,
-    acceleration: Value<Acceleration<T>>
-  ): Value<Velocity<T>> {
+    maxControlOutput: Value<O>,
+    acceleration: Value<Acceleration<E>>
+  ): Value<Velocity<E>> {
     return Value(feedforward.minAchievableVelocity(maxControlOutput.value, acceleration.value))
   }
 
   fun maxAchievableAcceleration(
-    maxControlOutput: Value<U>,
-    velocity: Value<Velocity<T>>
-  ): Value<Acceleration<T>> {
+    maxControlOutput: Value<O>,
+    velocity: Value<Velocity<E>>
+  ): Value<Acceleration<E>> {
     return Value(feedforward.maxAchievableAcceleration(maxControlOutput.value, velocity.value))
   }
 
   fun minAchievableAcceleration(
-    maxControlOutput: Value<U>,
-    velocity: Value<Velocity<T>>
-  ): Value<Acceleration<T>> {
+    maxControlOutput: Value<O>,
+    velocity: Value<Velocity<E>>
+  ): Value<Acceleration<E>> {
     return Value(feedforward.minAchievableAcceleration(maxControlOutput.value, velocity.value))
   }
 }

src/main/kotlin/org/team4099/lib/units/MechanismUnits.kt

@@ -68,7 +68,7 @@ interface MechanismSensor<U : UnitKey> {
     derivativeGain: DerivativeGain<Velocity<U>, Volt>,
   ): Double
 
-  fun velocityFeedforwardToRawUnits(velocityFeedforward: VelocityFeedforward<U>): Double
+  fun velocityFeedforwardToRawUnits(velocityFeedforward: VelocityFeedforward<U, Volt>): Double
 }
 
 class LinearMechanismSensor(
@@ -155,7 +155,7 @@ class LinearMechanismSensor(
   }
 
   override fun velocityFeedforwardToRawUnits(
-    velocityFeedforward: VelocityFeedforward<Meter>
+    velocityFeedforward: VelocityFeedforward<Meter, Volt>
   ): Double {
     return velocityFeedforward.value * velocityToRawUnits(1.0.meters.perSecond) /
       compensationVoltage.inVolts * fullPowerThrottle
@@ -241,7 +241,7 @@ class AngularMechanismSensor(
   }
 
   override fun velocityFeedforwardToRawUnits(
-    velocityFeedforward: VelocityFeedforward<Radian>
+    velocityFeedforward: VelocityFeedforward<Radian, Volt>
   ): Double {
     return velocityFeedforward.value * velocityToRawUnits(1.0.radians.perSecond) /
       compensationVoltage.inVolts * fullPowerThrottle

src/main/kotlin/org/team4099/lib/units/derived/Characterization.kt

@@ -10,9 +10,9 @@ import org.team4099.lib.units.base.Meter
 
 typealias StaticFeedforward<O> = Value<O>
 
-typealias VelocityFeedforward<O, E> = Value<Fraction<O, Velocity<E>>>
+typealias VelocityFeedforward<E, O> = Value<Fraction<O, Velocity<E>>>
 
-typealias AccelerationFeedforward<O, E> = Value<Fraction<O, Acceleration<E>>>
+typealias AccelerationFeedforward<E, O> = Value<Fraction<O, Acceleration<E>>>
 
 typealias AngularGravityFeedforward = Value<Fraction<Volt, Radian>>
 
@@ -24,14 +24,14 @@ val <K : UnitKey> Value<K>.perMeterPerSecondPerSecond
 val <K : UnitKey> Value<K>.perRadianPerSecondPerSecond
   get() = Value<Fraction<K, Acceleration<Radian>>>(value)
 
-val AccelerationFeedforward<Volt, Radian>.inVoltsPerRadianPerSecondPerSecond
+val AccelerationFeedforward<Radian, Volt>.inVoltsPerRadianPerSecondPerSecond
   get() = value
 
-val AccelerationFeedforward<Volt, Meter>.inVoltsPerMeterPerSecondPerSecond
+val AccelerationFeedforward<Meter, Volt>.inVoltsPerMeterPerSecondPerSecond
   get() = value
 
-val AccelerationFeedforward<Ampere, Radian>.inAmpsPerRadianPerSecondPerSecond
+val AccelerationFeedforward<Radian, Ampere>.inAmpsPerRadianPerSecondPerSecond
   get() = value
 
-val AccelerationFeedforward<Ampere, Meter>.inAmpsPerMeterPerSecondPerSecond
+val AccelerationFeedforward<Meter, Ampere>.inAmpsPerMeterPerSecondPerSecond
   get() = value