[wpimath] Fully discretized ElevatorFF and ArmFF (#7024)

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java

@@ -4,6 +4,9 @@
 
 package edu.wpi.first.math.controller;
 
+import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.Volts;
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertThrows;
@@ -68,19 +71,23 @@ class ArmFeedforwardTest {
    */
   private void calculateAndSimulate(
       double currentAngle, double currentVelocity, double nextVelocity, double dt) {
-    final double input = m_armFF.calculate(currentAngle, currentVelocity, nextVelocity, dt);
+    final double input =
+        m_armFF
+            .calculate(
+                Radians.of(currentAngle),
+                RadiansPerSecond.of(currentVelocity),
+                RadiansPerSecond.of(nextVelocity))
+            .in(Volts);
     assertEquals(nextVelocity, simulate(currentAngle, currentVelocity, input, dt).get(1, 0), 1e-12);
   }
 
   @Test
   void testCalculate() {
     // calculate(angle, angular velocity)
-    assertEquals(0.5, m_armFF.calculate(Math.PI / 3, 0), 0.002);
-    assertEquals(2.5, m_armFF.calculate(Math.PI / 3, 1), 0.002);
-
-    // calculate(angle, angular velocity, angular acceleration)
-    assertEquals(6.5, m_armFF.calculate(Math.PI / 3, 1, 2), 0.002);
-    assertEquals(2.5, m_armFF.calculate(Math.PI / 3, -1, 2), 0.002);
+    assertEquals(
+        0.5, m_armFF.calculate(Radians.of(Math.PI / 3), RadiansPerSecond.of(0)).in(Volts), 0.002);
+    assertEquals(
+        2.5, m_armFF.calculate(Radians.of(Math.PI / 3), RadiansPerSecond.of(1)).in(Volts), 0.002);
 
     // calculate(currentAngle, currentVelocity, nextAngle, dt)
     calculateAndSimulate(Math.PI / 3, 1.0, 1.05, 0.020);

wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java

@@ -4,6 +4,8 @@
 
 package edu.wpi.first.math.controller;
 
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.Volts;
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertThrows;
@@ -24,10 +26,8 @@ class ElevatorFeedforwardTest {
 
   @Test
   void testCalculate() {
-    assertEquals(1, m_elevatorFF.calculate(0), 0.002);
-    assertEquals(4.5, m_elevatorFF.calculate(2), 0.002);
-    assertEquals(6.5, m_elevatorFF.calculate(2, 1), 0.002);
-    assertEquals(-0.5, m_elevatorFF.calculate(-2, 1), 0.002);
+    assertEquals(1, m_elevatorFF.calculate(MetersPerSecond.of(0)).in(Volts), 0.002);
+    assertEquals(4.5, m_elevatorFF.calculate(MetersPerSecond.of(2)).in(Volts), 0.002);
 
     var A = MatBuilder.fill(Nat.N1(), Nat.N1(), -kv / ka);
     var B = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0 / ka);
@@ -38,7 +38,7 @@ class ElevatorFeedforwardTest {
     var nextR = VecBuilder.fill(3.0);
     assertEquals(
         plantInversion.calculate(r, nextR).get(0, 0) + ks + kg,
-        m_elevatorFF.calculate(2.0, 3.0, dt),
+        m_elevatorFF.calculate(MetersPerSecond.of(2.0), MetersPerSecond.of(3.0)).in(Volts),
         0.002);
   }
 

wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp

@@ -62,7 +62,7 @@ void CalculateAndSimulate(const frc::ArmFeedforward& armFF,
                           units::radians_per_second_t currentVelocity,
                           units::radians_per_second_t nextVelocity,
                           units::second_t dt) {
-  auto input = armFF.Calculate(currentAngle, currentVelocity, nextVelocity, dt);
+  auto input = armFF.Calculate(currentAngle, currentVelocity, nextVelocity);
   EXPECT_NEAR(nextVelocity.value(),
               Simulate(currentAngle, currentVelocity, input, dt)(1), 1e-12);
 }
@@ -80,17 +80,6 @@ TEST(ArmFeedforwardTest, Calculate) {
       armFF.Calculate(std::numbers::pi / 3 * 1_rad, 1_rad_per_s).value(), 2.5,
       0.002);
 
-  // Calculate(angle, angular velocity, angular acceleration)
-  EXPECT_NEAR(
-      armFF.Calculate(std::numbers::pi / 3 * 1_rad, 1_rad_per_s, 2_rad_per_s_sq)
-          .value(),
-      6.5, 0.002);
-  EXPECT_NEAR(
-      armFF
-          .Calculate(std::numbers::pi / 3 * 1_rad, -1_rad_per_s, 2_rad_per_s_sq)
-          .value(),
-      2.5, 0.002);
-
   // Calculate(currentAngle, currentVelocity, nextAngle, dt)
   CalculateAndSimulate(armFF, std::numbers::pi / 3 * 1_rad, 1_rad_per_s,
                        1.05_rad_per_s, 20_ms);

wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp

@@ -23,10 +23,6 @@ TEST(ElevatorFeedforwardTest, Calculate) {
 
   EXPECT_NEAR(elevatorFF.Calculate(0_m / 1_s).value(), Kg.value(), 0.002);
   EXPECT_NEAR(elevatorFF.Calculate(2_m / 1_s).value(), 4.5, 0.002);
-  EXPECT_NEAR(elevatorFF.Calculate(2_m / 1_s, 1_m / 1_s / 1_s).value(), 6.5,
-              0.002);
-  EXPECT_NEAR(elevatorFF.Calculate(-2_m / 1_s, 1_m / 1_s / 1_s).value(), -0.5,
-              0.002);
 
   frc::Matrixd<1, 1> A{-Kv.value() / Ka.value()};
   frc::Matrixd<1, 1> B{1.0 / Ka.value()};
@@ -36,7 +32,7 @@ TEST(ElevatorFeedforwardTest, Calculate) {
   frc::Vectord<1> r{2.0};
   frc::Vectord<1> nextR{3.0};
   EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks.value() + Kg.value(),
-              elevatorFF.Calculate(2_mps, 3_mps, dt).value(), 0.002);
+              elevatorFF.Calculate(2_mps, 3_mps).value(), 0.002);
 }
 
 TEST(ElevatorFeedforwardTest, AchievableVelocity) {