[wpimath] Make feedforward classes throw exceptions for negative Kv or Ka (#6084)

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

@@ -11,6 +11,7 @@
 #include "units/angular_velocity.h"
 #include "units/math.h"
 #include "units/voltage.h"
+#include "wpimath/MathShared.h"
 
 namespace frc {
 /**
@@ -29,8 +30,6 @@ class WPILIB_DLLEXPORT ArmFeedforward {
   using ka_unit =
       units::compound_unit<units::volts, units::inverse<Acceleration>>;
 
-  constexpr ArmFeedforward() = default;
-
   /**
    * Creates a new ArmFeedforward with the specified gains.
    *
@@ -42,7 +41,20 @@ class WPILIB_DLLEXPORT ArmFeedforward {
   constexpr ArmFeedforward(
       units::volt_t kS, units::volt_t kG, units::unit_t<kv_unit> kV,
       units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
-      : kS(kS), kG(kG), kV(kV), kA(kA) {}
+      : kS(kS), kG(kG), kV(kV), kA(kA) {
+    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};
+      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};
+      wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0;");
+    }
+  }
 
   /**
    * Calculates the feedforward from the gains and setpoints.
@@ -163,10 +175,10 @@ class WPILIB_DLLEXPORT ArmFeedforward {
     return MaxAchievableAcceleration(-maxVoltage, angle, velocity);
   }
 
-  units::volt_t kS{0};
-  units::volt_t kG{0};
-  units::unit_t<kv_unit> kV{0};
-  units::unit_t<ka_unit> kA{0};
+  const units::volt_t kS;
+  const units::volt_t kG;
+  const units::unit_t<kv_unit> kV;
+  const units::unit_t<ka_unit> kA;
 };
 }  // namespace frc
 

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

@@ -12,6 +12,7 @@
 #include "units/length.h"
 #include "units/time.h"
 #include "units/voltage.h"
+#include "wpimath/MathShared.h"
 
 namespace frc {
 /**
@@ -29,8 +30,6 @@ class ElevatorFeedforward {
   using ka_unit =
       units::compound_unit<units::volts, units::inverse<Acceleration>>;
 
-  ElevatorFeedforward() = default;
-
   /**
    * Creates a new ElevatorFeedforward with the specified gains.
    *
@@ -42,7 +41,20 @@ class ElevatorFeedforward {
   constexpr ElevatorFeedforward(
       units::volt_t kS, units::volt_t kG, units::unit_t<kv_unit> kV,
       units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
-      : kS(kS), kG(kG), kV(kV), kA(kA) {}
+      : kS(kS), kG(kG), kV(kV), kA(kA) {
+    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};
+      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};
+      wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0;");
+    }
+  }
 
   /**
    * Calculates the feedforward from the gains and setpoints.
@@ -170,10 +182,10 @@ class ElevatorFeedforward {
     return MaxAchievableAcceleration(-maxVoltage, velocity);
   }
 
-  units::volt_t kS{0};
-  units::volt_t kG{0};
-  units::unit_t<kv_unit> kV{0};
-  units::unit_t<ka_unit> kA{0};
+  const units::volt_t kS;
+  const units::volt_t kG;
+  const units::unit_t<kv_unit> kV;
+  const units::unit_t<ka_unit> kA;
 };
 }  // namespace frc
 

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

@@ -11,6 +11,7 @@
 #include "frc/system/plant/LinearSystemId.h"
 #include "units/time.h"
 #include "units/voltage.h"
+#include "wpimath/MathShared.h"
 
 namespace frc {
 /**
@@ -28,8 +29,6 @@ class SimpleMotorFeedforward {
   using ka_unit =
       units::compound_unit<units::volts, units::inverse<Acceleration>>;
 
-  constexpr SimpleMotorFeedforward() = default;
-
   /**
    * Creates a new SimpleMotorFeedforward with the specified gains.
    *
@@ -40,7 +39,20 @@ class SimpleMotorFeedforward {
   constexpr SimpleMotorFeedforward(
       units::volt_t kS, units::unit_t<kv_unit> kV,
       units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
-      : kS(kS), kV(kV), kA(kA) {}
+      : kS(kS), kV(kV), kA(kA) {
+    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};
+      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};
+      wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0;");
+    }
+  }
 
   /**
    * Calculates the feedforward from the gains and setpoints.
@@ -148,8 +160,8 @@ class SimpleMotorFeedforward {
     return MaxAchievableAcceleration(-maxVoltage, velocity);
   }
 
-  units::volt_t kS{0};
-  units::unit_t<kv_unit> kV{0};
-  units::unit_t<ka_unit> kA{0};
+  const units::volt_t kS;
+  const units::unit_t<kv_unit> kV;
+  const units::unit_t<ka_unit> kA;
 };
 }  // namespace frc