[wpimath] fully discretized ElevatorFF and ArmFF

wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java

@@ -188,72 +188,79 @@ public Voltage calculate(LinearVelocity currentVelocity) {
    */
   public Voltage calculate(LinearVelocity currentVelocity, LinearVelocity nextVelocity) {
     // For an elevator with the model
-    // dx/dt = −kᵥ/kₐ x + 1/kₐ u - kg/kₐ - kₛ/kₐ sgn(x),
-    //
+
+    //  dx/dt = −kᵥ/kₐ x + 1/kₐ u - kg/kₐ - kₛ/kₐ sgn(x),
+
     // where
-    // A = −kᵥ/kₐ
-    // B = 1/kₐ
-    // c = -(kg/kₐ + kₛ/kₐ sgn(x))
-    // A_d = eᴬᵀ
-    // B_d = A⁻¹(eᴬᵀ - I)B
-    // dx/dt = Ax + Bu + c
-    //
+
+    //  A = −kᵥ/kₐ
+    //  B = 1/kₐ
+    //  c = -(kg/kₐ + kₛ/kₐ sgn(x))
+    //  A_d = eᴬᵀ
+    //  B_d = A⁻¹(eᴬᵀ - I)B
+    //  dx/dt = Ax + Bu + c
+
     // Discretize the affine model.
-    // dx/dt = Ax + Bu + c
-    // dx/dt = Ax + B(u + B⁺c)
-    // xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ)
-    // xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ)
-    // xₖ₊₁ = A_d xₖ + B_d uₖ + B_d B⁺cₖ
-    //
+
+    //  dx/dt = Ax + Bu + c
+    //  dx/dt = Ax + B(u + B⁺c)
+    //  xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ)
+    //  xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ)
+    //  xₖ₊₁ = A_d xₖ + B_d uₖ + B_d B⁺cₖ
+
     // Solve for uₖ.
-    // B_d uₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ
-    // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ)
-    // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ
-    //
+
+    //  B_d uₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ
+    //  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ)
+    //  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ
+
     // Substitute in B assuming sgn(x) is a constant for the duration of the step.
-    // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − kₐ(-(kg/kₐ + kₛ/kₐ sgn(x)))
-    // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kₐ(kg/kₐ + kₛ/kₐ sgn(x))
-    // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kg + kₛ sgn(x)
+
+    //  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − kₐ(-(kg/kₐ + kₛ/kₐ sgn(x)))
+    //  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kₐ(kg/kₐ + kₛ/kₐ sgn(x))
+    //  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kg + kₛ sgn(x)
     if (ka == 0.0) {
       // Simplify the model when kₐ = 0.
-      //
+
       // Simplify A.
-      // A = −kᵥ/kₐ
+
+      //  A = −kᵥ/kₐ
       // As kₐ approaches zero, A approaches -∞.
-      // A = −∞
-      //
+      //  A = −∞
+
       // Simplify A_d.
-      //
-      // A_d = eᴬᵀ
-      // A_d = exp(−∞)
-      // A_d = 0
-      //
+
+      //  A_d = eᴬᵀ
+      //  A_d = exp(−∞)
+      //  A_d = 0
+
       // Simplify B_d.
-      // B_d = A⁻¹(eᴬᵀ - I)B
-      // B_d = A⁻¹((0) - I)B
-      // B_d = A⁻¹(-I)B
-      // B_d = -A⁻¹B
-      // B_d = -(−kᵥ/kₐ)⁻¹(1/kₐ)
-      // B_d = (kᵥ/kₐ)⁻¹(1/kₐ)
-      // B_d = kₐ/kᵥ(1/kₐ)
-      // B_d = 1/kᵥ
-      //
+
+      //  B_d = A⁻¹(eᴬᵀ - I)B
+      //  B_d = A⁻¹((0) - I)B
+      //  B_d = A⁻¹(-I)B
+      //  B_d = -A⁻¹B
+      //  B_d = -(−kᵥ/kₐ)⁻¹(1/kₐ)
+      //  B_d = (kᵥ/kₐ)⁻¹(1/kₐ)
+      //  B_d = kₐ/kᵥ(1/kₐ)
+      //  B_d = 1/kᵥ
+
       // Substitute these into the feedforward equation.
-      //
-      // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kg + kₛ sgn(x)
-      // uₖ = (1/kᵥ)⁺(xₖ₊₁ − (0) xₖ) + kg + kₛ sgn(x)
-      // uₖ = kᵥxₖ₊₁ + kg + kₛ sgn(x)
+
+      //  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kg + kₛ sgn(x)
+      //  uₖ = (1/kᵥ)⁺(xₖ₊₁ − (0) xₖ) + kg + kₛ sgn(x)
+      //  uₖ = kᵥxₖ₊₁ + kg + kₛ sgn(x)
       output.mut_replace(
           kg
               + ks * Math.signum(nextVelocity.in(MetersPerSecond))
               + kv * nextVelocity.in(MetersPerSecond),
           Volts);
       return output;
     } else {
-      // A = −kᵥ/kₐ
-      // B = 1/kₐ
-      // A_d = eᴬᵀ
-      // B_d = A⁻¹(eᴬᵀ - I)B
+      //  A = −kᵥ/kₐ
+      //  B = 1/kₐ
+      //  A_d = eᴬᵀ
+      //  B_d = A⁻¹(eᴬᵀ - I)B
       double A = -kv / ka;
       double B = 1.0 / ka;
       double A_d = Math.exp(A * m_dt);

wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java

@@ -188,67 +188,78 @@ public <U extends Unit> Voltage calculate(
       Measure<? extends PerUnit<U, TimeUnit>> currentVelocity,
       Measure<? extends PerUnit<U, TimeUnit>> nextVelocity) {
     // For a simple DC motor with the model
-    //   dx/dt = −kᵥ/kₐ x + 1/kₐ u - kₛ/kₐ sgn(x),
-    //
+
+    //  dx/dt = −kᵥ/kₐ x + 1/kₐ u - kₛ/kₐ sgn(x),
+
     // where
-    //   A = −kᵥ/kₐ
-    //   B = 1/kₐ
-    //   c = -kₛ/kₐ sgn(x))
-    //   A_d = eᴬᵀ
-    //   B_d = A⁻¹(eᴬᵀ - I)B
-    //   dx/dt = Ax + Bu + c
-    //
+
+    //  A = −kᵥ/kₐ
+    //  B = 1/kₐ
+    //  c = -kₛ/kₐ sgn(x))
+    //  A_d = eᴬᵀ
+    //  B_d = A⁻¹(eᴬᵀ - I)B
+    //  dx/dt = Ax + Bu + c
+
     // Discretize the affine model.
-    //   dx/dt = Ax + Bu + c
-    //   dx/dt = Ax + B(u + B⁺c)
-    //   xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ)
-    //   xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ)
-    //   xₖ₊₁ = A_d xₖ + B_d uₖ + B_d B⁺cₖ
-    //
+
+    //  dx/dt = Ax + Bu + c
+    //  dx/dt = Ax + B(u + B⁺c)
+    //  xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ)
+    //  xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ)
+    //  xₖ₊₁ = A_d xₖ + B_d uₖ + B_d B⁺cₖ
+
     // Solve for uₖ.
-    //   B_d uₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ
-    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ)
-    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ
-    //
+
+    //  B_d uₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ
+    //  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ)
+    //  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ
+
     // Substitute in B assuming sgn(x) is a constant for the duration of the step.
-    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − kₐ(-(kₛ/kₐ sgn(x)))
-    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kₐ(kₛ/kₐ sgn(x))
-    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kₛ sgn(x)
+
+    //  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − kₐ(-(kₛ/kₐ sgn(x)))
+    //  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kₐ(kₛ/kₐ sgn(x))
+    //  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kₛ sgn(x)
     if (ka == 0.0) {
       // Simplify the model when kₐ = 0.
-      //
+
       // Simplify A.
-      //   A = −kᵥ/kₐ
-      //   As kₐ approaches zero, A approaches -∞.
-      //   A = −∞
-      //
+
+      //  A = −kᵥ/kₐ
+
+      // As kₐ approaches zero, A approaches -∞.
+
+      //  A = −∞
+
       // Simplify A_d.
-      //   A_d = eᴬᵀ
-      //   A_d = exp(−∞)
-      //   A_d = 0
-      //
+
+      //  A_d = eᴬᵀ
+      //  A_d = exp(−∞)
+      //  A_d = 0
+
       // Simplify B_d.
-      //   B_d = A⁻¹(eᴬᵀ - I)B
-      //   B_d = A⁻¹((0) - I)B
-      //   B_d = A⁻¹(-I)B
-      //   B_d = -A⁻¹B
-      //   B_d = -(−kᵥ/kₐ)⁻¹(1/kₐ)
-      //   B_d = (kᵥ/kₐ)⁻¹(1/kₐ)
-      //   B_d = kₐ/kᵥ(1/kₐ)
-      //   B_d = 1/kᵥ
-      //
+
+      //  B_d = A⁻¹(eᴬᵀ - I)B
+      //  B_d = A⁻¹((0) - I)B
+      //  B_d = A⁻¹(-I)B
+      //  B_d = -A⁻¹B
+      //  B_d = -(−kᵥ/kₐ)⁻¹(1/kₐ)
+      //  B_d = (kᵥ/kₐ)⁻¹(1/kₐ)
+      //  B_d = kₐ/kᵥ(1/kₐ)
+      //  B_d = 1/kᵥ
+
       // Substitute these into the feedforward equation.
-      //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kₛ sgn(x)
-      //   uₖ = (1/kᵥ)⁺(xₖ₊₁ − (0) xₖ) + kₛ sgn(x)
-      //   uₖ = kᵥxₖ₊₁  + kₛ sgn(x)
+
+      //  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kₛ sgn(x)
+      //  uₖ = (1/kᵥ)⁺(xₖ₊₁ − (0) xₖ) + kₛ sgn(x)
+      //  uₖ = kᵥxₖ₊₁  + kₛ sgn(x)
       output.mut_replace(
           ks * Math.signum(nextVelocity.magnitude()) + kv * nextVelocity.magnitude(), Volts);
       return output;
     } else {
-      //   A = −kᵥ/kₐ
-      //   B = 1/kₐ
-      //   A_d = eᴬᵀ
-      //   B_d = A⁻¹(eᴬᵀ - I)B
+      //  A = −kᵥ/kₐ
+      //  B = 1/kₐ
+      //  A_d = eᴬᵀ
+      //  B_d = A⁻¹(eᴬᵀ - I)B
       double A = -kv / ka;
       double B = 1.0 / ka;
       double A_d = Math.exp(A * m_dt);

wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h

@@ -94,26 +94,26 @@ class ElevatorFeedforward {
                           units::unit_t<Velocity> nextVelocity,
                           units::second_t dt) const {
     // Discretize the affine model.
-    //
+
     //  dx/dt = Ax + Bu + c
     //  dx/dt = Ax + B(u + B⁺c)
     //  xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ)
     //  xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ)
     //  xₖ₊₁ = A_d xₖ + B_duₖ + B_d B⁺cₖ
-    //
+
     // Solve for uₖ.
-    //
+
     //  B_duₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ
     //  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ)
     //  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ
-    //
+
     // For an elevator with the model
-    //
+
     //  dx/dt = -Kv/Ka x + 1/Ka u - Kg/Ka - Ks/Ka sgn(x),
-    // 
+
     // A = -Kv/Ka, B = 1/Ka, and c = -(Kg/Ka + Ks/Ka sgn(x)). Substitute in B
     // assuming sgn(x) is a constant for the duration of the step.
-    //
+
     //  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − Ka(-(Kg/Ka + Ks/Ka sgn(x)))
     //  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Ka(Kg/Ka + Ks/Ka sgn(x))
     //  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Kg + Ks sgn(x)
@@ -153,57 +153,54 @@ class ElevatorFeedforward {
       units::unit_t<Velocity> currentVelocity,
       units::unit_t<Velocity> nextVelocity) const {
     // For an elevator with the model
-    //
+
     //  dx/dt = −kᵥ/kₐ x + 1/kₐ u - kg/kₐ - kₛ/kₐ sgn(x),
-    //
+
     // where
-    //
+
     //  A = −kᵥ/kₐ
     //  B = 1/kₐ
     //  c = -(kg/kₐ + kₛ/kₐ sgn(x))
     //  A_d = eᴬᵀ
     //  B_d = A⁻¹(eᴬᵀ - I)B
     //  dx/dt = Ax + Bu + c
-    //
+
     // Discretize the affine model.
-    //
+
     //  dx/dt = Ax + Bu + c
     //  dx/dt = Ax + B(u + B⁺c)
     //  xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ)
     //  xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ)
     //  xₖ₊₁ = A_d xₖ + B_d uₖ + B_d B⁺cₖ
-    //
+
     // Solve for uₖ.
-    //
+
     //  B_d uₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ
     //  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ)
     //  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ
-    //
-    // Substitute in B assuming sgn(x) is a constant for the duration of the
-    // step.
-    //
+
+    // Substitute in B assuming sgn(x) is a constant for the duration of the step.
+
     //  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − kₐ(-(kg/kₐ + kₛ/kₐ sgn(x)))
     //  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kₐ(kg/kₐ + kₛ/kₐ sgn(x))
     //  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kg + kₛ sgn(x)
     if (kA == decltype(kA)(0)) {
       // Simplify the model when kₐ = 0.
-      //
+
       // Simplify A.
-      // 
+
       //  A = −kᵥ/kₐ
-      //
       // As kₐ approaches zero, A approaches -∞.
-      //
       //  A = −∞
-      //
+
       // Simplify A_d.
-      //
+
       //  A_d = eᴬᵀ
-      //  A_d = std::exp(−∞)
+      //  A_d = exp(−∞)
       //  A_d = 0
-      //
+
       // Simplify B_d.
-      //
+
       //  B_d = A⁻¹(eᴬᵀ - I)B
       //  B_d = A⁻¹((0) - I)B
       //  B_d = A⁻¹(-I)B
@@ -212,12 +209,12 @@ class ElevatorFeedforward {
       //  B_d = (kᵥ/kₐ)⁻¹(1/kₐ)
       //  B_d = kₐ/kᵥ(1/kₐ)
       //  B_d = 1/kᵥ
-      //
+
       // Substitute these into the feedforward equation.
-      //
+
       //  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kg + kₛ sgn(x)
       //  uₖ = (1/kᵥ)⁺(xₖ₊₁ − (0) xₖ) + kg + kₛ sgn(x)
-      //  uₖ = kᵥxₖ₊₁  + kg + kₛ sgn(x)
+      //  uₖ = kᵥxₖ₊₁ + kg + kₛ sgn(x)
       return kS * wpi::sgn(nextVelocity) + kG + kV * nextVelocity;
     } else {
       //  A = −kᵥ/kₐ