From 88f487bc3ad4d19e896f29d23604e3fb0a810142 Mon Sep 17 00:00:00 2001
From: Saraansh Wadkar <saraanshwadkar@gmail.com>
Date: Sun, 8 Jan 2023 12:20:59 -0500
Subject: [PATCH] make arm ff and elevator ff compatible w generic ff update

---
 .../team4099/lib/controller/ArmFeedforward.kt | 11 ++--
 .../lib/controller/ElevatorFeedforward.kt     | 11 ++--
 .../lib/controller/SimpleMotorFeedforward.kt  | 53 +++++++++----------
 .../org/team4099/lib/units/MechanismUnits.kt  |  6 +--
 .../lib/units/derived/Characterization.kt     | 12 ++---
 5 files changed, 45 insertions(+), 48 deletions(-)

diff --git a/src/main/kotlin/org/team4099/lib/controller/ArmFeedforward.kt b/src/main/kotlin/org/team4099/lib/controller/ArmFeedforward.kt
index b6cf2a3..2c5fee5 100644
--- a/src/main/kotlin/org/team4099/lib/controller/ArmFeedforward.kt
+++ b/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(
diff --git a/src/main/kotlin/org/team4099/lib/controller/ElevatorFeedforward.kt b/src/main/kotlin/org/team4099/lib/controller/ElevatorFeedforward.kt
index 7b0abd7..13ea012 100644
--- a/src/main/kotlin/org/team4099/lib/controller/ElevatorFeedforward.kt
+++ b/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,10 +21,10 @@ 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(
@@ -31,9 +32,9 @@ class ElevatorFeedforward(
     )
 
   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 {
diff --git a/src/main/kotlin/org/team4099/lib/controller/SimpleMotorFeedforward.kt b/src/main/kotlin/org/team4099/lib/controller/SimpleMotorFeedforward.kt
index df247ed..688c540 100644
--- a/src/main/kotlin/org/team4099/lib/controller/SimpleMotorFeedforward.kt
+++ b/src/main/kotlin/org/team4099/lib/controller/SimpleMotorFeedforward.kt
@@ -7,36 +7,31 @@ 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
@@ -44,35 +39,35 @@ class SimpleMotorFeedforward<U : UnitKey, T : UnitKey>(
     )
   }
 
-  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))
   }
 }
diff --git a/src/main/kotlin/org/team4099/lib/units/MechanismUnits.kt b/src/main/kotlin/org/team4099/lib/units/MechanismUnits.kt
index f00b4f3..595d465 100644
--- a/src/main/kotlin/org/team4099/lib/units/MechanismUnits.kt
+++ b/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
diff --git a/src/main/kotlin/org/team4099/lib/units/derived/Characterization.kt b/src/main/kotlin/org/team4099/lib/units/derived/Characterization.kt
index 82b8a1c..c4a18fe 100644
--- a/src/main/kotlin/org/team4099/lib/units/derived/Characterization.kt
+++ b/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