Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11: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

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

@@ -35,22 +35,22 @@ using Kg_unit = decltype(1_V);
* @param dt The simulation time.
* @return The final state as a 2-vector of angle and angular velocity.
*/
frc::Matrixd<2, 1> Simulate(Ks_unit Ks, Kv_unit Kv, Ka_unit Ka, Kg_unit Kg,
units::radian_t currentAngle,
units::radians_per_second_t currentVelocity,
units::volt_t input, units::second_t dt) {
frc::Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -Kv.value() / Ka.value()}};
frc::Matrixd<2, 1> B{{0.0}, {1.0 / Ka.value()}};
wpi::math::Matrixd<2, 1> Simulate(Ks_unit Ks, Kv_unit Kv, Ka_unit Ka, Kg_unit Kg,
wpi::units::radian_t currentAngle,
wpi::units::radians_per_second_t currentVelocity,
wpi::units::volt_t input, wpi::units::second_t dt) {
wpi::math::Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -Kv.value() / Ka.value()}};
wpi::math::Matrixd<2, 1> B{{0.0}, {1.0 / Ka.value()}};
return frc::RK4(
[&](const frc::Matrixd<2, 1>& x,
const frc::Matrixd<1, 1>& u) -> frc::Matrixd<2, 1> {
frc::Matrixd<2, 1> c{0.0, -Ks.value() / Ka.value() * wpi::sgn(x(1)) -
return wpi::math::RK4(
[&](const wpi::math::Matrixd<2, 1>& x,
const wpi::math::Matrixd<1, 1>& u) -> wpi::math::Matrixd<2, 1> {
wpi::math::Matrixd<2, 1> c{0.0, -Ks.value() / Ka.value() * wpi::util::sgn(x(1)) -
Kg.value() / Ka.value() * std::cos(x(0))};
return A * x + B * u + c;
},
frc::Matrixd<2, 1>{currentAngle.value(), currentVelocity.value()},
frc::Matrixd<1, 1>{input.value()}, dt);
wpi::math::Matrixd<2, 1>{currentAngle.value(), currentVelocity.value()},
wpi::math::Matrixd<1, 1>{input.value()}, dt);
}
/**
@@ -66,12 +66,12 @@ frc::Matrixd<2, 1> Simulate(Ks_unit Ks, Kv_unit Kv, Ka_unit Ka, Kg_unit Kg,
* @param input The input voltage.
* @param dt The simulation time.
*/
void CalculateAndSimulate(const frc::ArmFeedforward& armFF, Ks_unit Ks,
void CalculateAndSimulate(const wpi::math::ArmFeedforward& armFF, Ks_unit Ks,
Kv_unit Kv, Ka_unit Ka, Kg_unit Kg,
units::radian_t currentAngle,
units::radians_per_second_t currentVelocity,
units::radians_per_second_t nextVelocity,
units::second_t dt) {
wpi::units::radian_t currentAngle,
wpi::units::radians_per_second_t currentVelocity,
wpi::units::radians_per_second_t nextVelocity,
wpi::units::second_t dt) {
auto input = armFF.Calculate(currentAngle, currentVelocity, nextVelocity);
EXPECT_NEAR(
nextVelocity.value(),
@@ -86,7 +86,7 @@ TEST(ArmFeedforwardTest, Calculate) {
constexpr auto Kv = 1.5_V / 1_rad_per_s;
constexpr auto Ka = 2_V / 1_rad_per_s_sq;
constexpr auto Kg = 1_V;
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
wpi::math::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
// Calculate(angle, angular velocity)
EXPECT_NEAR(
@@ -112,7 +112,7 @@ TEST(ArmFeedforwardTest, CalculateIllConditionedModel) {
constexpr auto Kv = 2.7167_V / 1_rad_per_s;
constexpr auto Ka = 1e-2_V / 1_rad_per_s_sq;
constexpr auto Kg = 0.2708_V;
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
wpi::math::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
constexpr auto currentAngle = 1_rad;
constexpr auto currentVelocity = 0.02_rad_per_s;
@@ -123,7 +123,7 @@ TEST(ArmFeedforwardTest, CalculateIllConditionedModel) {
EXPECT_DOUBLE_EQ(
armFF.Calculate(currentAngle, currentVelocity, nextVelocity).value(),
(Ks + Kv * currentVelocity + Ka * averageAccel +
Kg * units::math::cos(currentAngle))
Kg * wpi::units::math::cos(currentAngle))
.value());
}
@@ -132,7 +132,7 @@ TEST(ArmFeedforwardTest, CalculateIllConditionedGradient) {
constexpr auto Kv = 2.7167_V / 1_rad_per_s;
constexpr auto Ka = 0.50799_V / 1_rad_per_s_sq;
constexpr auto Kg = 0.2708_V;
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
wpi::math::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, 1_rad, 0.02_rad_per_s,
0_rad_per_s, 20_ms);
@@ -143,7 +143,7 @@ TEST(ArmFeedforwardTest, AchievableVelocity) {
constexpr auto Kv = 1.5_V / 1_rad_per_s;
constexpr auto Ka = 2_V / 1_rad_per_s_sq;
constexpr auto Kg = 1_V;
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
wpi::math::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
EXPECT_NEAR(armFF
.MaxAchievableVelocity(12_V, std::numbers::pi / 3 * 1_rad,
@@ -162,7 +162,7 @@ TEST(ArmFeedforwardTest, AchievableAcceleration) {
constexpr auto Kv = 1.5_V / 1_rad_per_s;
constexpr auto Ka = 2_V / 1_rad_per_s_sq;
constexpr auto Kg = 1_V;
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
wpi::math::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
EXPECT_NEAR(armFF
.MaxAchievableAcceleration(12_V, std::numbers::pi / 3 * 1_rad,
@@ -191,7 +191,7 @@ TEST(ArmFeedforwardTest, NegativeGains) {
constexpr auto Kv = 1.5_V / 1_rad_per_s;
constexpr auto Ka = 2_V / 1_rad_per_s_sq;
constexpr auto Kg = 1_V;
frc::ArmFeedforward armFF{Ks, Kg, -Kv, -Ka};
wpi::math::ArmFeedforward armFF{Ks, Kg, -Kv, -Ka};
EXPECT_EQ(armFF.GetKv().value(), 0);
EXPECT_EQ(armFF.GetKa().value(), 0);

4
wpimath/src/test/native/cpp/controller/BangBangInputOutputTest.cpp
View File

@@ -7,13 +7,13 @@
#include "wpi/math/controller/BangBangController.hpp"
TEST(BangBangInputOutputTest, ShouldOutput) {
frc::BangBangController controller;
wpi::math::BangBangController controller;
EXPECT_DOUBLE_EQ(controller.Calculate(0, 1), 1);
}
TEST(BangBangInputOutputTest, ShouldNotOutput) {
frc::BangBangController controller;
wpi::math::BangBangController controller;
EXPECT_DOUBLE_EQ(controller.Calculate(1, 0), 0);
}

4
wpimath/src/test/native/cpp/controller/BangBangToleranceTest.cpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/controller/BangBangController.hpp"
TEST(BangBangToleranceTest, InTolerance) {
frc::BangBangController controller{0.1};
wpi::math::BangBangController controller{0.1};
controller.SetSetpoint(1);
controller.Calculate(1);
@@ -15,7 +15,7 @@ TEST(BangBangToleranceTest, InTolerance) {
}
TEST(BangBangToleranceTest, OutOfTolerance) {
frc::BangBangController controller{0.1};
wpi::math::BangBangController controller{0.1};
controller.SetSetpoint(1);
controller.Calculate(0);

8
wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
View File

@@ -8,7 +8,7 @@
#include "wpi/math/linalg/EigenCore.hpp"
#include "wpi/units/time.hpp"
namespace frc {
namespace wpi::math {
Vectord<2> Dynamics(const Vectord<2>& x, const Vectord<1>& u) {
return Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}} * x +
@@ -20,7 +20,7 @@ Vectord<2> StateDynamics(const Vectord<2>& x) {
}
TEST(ControlAffinePlantInversionFeedforwardTest, Calculate) {
frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{&Dynamics,
wpi::math::ControlAffinePlantInversionFeedforward<2, 1> feedforward{&Dynamics,
20_ms};
Vectord<2> r{2, 2};
@@ -30,7 +30,7 @@ TEST(ControlAffinePlantInversionFeedforwardTest, Calculate) {
}
TEST(ControlAffinePlantInversionFeedforwardTest, CalculateState) {
frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{
wpi::math::ControlAffinePlantInversionFeedforward<2, 1> feedforward{
&StateDynamics, Matrixd<2, 1>{{0.0}, {1.0}}, 20_ms};
Vectord<2> r{2, 2};
@@ -39,4 +39,4 @@ TEST(ControlAffinePlantInversionFeedforwardTest, CalculateState) {
EXPECT_NEAR(48, feedforward.Calculate(r, nextR)(0, 0), 1e-6);
}
} // namespace frc
} // namespace wpi::math

100
wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp
View File

@@ -8,11 +8,11 @@
#include "wpi/math/system/plant/LinearSystemId.hpp"
#include "wpi/units/math.hpp"
namespace frc {
namespace wpi::math {
TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
constexpr auto trackwidth = 0.9_m;
constexpr units::second_t dt = 5_ms;
constexpr wpi::units::second_t dt = 5_ms;
constexpr auto maxA = 2_mps_sq;
constexpr auto maxAlpha = 2_rad_per_s_sq;
@@ -31,15 +31,15 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
{
Vectord<2> accels =
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{12.0, 12.0};
units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
EXPECT_GT(units::math::abs(a), maxA);
wpi::units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
EXPECT_GT(wpi::units::math::abs(a), maxA);
}
{
Vectord<2> accels =
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{-12.0, 12.0};
units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
wpi::units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
trackwidth.value()};
EXPECT_GT(units::math::abs(alpha), maxAlpha);
EXPECT_GT(wpi::units::math::abs(alpha), maxAlpha);
}
// Forward
@@ -47,19 +47,19 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
accelLimiter.Calculate(wpi::units::meters_per_second_t{xAccelLimiter(0)},
wpi::units::meters_per_second_t{xAccelLimiter(1)},
wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
Vectord<2> accels =
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
wpi::units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
wpi::units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
trackwidth.value()};
EXPECT_LE(units::math::abs(a), maxA);
EXPECT_LE(units::math::abs(alpha), maxAlpha);
EXPECT_LE(wpi::units::math::abs(a), maxA);
EXPECT_LE(wpi::units::math::abs(alpha), maxAlpha);
}
// Backward
@@ -67,19 +67,19 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
accelLimiter.Calculate(wpi::units::meters_per_second_t{xAccelLimiter(0)},
wpi::units::meters_per_second_t{xAccelLimiter(1)},
wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
Vectord<2> accels =
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
wpi::units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
wpi::units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
trackwidth.value()};
EXPECT_LE(units::math::abs(a), maxA);
EXPECT_LE(units::math::abs(alpha), maxAlpha);
EXPECT_LE(wpi::units::math::abs(a), maxA);
EXPECT_LE(wpi::units::math::abs(alpha), maxAlpha);
}
// Rotate CCW
@@ -87,25 +87,25 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
accelLimiter.Calculate(wpi::units::meters_per_second_t{xAccelLimiter(0)},
wpi::units::meters_per_second_t{xAccelLimiter(1)},
wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
Vectord<2> accels =
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
wpi::units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
wpi::units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
trackwidth.value()};
EXPECT_LE(units::math::abs(a), maxA);
EXPECT_LE(units::math::abs(alpha), maxAlpha);
EXPECT_LE(wpi::units::math::abs(a), maxA);
EXPECT_LE(wpi::units::math::abs(alpha), maxAlpha);
}
}
TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
constexpr auto trackwidth = 0.9_m;
constexpr units::second_t dt = 5_ms;
constexpr wpi::units::second_t dt = 5_ms;
using Kv_t = decltype(1_V / 1_mps);
using Ka_t = decltype(1_V / 1_mps_sq);
@@ -126,9 +126,9 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
accelLimiter.Calculate(wpi::units::meters_per_second_t{xAccelLimiter(0)},
wpi::units::meters_per_second_t{xAccelLimiter(1)},
wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
@@ -143,9 +143,9 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
accelLimiter.Calculate(wpi::units::meters_per_second_t{xAccelLimiter(0)},
wpi::units::meters_per_second_t{xAccelLimiter(1)},
wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
@@ -160,9 +160,9 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
accelLimiter.Calculate(wpi::units::meters_per_second_t{xAccelLimiter(0)},
wpi::units::meters_per_second_t{xAccelLimiter(1)},
wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
@@ -173,7 +173,7 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
TEST(DifferentialDriveAccelerationLimiterTest, SeparateMinMaxLowLimits) {
constexpr auto trackwidth = 0.9_m;
constexpr units::second_t dt = 5_ms;
constexpr wpi::units::second_t dt = 5_ms;
constexpr auto minA = -1_mps_sq;
constexpr auto maxA = 2_mps_sq;
constexpr auto maxAlpha = 2_rad_per_s_sq;
@@ -193,9 +193,9 @@ TEST(DifferentialDriveAccelerationLimiterTest, SeparateMinMaxLowLimits) {
{
Vectord<2> accels =
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{12.0, 12.0};
units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
EXPECT_GT(units::math::abs(a), maxA);
EXPECT_GT(units::math::abs(a), -minA);
wpi::units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
EXPECT_GT(wpi::units::math::abs(a), maxA);
EXPECT_GT(wpi::units::math::abs(a), -minA);
}
// a should always be within [minA, maxA]
@@ -204,15 +204,15 @@ TEST(DifferentialDriveAccelerationLimiterTest, SeparateMinMaxLowLimits) {
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
accelLimiter.Calculate(wpi::units::meters_per_second_t{xAccelLimiter(0)},
wpi::units::meters_per_second_t{xAccelLimiter(1)},
wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
Vectord<2> accels =
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
wpi::units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
EXPECT_GE(a, minA);
EXPECT_LE(a, maxA);
}
@@ -222,15 +222,15 @@ TEST(DifferentialDriveAccelerationLimiterTest, SeparateMinMaxLowLimits) {
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
accelLimiter.Calculate(wpi::units::meters_per_second_t{xAccelLimiter(0)},
wpi::units::meters_per_second_t{xAccelLimiter(1)},
wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
Vectord<2> accels =
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
wpi::units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
EXPECT_GE(a, minA);
EXPECT_LE(a, maxA);
}
@@ -254,4 +254,4 @@ TEST(DifferentialDriveAccelerationLimiterTest, MinAccelGreaterThanMaxAccel) {
std::invalid_argument);
}
} // namespace frc
} // namespace wpi::math

16
wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp
View File

@@ -20,12 +20,12 @@ TEST(DifferentialDriveFeedforwardTest, CalculateWithTrackwidth) {
constexpr auto kVAngular = 1_V / 1_rad_per_s;
constexpr auto kAAngular = 1_V / 1_rad_per_s_sq;
constexpr auto trackwidth = 1_m;
constexpr units::second_t dt = 20_ms;
constexpr wpi::units::second_t dt = 20_ms;
frc::DifferentialDriveFeedforward differentialDriveFeedforward{
wpi::math::DifferentialDriveFeedforward differentialDriveFeedforward{
kVLinear, kALinear, kVAngular, kAAngular, trackwidth};
frc::LinearSystem<2, 2, 2> plant =
frc::LinearSystemId::IdentifyDrivetrainSystem(
wpi::math::LinearSystem<2, 2, 2> plant =
wpi::math::LinearSystemId::IdentifyDrivetrainSystem(
kVLinear, kALinear, kVAngular, kAAngular, trackwidth);
for (auto currentLeftVelocity = -4_mps; currentLeftVelocity <= 4_mps;
currentLeftVelocity += 2_mps) {
@@ -54,12 +54,12 @@ TEST(DifferentialDriveFeedforwardTest, CalculateWithoutTrackwidth) {
constexpr auto kALinear = 1_V / 1_mps_sq;
constexpr auto kVAngular = 1_V / 1_mps;
constexpr auto kAAngular = 1_V / 1_mps_sq;
constexpr units::second_t dt = 20_ms;
constexpr wpi::units::second_t dt = 20_ms;
frc::DifferentialDriveFeedforward differentialDriveFeedforward{
wpi::math::DifferentialDriveFeedforward differentialDriveFeedforward{
kVLinear, kALinear, kVAngular, kAAngular};
frc::LinearSystem<2, 2, 2> plant =
frc::LinearSystemId::IdentifyDrivetrainSystem(kVLinear, kALinear,
wpi::math::LinearSystem<2, 2, 2> plant =
wpi::math::LinearSystemId::IdentifyDrivetrainSystem(kVLinear, kALinear,
kVAngular, kAAngular);
for (auto currentLeftVelocity = -4_mps; currentLeftVelocity <= 4_mps;
currentLeftVelocity += 2_mps) {

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

@@ -19,24 +19,24 @@ static constexpr auto Ka = 2_V * 1_s * 1_s / 1_m;
static constexpr auto Kg = 1_V;
TEST(ElevatorFeedforwardTest, Calculate) {
frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka};
wpi::math::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka};
EXPECT_NEAR(elevatorFF.Calculate(0_m / 1_s).value(), Kg.value(), 0.002);
EXPECT_NEAR(elevatorFF.Calculate(2_m / 1_s).value(), 4.5, 0.002);
frc::Matrixd<1, 1> A{-Kv.value() / Ka.value()};
frc::Matrixd<1, 1> B{1.0 / Ka.value()};
constexpr units::second_t dt = 20_ms;
frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt};
wpi::math::Matrixd<1, 1> A{-Kv.value() / Ka.value()};
wpi::math::Matrixd<1, 1> B{1.0 / Ka.value()};
constexpr wpi::units::second_t dt = 20_ms;
wpi::math::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt};
frc::Vectord<1> r{2.0};
frc::Vectord<1> nextR{3.0};
wpi::math::Vectord<1> r{2.0};
wpi::math::Vectord<1> nextR{3.0};
EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks.value() + Kg.value(),
elevatorFF.Calculate(2_mps, 3_mps).value(), 0.002);
}
TEST(ElevatorFeedforwardTest, AchievableVelocity) {
frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka};
wpi::math::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka};
EXPECT_NEAR(elevatorFF.MaxAchievableVelocity(11_V, 1_m / 1_s / 1_s).value(),
5, 0.002);
EXPECT_NEAR(elevatorFF.MinAchievableVelocity(11_V, 1_m / 1_s / 1_s).value(),
@@ -44,7 +44,7 @@ TEST(ElevatorFeedforwardTest, AchievableVelocity) {
}
TEST(ElevatorFeedforwardTest, AchievableAcceleration) {
frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka};
wpi::math::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka};
EXPECT_NEAR(elevatorFF.MaxAchievableAcceleration(12_V, 2_m / 1_s).value(),
3.75, 0.002);
EXPECT_NEAR(elevatorFF.MaxAchievableAcceleration(12_V, -2_m / 1_s).value(),
@@ -56,7 +56,7 @@ TEST(ElevatorFeedforwardTest, AchievableAcceleration) {
}
TEST(ElevatorFeedforwardTest, NegativeGains) {
frc::ElevatorFeedforward elevatorFF{Ks, Kg, -Kv, -Ka};
wpi::math::ElevatorFeedforward elevatorFF{Ks, Kg, -Kv, -Ka};
EXPECT_EQ(elevatorFF.GetKv().value(), 0);
EXPECT_EQ(elevatorFF.GetKa().value(), 0);
}

8
wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp
View File

@@ -7,10 +7,10 @@
#include "wpi/math/controller/ImplicitModelFollower.hpp"
#include "wpi/math/system/plant/LinearSystemId.hpp"
namespace frc {
namespace wpi::math {
TEST(ImplicitModelFollowerTest, SameModel) {
constexpr units::second_t dt = 5_ms;
constexpr wpi::units::second_t dt = 5_ms;
using Kv_t = decltype(1_V / 1_mps);
using Ka_t = decltype(1_V / 1_mps_sq);
@@ -54,7 +54,7 @@ TEST(ImplicitModelFollowerTest, SameModel) {
}
TEST(ImplicitModelFollowerTest, SlowerRefModel) {
constexpr units::second_t dt = 5_ms;
constexpr wpi::units::second_t dt = 5_ms;
using Kv_t = decltype(1_V / 1_mps);
using Ka_t = decltype(1_V / 1_mps_sq);
@@ -106,4 +106,4 @@ TEST(ImplicitModelFollowerTest, SlowerRefModel) {
}
}
} // namespace frc
} // namespace wpi::math

40
wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp
View File

@@ -14,10 +14,10 @@
#include "wpi/units/math.hpp"
#define EXPECT_NEAR_UNITS(val1, val2, eps) \
EXPECT_LE(units::math::abs(val1 - val2), eps)
EXPECT_LE(wpi::units::math::abs(val1 - val2), eps)
static constexpr units::meter_t kTolerance{1 / 12.0};
static constexpr units::radian_t kAngularTolerance{2.0 * std::numbers::pi /
static constexpr wpi::units::meter_t kTolerance{1 / 12.0};
static constexpr wpi::units::radian_t kAngularTolerance{2.0 * std::numbers::pi /
180.0};
/**
@@ -45,14 +45,14 @@ static constexpr auto kLinearV = 3.02_V / 1_mps;
static constexpr auto kLinearA = 0.642_V / 1_mps_sq;
static constexpr auto kAngularV = 1.382_V / 1_mps;
static constexpr auto kAngularA = 0.08495_V / 1_mps_sq;
static auto plant = frc::LinearSystemId::IdentifyDrivetrainSystem(
static auto plant = wpi::math::LinearSystemId::IdentifyDrivetrainSystem(
kLinearV, kLinearA, kAngularV, kAngularA);
static constexpr auto kTrackwidth = 0.9_m;
frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
wpi::math::Vectord<5> Dynamics(const wpi::math::Vectord<5>& x, const wpi::math::Vectord<2>& u) {
double v = (x(State::kLeftVelocity) + x(State::kRightVelocity)) / 2.0;
frc::Vectord<5> xdot;
wpi::math::Vectord<5> xdot;
xdot(0) = v * std::cos(x(State::kHeading));
xdot(1) = v * std::sin(x(State::kHeading));
xdot(2) = ((x(State::kRightVelocity) - x(State::kLeftVelocity)) / kTrackwidth)
@@ -62,18 +62,18 @@ frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
}
TEST(LTVDifferentialDriveControllerTest, ReachesReference) {
constexpr units::second_t kDt = 20_ms;
constexpr wpi::units::second_t kDt = 20_ms;
frc::LTVDifferentialDriveController controller{
wpi::math::LTVDifferentialDriveController controller{
plant, kTrackwidth, {0.0625, 0.125, 2.5, 0.95, 0.95}, {12.0, 12.0}, kDt};
frc::Pose2d robotPose{2.7_m, 23_m, 0_deg};
wpi::math::Pose2d robotPose{2.7_m, 23_m, 0_deg};
auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
auto waypoints = std::vector{wpi::math::Pose2d{2.75_m, 22.521_m, 0_rad},
wpi::math::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.8_mps, 0.1_mps_sq});
frc::Vectord<5> x = frc::Vectord<5>::Zero();
wpi::math::Vectord<5> x = wpi::math::Vectord<5>::Zero();
x(State::kX) = robotPose.X().value();
x(State::kY) = robotPose.Y().value();
x(State::kHeading) = robotPose.Rotation().Radians().value();
@@ -82,21 +82,21 @@ TEST(LTVDifferentialDriveControllerTest, ReachesReference) {
for (size_t i = 0; i < (totalTime / kDt).value(); ++i) {
auto state = trajectory.Sample(kDt * i);
robotPose =
frc::Pose2d{units::meter_t{x(State::kX)}, units::meter_t{x(State::kY)},
units::radian_t{x(State::kHeading)}};
wpi::math::Pose2d{wpi::units::meter_t{x(State::kX)}, wpi::units::meter_t{x(State::kY)},
wpi::units::radian_t{x(State::kHeading)}};
auto [leftVoltage, rightVoltage] = controller.Calculate(
robotPose, units::meters_per_second_t{x(State::kLeftVelocity)},
units::meters_per_second_t{x(State::kRightVelocity)}, state);
robotPose, wpi::units::meters_per_second_t{x(State::kLeftVelocity)},
wpi::units::meters_per_second_t{x(State::kRightVelocity)}, state);
x = frc::RKDP(&Dynamics, x,
frc::Vectord<2>{leftVoltage.value(), rightVoltage.value()},
x = wpi::math::RKDP(&Dynamics, x,
wpi::math::Vectord<2>{leftVoltage.value(), rightVoltage.value()},
kDt);
}
auto& endPose = trajectory.States().back().pose;
EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance);
EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance);
EXPECT_NEAR_UNITS(frc::AngleModulus(endPose.Rotation().Radians() -
EXPECT_NEAR_UNITS(wpi::math::AngleModulus(endPose.Rotation().Radians() -
robotPose.Rotation().Radians()),
0_rad, kAngularTolerance);
}

22
wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.cpp
View File

@@ -10,21 +10,21 @@
#include "wpi/units/math.hpp"
#define EXPECT_NEAR_UNITS(val1, val2, eps) \
EXPECT_LE(units::math::abs(val1 - val2), eps)
EXPECT_LE(wpi::units::math::abs(val1 - val2), eps)
static constexpr units::meter_t kTolerance{1 / 12.0};
static constexpr units::radian_t kAngularTolerance{2.0 * std::numbers::pi /
static constexpr wpi::units::meter_t kTolerance{1 / 12.0};
static constexpr wpi::units::radian_t kAngularTolerance{2.0 * std::numbers::pi /
180.0};
TEST(LTVUnicycleControllerTest, ReachesReference) {
constexpr units::second_t kDt = 20_ms;
constexpr wpi::units::second_t kDt = 20_ms;
frc::LTVUnicycleController controller{{0.0625, 0.125, 2.5}, {4.0, 4.0}, kDt};
frc::Pose2d robotPose{2.7_m, 23_m, 0_deg};
wpi::math::LTVUnicycleController controller{{0.0625, 0.125, 2.5}, {4.0, 4.0}, kDt};
wpi::math::Pose2d robotPose{2.7_m, 23_m, 0_deg};
auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
auto waypoints = std::vector{wpi::math::Pose2d{2.75_m, 22.521_m, 0_rad},
wpi::math::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.8_mps, 0.1_mps_sq});
auto totalTime = trajectory.TotalTime();
@@ -33,13 +33,13 @@ TEST(LTVUnicycleControllerTest, ReachesReference) {
auto [vx, vy, omega] = controller.Calculate(robotPose, state);
static_cast<void>(vy);
robotPose = robotPose + frc::Twist2d{vx * kDt, 0_m, omega * kDt}.Exp();
robotPose = robotPose + wpi::math::Twist2d{vx * kDt, 0_m, omega * kDt}.Exp();
}
auto& endPose = trajectory.States().back().pose;
EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance);
EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance);
EXPECT_NEAR_UNITS(frc::AngleModulus(endPose.Rotation().Radians() -
EXPECT_NEAR_UNITS(wpi::math::AngleModulus(endPose.Rotation().Radians() -
robotPose.Rotation().Radians()),
0_rad, kAngularTolerance);
}

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

@@ -10,13 +10,13 @@
#include "wpi/math/linalg/EigenCore.hpp"
#include "wpi/units/time.hpp"
namespace frc {
namespace wpi::math {
TEST(LinearPlantInversionFeedforwardTest, Calculate) {
Matrixd<2, 2> A{{1, 0}, {0, 1}};
Matrixd<2, 1> B{0, 1};
frc::LinearPlantInversionFeedforward<2, 1> feedforward{A, B, 20_ms};
wpi::math::LinearPlantInversionFeedforward<2, 1> feedforward{A, B, 20_ms};
Vectord<2> r{2, 2};
Vectord<2> nextR{3, 3};
@@ -24,4 +24,4 @@ TEST(LinearPlantInversionFeedforwardTest, Calculate) {
EXPECT_NEAR(47.502599, feedforward.Calculate(r, nextR)(0, 0), 0.002);
}
} // namespace frc
} // namespace wpi::math

14
wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
View File

@@ -13,7 +13,7 @@
#include "wpi/math/system/plant/LinearSystemId.hpp"
#include "wpi/units/time.hpp"
namespace frc {
namespace wpi::math {
TEST(LinearQuadraticRegulatorTest, ElevatorGains) {
LinearSystem<2, 1, 1> plant = [] {
@@ -28,7 +28,7 @@ TEST(LinearQuadraticRegulatorTest, ElevatorGains) {
// Gear ratio
constexpr double G = 40.0 / 40.0;
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
return wpi::math::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
}();
Matrixd<1, 2> K =
LinearQuadraticRegulator<2, 1>{plant, {0.02, 0.4}, {12.0}, 5_ms}.K();
@@ -50,7 +50,7 @@ TEST(LinearQuadraticRegulatorTest, ArmGains) {
// Gear ratio
constexpr double G = 100.0;
return frc::LinearSystemId::SingleJointedArmSystem(motors,
return wpi::math::LinearSystemId::SingleJointedArmSystem(motors,
1.0 / 3.0 * m * r * r, G)
.Slice(0);
}();
@@ -76,7 +76,7 @@ TEST(LinearQuadraticRegulatorTest, FourMotorElevator) {
// Gear ratio
constexpr double G = 14.67;
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
return wpi::math::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
}();
Matrixd<1, 2> K =
LinearQuadraticRegulator<2, 1>{plant, {0.1, 0.2}, {12.0}, 20_ms}.K();
@@ -103,7 +103,7 @@ template <int States, int Inputs>
Matrixd<Inputs, States> GetImplicitModelFollowingK(
const Matrixd<States, States>& A, const Matrixd<States, Inputs>& B,
const Matrixd<States, States>& Q, const Matrixd<Inputs, Inputs>& R,
const Matrixd<States, States>& Aref, units::second_t dt) {
const Matrixd<States, States>& Aref, wpi::units::second_t dt) {
// Discretize real dynamics
Matrixd<States, States> discA;
Matrixd<States, Inputs> discB;
@@ -178,7 +178,7 @@ TEST(LinearQuadraticRegulatorTest, LatencyCompensate) {
// Gear ratio
constexpr double G = 14.67;
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
return wpi::math::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
}();
LinearQuadraticRegulator<2, 1> controller{plant, {0.1, 0.2}, {12.0}, 20_ms};
@@ -188,4 +188,4 @@ TEST(LinearQuadraticRegulatorTest, LatencyCompensate) {
EXPECT_NEAR(0.07904881, controller.K(0, 1), 1e-3);
}
} // namespace frc
} // namespace wpi::math

12
wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/controller/PIDController.hpp"
TEST(PIDInputOutputTest, ContinuousInput) {
frc::PIDController controller{0.0, 0.0, 0.0};
wpi::math::PIDController controller{0.0, 0.0, 0.0};
controller.SetP(1);
controller.EnableContinuousInput(-180, 180);
@@ -18,7 +18,7 @@ TEST(PIDInputOutputTest, ContinuousInput) {
}
TEST(PIDInputOutputTest, ProportionalGainOutput) {
frc::PIDController controller{0.0, 0.0, 0.0};
wpi::math::PIDController controller{0.0, 0.0, 0.0};
controller.SetP(4);
@@ -26,7 +26,7 @@ TEST(PIDInputOutputTest, ProportionalGainOutput) {
}
TEST(PIDInputOutputTest, IntegralGainOutput) {
frc::PIDController controller{0.0, 0.0, 0.0};
wpi::math::PIDController controller{0.0, 0.0, 0.0};
controller.SetI(4);
@@ -40,7 +40,7 @@ TEST(PIDInputOutputTest, IntegralGainOutput) {
}
TEST(PIDInputOutputTest, DerivativeGainOutput) {
frc::PIDController controller{0.0, 0.0, 0.0};
wpi::math::PIDController controller{0.0, 0.0, 0.0};
controller.SetD(4);
@@ -51,7 +51,7 @@ TEST(PIDInputOutputTest, DerivativeGainOutput) {
}
TEST(PIDInputOutputTest, IZoneNoOutput) {
frc::PIDController controller{0.0, 0.0, 0.0};
wpi::math::PIDController controller{0.0, 0.0, 0.0};
controller.SetI(1);
controller.SetIZone(1);
@@ -62,7 +62,7 @@ TEST(PIDInputOutputTest, IZoneNoOutput) {
}
TEST(PIDInputOutputTest, IZoneOutput) {
frc::PIDController controller{0.0, 0.0, 0.0};
wpi::math::PIDController controller{0.0, 0.0, 0.0};
controller.SetI(1);
controller.SetIZone(1);

4
wpimath/src/test/native/cpp/controller/PIDToleranceTest.cpp
View File

@@ -11,14 +11,14 @@ static constexpr double kRange = 200;
static constexpr double kTolerance = 10.0;
TEST(PIDToleranceTest, InitialTolerance) {
frc::PIDController controller{0.5, 0.0, 0.0};
wpi::math::PIDController controller{0.5, 0.0, 0.0};
controller.EnableContinuousInput(-kRange / 2, kRange / 2);
EXPECT_FALSE(controller.AtSetpoint());
}
TEST(PIDToleranceTest, AbsoluteTolerance) {
frc::PIDController controller{0.5, 0.0, 0.0};
wpi::math::PIDController controller{0.5, 0.0, 0.0};
controller.EnableContinuousInput(-kRange / 2, kRange / 2);
EXPECT_FALSE(controller.AtSetpoint());

62
wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp
View File

@@ -12,86 +12,86 @@
#include "wpi/units/angular_velocity.hpp"
TEST(ProfiledPIDInputOutputTest, ContinuousInput1) {
frc::ProfiledPIDController<units::degree> controller{
wpi::math::ProfiledPIDController<wpi::units::degree> controller{
0.0, 0.0, 0.0, {360_deg_per_s, 180_deg_per_s_sq}};
controller.SetP(1);
controller.EnableContinuousInput(-180_deg, 180_deg);
static constexpr units::degree_t kSetpoint{-179.0};
static constexpr units::degree_t kMeasurement{-179.0};
static constexpr units::degree_t kGoal{179.0};
static constexpr wpi::units::degree_t kSetpoint{-179.0};
static constexpr wpi::units::degree_t kMeasurement{-179.0};
static constexpr wpi::units::degree_t kGoal{179.0};
controller.Reset(kSetpoint);
EXPECT_LT(controller.Calculate(kMeasurement, kGoal), 0.0);
// Error must be less than half the input range at all times
EXPECT_LT(units::math::abs(controller.GetSetpoint().position - kMeasurement),
EXPECT_LT(wpi::units::math::abs(controller.GetSetpoint().position - kMeasurement),
180_deg);
}
TEST(ProfiledPIDInputOutputTest, ContinuousInput2) {
frc::ProfiledPIDController<units::degree> controller{
wpi::math::ProfiledPIDController<wpi::units::degree> controller{
0.0, 0.0, 0.0, {360_deg_per_s, 180_deg_per_s_sq}};
controller.SetP(1);
controller.EnableContinuousInput(-units::radian_t{std::numbers::pi},
units::radian_t{std::numbers::pi});
controller.EnableContinuousInput(-wpi::units::radian_t{std::numbers::pi},
wpi::units::radian_t{std::numbers::pi});
static constexpr units::radian_t kSetpoint{-3.4826633343199735};
static constexpr units::radian_t kMeasurement{-3.1352207333939606};
static constexpr units::radian_t kGoal{-3.534162788601621};
static constexpr wpi::units::radian_t kSetpoint{-3.4826633343199735};
static constexpr wpi::units::radian_t kMeasurement{-3.1352207333939606};
static constexpr wpi::units::radian_t kGoal{-3.534162788601621};
controller.Reset(kSetpoint);
EXPECT_LT(controller.Calculate(kMeasurement, kGoal), 0.0);
// Error must be less than half the input range at all times
EXPECT_LT(units::math::abs(controller.GetSetpoint().position - kMeasurement),
units::radian_t{std::numbers::pi});
EXPECT_LT(wpi::units::math::abs(controller.GetSetpoint().position - kMeasurement),
wpi::units::radian_t{std::numbers::pi});
}
TEST(ProfiledPIDInputOutputTest, ContinuousInput3) {
frc::ProfiledPIDController<units::degree> controller{
wpi::math::ProfiledPIDController<wpi::units::degree> controller{
0.0, 0.0, 0.0, {360_deg_per_s, 180_deg_per_s_sq}};
controller.SetP(1);
controller.EnableContinuousInput(-units::radian_t{std::numbers::pi},
units::radian_t{std::numbers::pi});
controller.EnableContinuousInput(-wpi::units::radian_t{std::numbers::pi},
wpi::units::radian_t{std::numbers::pi});
static constexpr units::radian_t kSetpoint{-3.5176604690006377};
static constexpr units::radian_t kMeasurement{3.1191729343822456};
static constexpr units::radian_t kGoal{2.709680418117445};
static constexpr wpi::units::radian_t kSetpoint{-3.5176604690006377};
static constexpr wpi::units::radian_t kMeasurement{3.1191729343822456};
static constexpr wpi::units::radian_t kGoal{2.709680418117445};
controller.Reset(kSetpoint);
EXPECT_LT(controller.Calculate(kMeasurement, kGoal), 0.0);
// Error must be less than half the input range at all times
EXPECT_LT(units::math::abs(controller.GetSetpoint().position - kMeasurement),
units::radian_t{std::numbers::pi});
EXPECT_LT(wpi::units::math::abs(controller.GetSetpoint().position - kMeasurement),
wpi::units::radian_t{std::numbers::pi});
}
TEST(ProfiledPIDInputOutputTest, ContinuousInput4) {
frc::ProfiledPIDController<units::degree> controller{
wpi::math::ProfiledPIDController<wpi::units::degree> controller{
0.0, 0.0, 0.0, {360_deg_per_s, 180_deg_per_s_sq}};
controller.SetP(1);
controller.EnableContinuousInput(0_rad,
units::radian_t{2.0 * std::numbers::pi});
wpi::units::radian_t{2.0 * std::numbers::pi});
static constexpr units::radian_t kSetpoint{2.78};
static constexpr units::radian_t kMeasurement{3.12};
static constexpr units::radian_t kGoal{2.71};
static constexpr wpi::units::radian_t kSetpoint{2.78};
static constexpr wpi::units::radian_t kMeasurement{3.12};
static constexpr wpi::units::radian_t kGoal{2.71};
controller.Reset(kSetpoint);
EXPECT_LT(controller.Calculate(kMeasurement, kGoal), 0.0);
// Error must be less than half the input range at all times
EXPECT_LT(units::math::abs(controller.GetSetpoint().position - kMeasurement),
units::radian_t{std::numbers::pi});
EXPECT_LT(wpi::units::math::abs(controller.GetSetpoint().position - kMeasurement),
wpi::units::radian_t{std::numbers::pi});
}
TEST(ProfiledPIDInputOutputTest, ProportionalGainOutput) {
frc::ProfiledPIDController<units::degree> controller{
wpi::math::ProfiledPIDController<wpi::units::degree> controller{
0.0, 0.0, 0.0, {360_deg_per_s, 180_deg_per_s_sq}};
controller.SetP(4);
@@ -100,7 +100,7 @@ TEST(ProfiledPIDInputOutputTest, ProportionalGainOutput) {
}
TEST(ProfiledPIDInputOutputTest, IntegralGainOutput) {
frc::ProfiledPIDController<units::degree> controller{
wpi::math::ProfiledPIDController<wpi::units::degree> controller{
0.0, 0.0, 0.0, {360_deg_per_s, 180_deg_per_s_sq}};
controller.SetI(4);
@@ -115,7 +115,7 @@ TEST(ProfiledPIDInputOutputTest, IntegralGainOutput) {
}
TEST(ProfiledPIDInputOutputTest, DerivativeGainOutput) {
frc::ProfiledPIDController<units::degree> controller{
wpi::math::ProfiledPIDController<wpi::units::degree> controller{
0.0, 0.0, 0.0, {360_deg_per_s, 180_deg_per_s_sq}};
controller.SetD(4);

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

@@ -14,19 +14,19 @@
#include "wpi/units/time.hpp"
#include "wpi/units/velocity.hpp"
namespace frc {
namespace wpi::math {
TEST(SimpleMotorFeedforwardTest, Calculate) {
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 = 20_ms;
constexpr wpi::units::second_t dt = 20_ms;
constexpr Matrixd<1, 1> A{{-Kv.value() / Ka.value()}};
constexpr Matrixd<1, 1> B{{1.0 / Ka.value()}};
frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt};
frc::SimpleMotorFeedforward<units::meter> simpleMotor{Ks, Kv, Ka};
wpi::math::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt};
wpi::math::SimpleMotorFeedforward<wpi::units::meter> simpleMotor{Ks, Kv, Ka};
constexpr Vectord<1> r{{2.0}};
constexpr Vectord<1> nextR{{3.0}};
@@ -46,11 +46,11 @@ 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};
constexpr wpi::units::second_t dt = 0_ms;
wpi::math::SimpleMotorFeedforward<wpi::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
} // namespace wpi::math

6
wpimath/src/test/native/cpp/controller/proto/ArmFeedforwardProtoTest.cpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/controller/ArmFeedforward.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
@@ -19,8 +19,8 @@ const ArmFeedforward kExpectedData{Ks, Kg, Kv, Ka};
} // namespace
TEST(ArmFeedforwardProtoTest, Roundtrip) {
wpi::ProtobufMessage<decltype(kExpectedData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, kExpectedData));
auto unpacked_data = message.Unpack(buf);

2
wpimath/src/test/native/cpp/controller/proto/DifferentialDriveFeedforwardProtoTest.cpp
View File

@@ -7,7 +7,7 @@
#include "../../ProtoTestBase.hpp"
#include "wpi/math/controller/DifferentialDriveFeedforward.hpp"
using namespace frc;
using namespace wpi::math;
struct DifferentialDriveFeedforwardProtoTestData {
using Type = DifferentialDriveFeedforward;

8
wpimath/src/test/native/cpp/controller/proto/DifferentialDriveWheelVoltagesProtoTest.cpp
View File

@@ -7,19 +7,19 @@
#include "wpi/math/controller/DifferentialDriveWheelVoltages.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using ProtoType = wpi::Protobuf<frc::DifferentialDriveWheelVoltages>;
using ProtoType = wpi::util::Protobuf<wpi::math::DifferentialDriveWheelVoltages>;
const DifferentialDriveWheelVoltages kExpectedData =
DifferentialDriveWheelVoltages{0.174_V, 0.191_V};
} // namespace
TEST(DifferentialDriveWheelVoltagesProtoTest, Roundtrip) {
wpi::ProtobufMessage<decltype(kExpectedData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, kExpectedData));
auto unpacked_data = message.Unpack(buf);

6
wpimath/src/test/native/cpp/controller/proto/ElevatorFeedforwardProtoTest.cpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/controller/ElevatorFeedforward.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
@@ -20,8 +20,8 @@ constexpr ElevatorFeedforward kExpectedData{Ks, Kg, Kv, Ka};
} // namespace
TEST(ElevatorFeedforwardProtoTest, Roundtrip) {
wpi::ProtobufMessage<decltype(kExpectedData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, kExpectedData));
auto unpacked_data = message.Unpack(buf);

12
wpimath/src/test/native/cpp/controller/proto/SimpleMotorFeedforwardProtoTest.cpp
View File

@@ -10,15 +10,15 @@
#include "wpi/units/acceleration.hpp"
#include "wpi/units/velocity.hpp"
using namespace frc;
using namespace wpi::math;
template <typename T>
struct SimpleMotorFeedforwardProtoTestData {
using Type = SimpleMotorFeedforward<T>;
inline static const Type kTestData = {
units::volt_t{0.4}, units::volt_t{4.0} / (units::unit_t<T>{1} / 1_s),
units::volt_t{0.7} / (units::unit_t<T>{1} / 1_s / 1_s), 25_ms};
wpi::units::volt_t{0.4}, wpi::units::volt_t{4.0} / (wpi::units::unit_t<T>{1} / 1_s),
wpi::units::volt_t{0.7} / (wpi::units::unit_t<T>{1} / 1_s / 1_s), 25_ms};
static void CheckEq(const Type& testData, const Type& data) {
EXPECT_EQ(testData.GetKs().value(), data.GetKs().value());
@@ -30,10 +30,10 @@ struct SimpleMotorFeedforwardProtoTestData {
INSTANTIATE_TYPED_TEST_SUITE_P(
SimpleMotorFeedforwardMeters, ProtoTest,
SimpleMotorFeedforwardProtoTestData<units::meters>);
SimpleMotorFeedforwardProtoTestData<wpi::units::meters>);
INSTANTIATE_TYPED_TEST_SUITE_P(
SimpleMotorFeedforwardFeet, ProtoTest,
SimpleMotorFeedforwardProtoTestData<units::feet>);
SimpleMotorFeedforwardProtoTestData<wpi::units::feet>);
INSTANTIATE_TYPED_TEST_SUITE_P(
SimpleMotorFeedforwardRadians, ProtoTest,
SimpleMotorFeedforwardProtoTestData<units::radians>);
SimpleMotorFeedforwardProtoTestData<wpi::units::radians>);

4
wpimath/src/test/native/cpp/controller/struct/ArmFeedforwardStructTest.cpp
View File

@@ -6,11 +6,11 @@
#include "wpi/math/controller/ArmFeedforward.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using StructType = wpi::Struct<frc::ArmFeedforward>;
using StructType = wpi::util::Struct<wpi::math::ArmFeedforward>;
static constexpr auto Ks = 1.91_V;
static constexpr auto Kg = 2.29_V;

2
wpimath/src/test/native/cpp/controller/struct/DifferentialDriveFeedforwardStructTest.cpp
View File

@@ -7,7 +7,7 @@
#include "../../StructTestBase.hpp"
#include "wpi/math/controller/DifferentialDriveFeedforward.hpp"
using namespace frc;
using namespace wpi::math;
struct DifferentialDriveFeedforwardStructTestData {
using Type = DifferentialDriveFeedforward;

4
wpimath/src/test/native/cpp/controller/struct/DifferentialDriveWheelVoltagesStructTest.cpp
View File

@@ -6,11 +6,11 @@
#include "wpi/math/controller/DifferentialDriveWheelVoltages.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using StructType = wpi::Struct<frc::DifferentialDriveWheelVoltages>;
using StructType = wpi::util::Struct<wpi::math::DifferentialDriveWheelVoltages>;
const DifferentialDriveWheelVoltages kExpectedData{
DifferentialDriveWheelVoltages{0.174_V, 0.191_V}};
} // namespace

4
wpimath/src/test/native/cpp/controller/struct/ElevatorFeedforwardStructTest.cpp
View File

@@ -6,11 +6,11 @@
#include "wpi/math/controller/ElevatorFeedforward.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using StructType = wpi::Struct<frc::ElevatorFeedforward>;
using StructType = wpi::util::Struct<wpi::math::ElevatorFeedforward>;
static constexpr auto Ks = 1.91_V;
static constexpr auto Kg = 2.29_V;

12
wpimath/src/test/native/cpp/controller/struct/SimpleMotorFeedforwardStructTest.cpp
View File

@@ -10,15 +10,15 @@
#include "wpi/units/acceleration.hpp"
#include "wpi/units/velocity.hpp"
using namespace frc;
using namespace wpi::math;
template <typename T>
struct SimpleMotorFeedforwardStructTestData {
using Type = SimpleMotorFeedforward<T>;
inline static const Type kTestData = {
units::volt_t{0.4}, units::volt_t{4.0} / (units::unit_t<T>{1} / 1_s),
units::volt_t{0.7} / (units::unit_t<T>{1} / 1_s / 1_s), 25_ms};
wpi::units::volt_t{0.4}, wpi::units::volt_t{4.0} / (wpi::units::unit_t<T>{1} / 1_s),
wpi::units::volt_t{0.7} / (wpi::units::unit_t<T>{1} / 1_s / 1_s), 25_ms};
static void CheckEq(const Type& testData, const Type& data) {
EXPECT_EQ(testData.GetKs().value(), data.GetKs().value());
@@ -30,10 +30,10 @@ struct SimpleMotorFeedforwardStructTestData {
INSTANTIATE_TYPED_TEST_SUITE_P(
SimpleMotorFeedforwardMeters, StructTest,
SimpleMotorFeedforwardStructTestData<units::meters>);
SimpleMotorFeedforwardStructTestData<wpi::units::meters>);
INSTANTIATE_TYPED_TEST_SUITE_P(
SimpleMotorFeedforwardFeet, StructTest,
SimpleMotorFeedforwardStructTestData<units::feet>);
SimpleMotorFeedforwardStructTestData<wpi::units::feet>);
INSTANTIATE_TYPED_TEST_SUITE_P(
SimpleMotorFeedforwardRadians, StructTest,
SimpleMotorFeedforwardStructTestData<units::radians>);
SimpleMotorFeedforwardStructTestData<wpi::units::radians>);