From e83a432731a8ffd4cf5509fe0493bdc8b5a6140a Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Sat, 27 Jul 2024 16:28:59 -0700 Subject: [PATCH] [wpimath] Fix C++ feedforward constructors and add tests (#6873) --- .../include/frc/controller/ArmFeedforward.h | 4 +- .../frc/controller/ElevatorFeedforward.h | 4 +- .../frc/controller/SimpleMotorFeedforward.h | 38 +++++++------------ .../math/controller/ArmFeedforwardTest.java | 12 ++++++ .../controller/ElevatorFeedforwardTest.java | 13 +++++++ .../SimpleMotorFeedforwardTest.java | 21 ++++++++++ .../cpp/controller/ArmFeedforwardTest.cpp | 6 +++ .../controller/ElevatorFeedforwardTest.cpp | 6 +++ .../controller/SimpleMotorFeedforwardTest.cpp | 11 ++++++ 9 files changed, 87 insertions(+), 28 deletions(-) diff --git a/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h index 2e986a9baa7..ab5ba920fcb 100644 --- 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{0}; + this->kV = units::unit_t{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{0}; + this->kA = units::unit_t{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 index d9edde77961..2180ab0b7b8 100644 --- 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{0}; + this->kV = units::unit_t{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{0}; + this->kA = units::unit_t{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 index b6ed239e78e..5c400501e4e 100644 --- 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, units::unit_t kA = units::unit_t(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{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{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{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{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 index 16d0269d0ef..b553fb0da37 100644 --- 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 index 14993029504..90344937b3e 100644 --- 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 index 9d501edf92a..0f4edf75d4c 100644 --- 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 index 615f10ae89e..890c462cc1d 100644 --- 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 index a2db70dfd51..c6af43a1e73 100644 --- 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 index 35d35b8f365..7fc3d65b8e5 100644 --- 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 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