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: wpiformat

Browse Source
This commit is contained in:
PJ Reiniger
2025-11-07 20:01:58 -05:00
committed by Peter Johnson
parent ae6bdc9d25
commit 2109161534
749 changed files with 5504 additions and 3936 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -35,18 +35,20 @@ using Kg_unit = decltype(1_V);
* @param dt The simulation time.
* @return The final state as a 2-vector of angle and angular velocity.
*/
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, 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 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))};
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;
},
wpi::math::Matrixd<2, 1>{currentAngle.value(), currentVelocity.value()},

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

@@ -21,7 +21,7 @@ Vectord<2> StateDynamics(const Vectord<2>& x) {
TEST(ControlAffinePlantInversionFeedforwardTest, Calculate) {
wpi::math::ControlAffinePlantInversionFeedforward<2, 1> feedforward{&Dynamics,
20_ms};
20_ms};
Vectord<2> r{2, 2};
Vectord<2> nextR{3, 3};

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

@@ -38,7 +38,7 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
Vectord<2> accels =
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{-12.0, 12.0};
wpi::units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
trackwidth.value()};
trackwidth.value()};
EXPECT_GT(wpi::units::math::abs(alpha), maxAlpha);
}
@@ -46,10 +46,10 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
Vectord<2> u{12.0, 12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
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)});
auto [left, right] = 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);
@@ -57,7 +57,7 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
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()};
trackwidth.value()};
EXPECT_LE(wpi::units::math::abs(a), maxA);
EXPECT_LE(wpi::units::math::abs(alpha), maxAlpha);
}
@@ -66,10 +66,10 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
u = Vectord<2>{-12.0, -12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
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)});
auto [left, right] = 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);
@@ -77,7 +77,7 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
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()};
trackwidth.value()};
EXPECT_LE(wpi::units::math::abs(a), maxA);
EXPECT_LE(wpi::units::math::abs(alpha), maxAlpha);
}
@@ -86,10 +86,10 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
u = Vectord<2>{-12.0, 12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
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)});
auto [left, right] = 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);
@@ -97,7 +97,7 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
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()};
trackwidth.value()};
EXPECT_LE(wpi::units::math::abs(a), maxA);
EXPECT_LE(wpi::units::math::abs(alpha), maxAlpha);
}
@@ -125,10 +125,10 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
Vectord<2> u{12.0, 12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
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)});
auto [left, right] = 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);
@@ -142,10 +142,10 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
u = Vectord<2>{-12.0, -12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
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)});
auto [left, right] = 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);
@@ -159,10 +159,10 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
u = Vectord<2>{-12.0, 12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
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)});
auto [left, right] = 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);
@@ -203,10 +203,10 @@ TEST(DifferentialDriveAccelerationLimiterTest, SeparateMinMaxLowLimits) {
Vectord<2> u{12.0, 12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
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)});
auto [left, right] = 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);
@@ -221,10 +221,10 @@ TEST(DifferentialDriveAccelerationLimiterTest, SeparateMinMaxLowLimits) {
u = Vectord<2>{-12.0, -12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
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)});
auto [left, right] = 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);

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

@@ -60,7 +60,7 @@ TEST(DifferentialDriveFeedforwardTest, CalculateWithoutTrackwidth) {
kVLinear, kALinear, kVAngular, kAAngular};
wpi::math::LinearSystem<2, 2, 2> plant =
wpi::math::LinearSystemId::IdentifyDrivetrainSystem(kVLinear, kALinear,
kVAngular, kAAngular);
kVAngular, kAAngular);
for (auto currentLeftVelocity = -4_mps; currentLeftVelocity <= 4_mps;
currentLeftVelocity += 2_mps) {
for (auto currentRightVelocity = -4_mps; currentRightVelocity <= 4_mps;

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

@@ -18,7 +18,7 @@
static constexpr wpi::units::meter_t kTolerance{1 / 12.0};
static constexpr wpi::units::radian_t kAngularTolerance{2.0 * std::numbers::pi /
180.0};
180.0};
/**
* States of the drivetrain system.
@@ -49,7 +49,8 @@ static auto plant = wpi::math::LinearSystemId::IdentifyDrivetrainSystem(
kLinearV, kLinearA, kAngularV, kAngularA);
static constexpr auto kTrackwidth = 0.9_m;
wpi::math::Vectord<5> Dynamics(const wpi::math::Vectord<5>& x, const wpi::math::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;
wpi::math::Vectord<5> xdot;
@@ -81,22 +82,22 @@ TEST(LTVDifferentialDriveControllerTest, ReachesReference) {
auto totalTime = trajectory.TotalTime();
for (size_t i = 0; i < (totalTime / kDt).value(); ++i) {
auto state = trajectory.Sample(kDt * i);
robotPose =
wpi::math::Pose2d{wpi::units::meter_t{x(State::kX)}, wpi::units::meter_t{x(State::kY)},
wpi::units::radian_t{x(State::kHeading)}};
robotPose = 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, wpi::units::meters_per_second_t{x(State::kLeftVelocity)},
wpi::units::meters_per_second_t{x(State::kRightVelocity)}, state);
x = wpi::math::RKDP(&Dynamics, x,
wpi::math::Vectord<2>{leftVoltage.value(), rightVoltage.value()},
kDt);
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(wpi::math::AngleModulus(endPose.Rotation().Radians() -
robotPose.Rotation().Radians()),
robotPose.Rotation().Radians()),
0_rad, kAngularTolerance);
}

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

@@ -14,12 +14,13 @@
static constexpr wpi::units::meter_t kTolerance{1 / 12.0};
static constexpr wpi::units::radian_t kAngularTolerance{2.0 * std::numbers::pi /
180.0};
180.0};
TEST(LTVUnicycleControllerTest, ReachesReference) {
constexpr wpi::units::second_t kDt = 20_ms;
wpi::math::LTVUnicycleController controller{{0.0625, 0.125, 2.5}, {4.0, 4.0}, kDt};
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{wpi::math::Pose2d{2.75_m, 22.521_m, 0_rad},
@@ -33,13 +34,14 @@ TEST(LTVUnicycleControllerTest, ReachesReference) {
auto [vx, vy, omega] = controller.Calculate(robotPose, state);
static_cast<void>(vy);
robotPose = robotPose + wpi::math::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(wpi::math::AngleModulus(endPose.Rotation().Radians() -
robotPose.Rotation().Radians()),
robotPose.Rotation().Radians()),
0_rad, kAngularTolerance);
}

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

@@ -50,8 +50,8 @@ TEST(LinearQuadraticRegulatorTest, ArmGains) {
// Gear ratio
constexpr double G = 100.0;
return wpi::math::LinearSystemId::SingleJointedArmSystem(motors,
1.0 / 3.0 * m * r * r, G)
return wpi::math::LinearSystemId::SingleJointedArmSystem(
motors, 1.0 / 3.0 * m * r * r, G)
.Slice(0);
}();

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

@@ -26,8 +26,9 @@ TEST(ProfiledPIDInputOutputTest, ContinuousInput1) {
EXPECT_LT(controller.Calculate(kMeasurement, kGoal), 0.0);
// Error must be less than half the input range at all times
EXPECT_LT(wpi::units::math::abs(controller.GetSetpoint().position - kMeasurement),
180_deg);
EXPECT_LT(
wpi::units::math::abs(controller.GetSetpoint().position - kMeasurement),
180_deg);
}
TEST(ProfiledPIDInputOutputTest, ContinuousInput2) {
@@ -46,8 +47,9 @@ TEST(ProfiledPIDInputOutputTest, ContinuousInput2) {
EXPECT_LT(controller.Calculate(kMeasurement, kGoal), 0.0);
// Error must be less than half the input range at all times
EXPECT_LT(wpi::units::math::abs(controller.GetSetpoint().position - kMeasurement),
wpi::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) {
@@ -66,8 +68,9 @@ TEST(ProfiledPIDInputOutputTest, ContinuousInput3) {
EXPECT_LT(controller.Calculate(kMeasurement, kGoal), 0.0);
// Error must be less than half the input range at all times
EXPECT_LT(wpi::units::math::abs(controller.GetSetpoint().position - kMeasurement),
wpi::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) {
@@ -75,8 +78,8 @@ TEST(ProfiledPIDInputOutputTest, ContinuousInput4) {
0.0, 0.0, 0.0, {360_deg_per_s, 180_deg_per_s_sq}};
controller.SetP(1);
controller.EnableContinuousInput(0_rad,
wpi::units::radian_t{2.0 * std::numbers::pi});
controller.EnableContinuousInput(
0_rad, wpi::units::radian_t{2.0 * std::numbers::pi});
static constexpr wpi::units::radian_t kSetpoint{2.78};
static constexpr wpi::units::radian_t kMeasurement{3.12};
@@ -86,8 +89,9 @@ TEST(ProfiledPIDInputOutputTest, ContinuousInput4) {
EXPECT_LT(controller.Calculate(kMeasurement, kGoal), 0.0);
// Error must be less than half the input range at all times
EXPECT_LT(wpi::units::math::abs(controller.GetSetpoint().position - kMeasurement),
wpi::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) {

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

@@ -47,7 +47,8 @@ TEST(SimpleMotorFeedforwardTest, NegativeGains) {
constexpr auto Kv = -3_V / 1_mps;
constexpr auto Ka = -0.6_V / 1_mps_sq;
constexpr wpi::units::second_t dt = 0_ms;
wpi::math::SimpleMotorFeedforward<wpi::units::meter> simpleMotor{Ks, Kv, Ka, dt};
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);

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

@@ -11,7 +11,8 @@ using namespace wpi::math;
namespace {
using ProtoType = wpi::util::Protobuf<wpi::math::DifferentialDriveWheelVoltages>;
using ProtoType =
wpi::util::Protobuf<wpi::math::DifferentialDriveWheelVoltages>;
const DifferentialDriveWheelVoltages kExpectedData =
DifferentialDriveWheelVoltages{0.174_V, 0.191_V};

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

@@ -17,7 +17,8 @@ struct SimpleMotorFeedforwardProtoTestData {
using Type = SimpleMotorFeedforward<T>;
inline static const Type kTestData = {
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.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) {

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

@@ -17,7 +17,8 @@ struct SimpleMotorFeedforwardStructTestData {
using Type = SimpleMotorFeedforward<T>;
inline static const Type kTestData = {
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.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) {