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

SCRIPT namespace replacements

Browse Source
This commit is contained in:
PJ Reiniger
2025-11-07 20:00:05 -05:00
committed by Peter Johnson
parent ae6c043632
commit 9aca8e0fd6
2622 changed files with 22275 additions and 22275 deletions
Download Patch File Download Diff File Expand all files Collapse all files

140
wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
View File

@@ -17,7 +17,7 @@
static constexpr auto kDt = 10_ms;
#define EXPECT_NEAR_UNITS(val1, val2, eps) \
EXPECT_LE(units::math::abs(val1 - val2), eps)
EXPECT_LE(wpi::units::math::abs(val1 - val2), eps)
#define EXPECT_LT_OR_NEAR_UNITS(val1, val2, eps) \
if (val1 <= val2) { \
@@ -27,12 +27,12 @@ static constexpr auto kDt = 10_ms;
}
TEST(TrapezoidProfileTest, ReachesGoal) {
frc::TrapezoidProfile<units::meter>::Constraints constraints{1.75_mps,
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{1.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{3_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{3_m, 0_mps};
wpi::math::TrapezoidProfile<wpi::units::meter>::State state;
frc::TrapezoidProfile<units::meter> profile{constraints};
wpi::math::TrapezoidProfile<wpi::units::meter> profile{constraints};
for (int i = 0; i < 450; ++i) {
state = profile.Calculate(kDt, state, goal);
}
@@ -42,19 +42,19 @@ TEST(TrapezoidProfileTest, ReachesGoal) {
// Tests that decreasing the maximum velocity in the middle when it is already
// moving faster than the new max is handled correctly
TEST(TrapezoidProfileTest, PosContinuousUnderVelChange) {
frc::TrapezoidProfile<units::meter>::Constraints constraints{1.75_mps,
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{1.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{12_m, 0_mps};
wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{12_m, 0_mps};
frc::TrapezoidProfile<units::meter> profile{constraints};
wpi::math::TrapezoidProfile<wpi::units::meter> profile{constraints};
auto state = profile.Calculate(
kDt, frc::TrapezoidProfile<units::meter>::State{}, goal);
kDt, wpi::math::TrapezoidProfile<wpi::units::meter>::State{}, goal);
auto lastPos = state.position;
for (int i = 0; i < 1600; ++i) {
if (i == 400) {
constraints.maxVelocity = 0.75_mps;
profile = frc::TrapezoidProfile<units::meter>{constraints};
profile = wpi::math::TrapezoidProfile<wpi::units::meter>{constraints};
}
state = profile.Calculate(kDt, state, goal);
@@ -76,12 +76,12 @@ TEST(TrapezoidProfileTest, PosContinuousUnderVelChange) {
// There is some somewhat tricky code for dealing with going backwards
TEST(TrapezoidProfileTest, Backwards) {
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{-2_m, 0_mps};
wpi::math::TrapezoidProfile<wpi::units::meter>::State state;
frc::TrapezoidProfile<units::meter> profile{constraints};
wpi::math::TrapezoidProfile<wpi::units::meter> profile{constraints};
for (int i = 0; i < 400; ++i) {
state = profile.Calculate(kDt, state, goal);
}
@@ -89,19 +89,19 @@ TEST(TrapezoidProfileTest, Backwards) {
}
TEST(TrapezoidProfileTest, SwitchGoalInMiddle) {
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{-2_m, 0_mps};
wpi::math::TrapezoidProfile<wpi::units::meter>::State state;
frc::TrapezoidProfile<units::meter> profile{constraints};
wpi::math::TrapezoidProfile<wpi::units::meter> profile{constraints};
for (int i = 0; i < 200; ++i) {
state = profile.Calculate(kDt, state, goal);
}
EXPECT_NE(state, goal);
goal = {0.0_m, 0.0_mps};
profile = frc::TrapezoidProfile<units::meter>{constraints};
profile = wpi::math::TrapezoidProfile<wpi::units::meter>{constraints};
for (int i = 0; i < 550; ++i) {
state = profile.Calculate(kDt, state, goal);
}
@@ -110,18 +110,18 @@ TEST(TrapezoidProfileTest, SwitchGoalInMiddle) {
// Checks to make sure that it hits top speed
TEST(TrapezoidProfileTest, TopSpeed) {
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{4_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{4_m, 0_mps};
wpi::math::TrapezoidProfile<wpi::units::meter>::State state;
frc::TrapezoidProfile<units::meter> profile{constraints};
wpi::math::TrapezoidProfile<wpi::units::meter> profile{constraints};
for (int i = 0; i < 200; ++i) {
state = profile.Calculate(kDt, state, goal);
}
EXPECT_NEAR_UNITS(constraints.maxVelocity, state.velocity, 10e-5_mps);
profile = frc::TrapezoidProfile<units::meter>{constraints};
profile = wpi::math::TrapezoidProfile<wpi::units::meter>{constraints};
for (int i = 0; i < 2000; ++i) {
state = profile.Calculate(kDt, state, goal);
}
@@ -129,12 +129,12 @@ TEST(TrapezoidProfileTest, TopSpeed) {
}
TEST(TrapezoidProfileTest, TimingToCurrent) {
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{2_m, 0_mps};
wpi::math::TrapezoidProfile<wpi::units::meter>::State state;
frc::TrapezoidProfile<units::meter> profile{constraints};
wpi::math::TrapezoidProfile<wpi::units::meter> profile{constraints};
for (int i = 0; i < 400; i++) {
state = profile.Calculate(kDt, state, goal);
EXPECT_NEAR_UNITS(profile.TimeLeftUntil(state.position), 0_s, 2e-2_s);
@@ -142,15 +142,15 @@ TEST(TrapezoidProfileTest, TimingToCurrent) {
}
TEST(TrapezoidProfileTest, TimingToGoal) {
using units::unit_cast;
using wpi::units::unit_cast;
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{2_m, 0_mps};
frc::TrapezoidProfile<units::meter> profile{constraints};
wpi::math::TrapezoidProfile<wpi::units::meter> profile{constraints};
auto state = profile.Calculate(kDt, goal,
frc::TrapezoidProfile<units::meter>::State{});
wpi::math::TrapezoidProfile<wpi::units::meter>::State{});
auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
bool reachedGoal = false;
@@ -166,22 +166,22 @@ TEST(TrapezoidProfileTest, TimingToGoal) {
}
TEST(TrapezoidProfileTest, TimingBeforeGoal) {
using units::unit_cast;
using wpi::units::unit_cast;
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{2_m, 0_mps};
frc::TrapezoidProfile<units::meter> profile{constraints};
wpi::math::TrapezoidProfile<wpi::units::meter> profile{constraints};
auto state = profile.Calculate(kDt, goal,
frc::TrapezoidProfile<units::meter>::State{});
wpi::math::TrapezoidProfile<wpi::units::meter>::State{});
auto predictedTimeLeft = profile.TimeLeftUntil(1_m);
bool reachedGoal = false;
for (int i = 0; i < 400; i++) {
state = profile.Calculate(kDt, state, goal);
if (!reachedGoal &&
(units::math::abs(state.velocity - 1_mps) < 10e-5_mps)) {
(wpi::units::math::abs(state.velocity - 1_mps) < 10e-5_mps)) {
EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);
reachedGoal = true;
}
@@ -189,15 +189,15 @@ TEST(TrapezoidProfileTest, TimingBeforeGoal) {
}
TEST(TrapezoidProfileTest, TimingToNegativeGoal) {
using units::unit_cast;
using wpi::units::unit_cast;
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{-2_m, 0_mps};
frc::TrapezoidProfile<units::meter> profile{constraints};
wpi::math::TrapezoidProfile<wpi::units::meter> profile{constraints};
auto state = profile.Calculate(kDt, goal,
frc::TrapezoidProfile<units::meter>::State{});
wpi::math::TrapezoidProfile<wpi::units::meter>::State{});
auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
bool reachedGoal = false;
@@ -213,22 +213,22 @@ TEST(TrapezoidProfileTest, TimingToNegativeGoal) {
}
TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) {
using units::unit_cast;
using wpi::units::unit_cast;
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{-2_m, 0_mps};
frc::TrapezoidProfile<units::meter> profile{constraints};
wpi::math::TrapezoidProfile<wpi::units::meter> profile{constraints};
auto state = profile.Calculate(kDt, goal,
frc::TrapezoidProfile<units::meter>::State{});
wpi::math::TrapezoidProfile<wpi::units::meter>::State{});
auto predictedTimeLeft = profile.TimeLeftUntil(-1_m);
bool reachedGoal = false;
for (int i = 0; i < 400; i++) {
state = profile.Calculate(kDt, state, goal);
if (!reachedGoal &&
(units::math::abs(state.velocity + 1_mps) < 10e-5_mps)) {
(wpi::units::math::abs(state.velocity + 1_mps) < 10e-5_mps)) {
EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);
reachedGoal = true;
}
@@ -236,53 +236,53 @@ TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) {
}
TEST(TrapezoidProfileTest, InitalizationOfCurrentState) {
frc::TrapezoidProfile<units::meter>::Constraints constraints{1_mps, 1_mps_sq};
frc::TrapezoidProfile<units::meter> profile{constraints};
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{1_mps, 1_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter> profile{constraints};
EXPECT_NEAR_UNITS(profile.TimeLeftUntil(0_m), 0_s, 1e-10_s);
EXPECT_NEAR_UNITS(profile.TotalTime(), 0_s, 1e-10_s);
}
TEST(TrapezoidProfileTest, InitialVelocityConstraints) {
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{10_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state{0_m, -10_mps};
wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{10_m, 0_mps};
wpi::math::TrapezoidProfile<wpi::units::meter>::State state{0_m, -10_mps};
frc::TrapezoidProfile<units::meter> profile{constraints};
wpi::math::TrapezoidProfile<wpi::units::meter> profile{constraints};
for (int i = 0; i < 200; ++i) {
state = profile.Calculate(kDt, state, goal);
EXPECT_LE(units::math::abs(state.velocity),
units::math::abs(constraints.maxVelocity));
EXPECT_LE(wpi::units::math::abs(state.velocity),
wpi::units::math::abs(constraints.maxVelocity));
}
}
TEST(TrapezoidProfileTest, GoalVelocityConstraints) {
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{10_m, 5_mps};
frc::TrapezoidProfile<units::meter>::State state{0_m, 0.75_mps};
wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{10_m, 5_mps};
wpi::math::TrapezoidProfile<wpi::units::meter>::State state{0_m, 0.75_mps};
frc::TrapezoidProfile<units::meter> profile{constraints};
wpi::math::TrapezoidProfile<wpi::units::meter> profile{constraints};
for (int i = 0; i < 200; ++i) {
state = profile.Calculate(kDt, state, goal);
EXPECT_LE(units::math::abs(state.velocity),
units::math::abs(constraints.maxVelocity));
EXPECT_LE(wpi::units::math::abs(state.velocity),
wpi::units::math::abs(constraints.maxVelocity));
}
}
TEST(TrapezoidProfileTest, NegativeGoalVelocityConstraints) {
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{10_m, -5_mps};
frc::TrapezoidProfile<units::meter>::State state{0_m, 0.75_mps};
wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{10_m, -5_mps};
wpi::math::TrapezoidProfile<wpi::units::meter>::State state{0_m, 0.75_mps};
frc::TrapezoidProfile<units::meter> profile{constraints};
wpi::math::TrapezoidProfile<wpi::units::meter> profile{constraints};
for (int i = 0; i < 200; ++i) {
state = profile.Calculate(kDt, state, goal);
EXPECT_LE(units::math::abs(state.velocity),
units::math::abs(constraints.maxVelocity));
EXPECT_LE(wpi::units::math::abs(state.velocity),
wpi::units::math::abs(constraints.maxVelocity));
}
}