[wpimath] Fix C++ feedforward constructors and add tests (#6873)

wpilibsuite · Jul 27, 2024 · e83a432 · e83a432
1 parent 03b332d
commit e83a432
Show file tree

Hide file tree

Showing 9 changed files with 87 additions and 28 deletions.
diff --git a/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h
@@ -45,13 +45,13 @@ class WPILIB_DLLEXPORT ArmFeedforward {
     if (kV.value() < 0) {
       wpi::math::MathSharedStore::ReportError(
           "kV must be a non-negative number, got {}!", kV.value());
-      kV = units::unit_t<kv_unit>{0};
+      this->kV = units::unit_t<kv_unit>{0};
       wpi::math::MathSharedStore::ReportWarning("kV defaulted to 0.");
     }
     if (kA.value() < 0) {
       wpi::math::MathSharedStore::ReportError(
           "kA must be a non-negative number, got {}!", kA.value());
-      kA = units::unit_t<ka_unit>{0};
+      this->kA = units::unit_t<ka_unit>{0};
       wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0;");
     }
   }

diff --git a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
@@ -45,13 +45,13 @@ class ElevatorFeedforward {
     if (kV.value() < 0) {
       wpi::math::MathSharedStore::ReportError(
           "kV must be a non-negative number, got {}!", kV.value());
-      kV = units::unit_t<kv_unit>{0};
+      this->kV = units::unit_t<kv_unit>{0};
       wpi::math::MathSharedStore::ReportWarning("kV defaulted to 0.");
     }
     if (kA.value() < 0) {
       wpi::math::MathSharedStore::ReportError(
           "kA must be a non-negative number, got {}!", kA.value());
-      kA = units::unit_t<ka_unit>{0};
+      this->kA = units::unit_t<ka_unit>{0};
       wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0;");
     }
   }

diff --git a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
@@ -40,34 +40,24 @@ class SimpleMotorFeedforward {
       units::volt_t kS, units::unit_t<kv_unit> kV,
       units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0),
       units::second_t dt = 20_ms)
-      : kS(kS),
-        kV([&] {
-          if (kV.value() < 0) {
-            wpi::math::MathSharedStore::ReportError(
-                "kV must be a non-negative number, got {}!", kV.value());
-            wpi::math::MathSharedStore::ReportWarning("kV defaulted to 0.");
-            return units::unit_t<kv_unit>{0};
-          } else {
-            return kV;
-          }
-        }()),
-        kA([&] {
-          if (kA.value() < 0) {
-            wpi::math::MathSharedStore::ReportError(
-                "kA must be a non-negative number, got {}!", kA.value());
-            wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0.");
-            return units::unit_t<ka_unit>{0};
-          } else {
-            return kA;
-          }
-        }()) {
+      : kS(kS), kV(kV), kA(kA), m_dt(dt) {
+    if (kV.value() < 0) {
+      wpi::math::MathSharedStore::ReportError(
+          "kV must be a non-negative number, got {}!", kV.value());
+      this->kV = units::unit_t<kv_unit>{0};
+      wpi::math::MathSharedStore::ReportWarning("kV defaulted to 0.");
+    }
+    if (kA.value() < 0) {
+      wpi::math::MathSharedStore::ReportError(
+          "kA must be a non-negative number, got {}!", kA.value());
+      this->kA = units::unit_t<ka_unit>{0};
+      wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0.");
+    }
     if (dt <= 0_ms) {
       wpi::math::MathSharedStore::ReportError(
           "period must be a positive number, got {}!", dt.value());
-      m_dt = 20_ms;
+      this->m_dt = 20_ms;
       wpi::math::MathSharedStore::ReportWarning("period defaulted to 20 ms.");
-    } else {
-      m_dt = dt;
     }
   }
 

diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java
@@ -4,7 +4,9 @@
 
 package edu.wpi.first.math.controller;
 
+import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
 
 import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Matrix;
@@ -100,4 +102,14 @@ void testAcheviableAcceleration() {
     assertEquals(-7.25, m_armFF.minAchievableAcceleration(12, Math.PI / 3, 1), 0.002);
     assertEquals(-5.25, m_armFF.minAchievableAcceleration(12, Math.PI / 3, -1), 0.002);
   }
+
+  @Test
+  void testNegativeGains() {
+    assertAll(
+        () ->
+            assertThrows(IllegalArgumentException.class, () -> new ArmFeedforward(ks, kg, -kv, ka)),
+        () ->
+            assertThrows(
+                IllegalArgumentException.class, () -> new ArmFeedforward(ks, kg, kv, -ka)));
+  }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java
@@ -4,7 +4,9 @@
 
 package edu.wpi.first.math.controller;
 
+import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
 
 import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Nat;
@@ -53,4 +55,15 @@ void testAcheviableAcceleration() {
     assertEquals(-8.25, m_elevatorFF.minAchievableAcceleration(12, 2), 0.002);
     assertEquals(-4.75, m_elevatorFF.minAchievableAcceleration(12, -2), 0.002);
   }
+
+  @Test
+  void testNegativeGains() {
+    assertAll(
+        () ->
+            assertThrows(
+                IllegalArgumentException.class, () -> new ElevatorFeedforward(ks, kg, -kv, ka)),
+        () ->
+            assertThrows(
+                IllegalArgumentException.class, () -> new ElevatorFeedforward(ks, kg, kv, -ka)));
+  }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java
@@ -5,7 +5,9 @@
 package edu.wpi.first.math.controller;
 
 import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
 
 import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Nat;
@@ -49,4 +51,23 @@ void testCalculate() {
         simpleMotor.calculate(currentVelocity, nextVelocity).magnitude(),
         2.0);
   }
+
+  @Test
+  void testNegativeGains() {
+    double Ks = 0.5;
+    double Kv = 3.5;
+    double Ka = 3.5;
+    double dt = 0.02;
+
+    assertAll(
+        () ->
+            assertThrows(
+                IllegalArgumentException.class, () -> new SimpleMotorFeedforward(Ks, -Kv, Ka, dt)),
+        () ->
+            assertThrows(
+                IllegalArgumentException.class, () -> new SimpleMotorFeedforward(Ks, Kv, -Ka, dt)),
+        () ->
+            assertThrows(
+                IllegalArgumentException.class, () -> new SimpleMotorFeedforward(Ks, Kv, Ka, 0)));
+  }
 }
diff --git a/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp
@@ -139,3 +139,9 @@ TEST(ArmFeedforwardTest, AchievableAcceleration) {
                   .value(),
               -5.25, 0.002);
 }
+
+TEST(ArmFeedforwardTest, NegativeGains) {
+  frc::ArmFeedforward armFF{Ks, Kg, -Kv, -Ka};
+  EXPECT_EQ(armFF.GetKv().value(), 0);
+  EXPECT_EQ(armFF.GetKa().value(), 0);
+}
diff --git a/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp
@@ -58,3 +58,9 @@ TEST(ElevatorFeedforwardTest, AchievableAcceleration) {
   EXPECT_NEAR(elevatorFF.MinAchievableAcceleration(12_V, -2_m / 1_s).value(),
               -4.75, 0.002);
 }
+
+TEST(ElevatorFeedforwardTest, NegativeGains) {
+  frc::ElevatorFeedforward elevatorFF{Ks, Kg, -Kv, -Ka};
+  EXPECT_EQ(elevatorFF.GetKv().value(), 0);
+  EXPECT_EQ(elevatorFF.GetKa().value(), 0);
+}
diff --git a/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
@@ -42,4 +42,15 @@ TEST(SimpleMotorFeedforwardTest, Calculate) {
               simpleMotor.Calculate(2_mps, 3_mps).value(), 2.0);
 }
 
+TEST(SimpleMotorFeedforwardTest, NegativeGains) {
+  constexpr auto Ks = 0.5_V;
+  constexpr auto Kv = -3_V / 1_mps;
+  constexpr auto Ka = -0.6_V / 1_mps_sq;
+  constexpr units::second_t dt = 0_ms;
+  frc::SimpleMotorFeedforward<units::meter> simpleMotor{Ks, Kv, Ka, dt};
+  EXPECT_EQ(simpleMotor.GetKv().value(), 0);
+  EXPECT_EQ(simpleMotor.GetKa().value(), 0);
+  EXPECT_EQ(simpleMotor.GetDt().value(), 0.02);
+}
+
 }  // namespace frc