Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-29 02:21:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Joseph Eng
2024-07-27 16:28:59 -07:00
committed by GitHub
parent 03b332d3cf
commit e83a432731
9 changed files with 87 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
wpimath/src/main/native/include/frc/controller/ArmFeedforward.h
View File

@@ -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;");
}
}

4
wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
View File

@@ -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;");
}
}

38
wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
View File

@@ -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;
}
}

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

@@ -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 @@ class ArmFeedforwardTest {
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)));
}
}

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

@@ -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 @@ class ElevatorFeedforwardTest {
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)));
}
}

21
wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java
View File

@@ -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 @@ class SimpleMotorFeedforwardTest {
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)));
}
}

6
wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp
View File

@@ -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);
}

6
wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp
View File

@@ -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);
}

11
wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
View File

@@ -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