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

[wpimath] Add typedefs for common types

Browse Source 
This makes complex code significantly easier to read.

frc::Vectord<Size> = Eigen::Vector<double, Size>
frc::Matrixd<Rows, Cols> = Eigen::Matrix<double, Rows, Cols>
This commit is contained in:
Peter Johnson
2022-04-29 22:29:20 -07:00
parent 97c493241f
commit e767605e94
76 changed files with 1136 additions and 1449 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -24,40 +24,37 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
DifferentialDriveAccelerationLimiter accelLimiter{plant, trackwidth, maxA,
maxAlpha};
Eigen::Vector<double, 2> x{0.0, 0.0};
Eigen::Vector<double, 2> xAccelLimiter{0.0, 0.0};
Vectord<2> x{0.0, 0.0};
Vectord<2> xAccelLimiter{0.0, 0.0};
// Ensure voltage exceeds acceleration before limiting
{
Eigen::Vector<double, 2> accels =
plant.A() * xAccelLimiter +
plant.B() * Eigen::Vector<double, 2>{12.0, 12.0};
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);
}
{
Eigen::Vector<double, 2> accels =
plant.A() * xAccelLimiter +
plant.B() * Eigen::Vector<double, 2>{-12.0, 12.0};
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)) /
trackwidth.value()};
EXPECT_GT(units::math::abs(alpha), maxAlpha);
}
// Forward
Eigen::Vector<double, 2> u{12.0, 12.0};
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(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
xAccelLimiter = plant.CalculateX(xAccelLimiter,
Eigen::Vector<double, 2>{left, right}, dt);
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
Eigen::Vector<double, 2> accels =
plant.A() * xAccelLimiter +
plant.B() * Eigen::Vector<double, 2>{left, right};
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)) /
trackwidth.value()};
@@ -66,19 +63,18 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
}
// Backward
u = Eigen::Vector<double, 2>{-12.0, -12.0};
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(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
xAccelLimiter = plant.CalculateX(xAccelLimiter,
Eigen::Vector<double, 2>{left, right}, dt);
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
Eigen::Vector<double, 2> accels =
plant.A() * xAccelLimiter +
plant.B() * Eigen::Vector<double, 2>{left, right};
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)) /
trackwidth.value()};
@@ -87,19 +83,18 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
}
// Rotate CCW
u = Eigen::Vector<double, 2>{-12.0, 12.0};
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(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
xAccelLimiter = plant.CalculateX(xAccelLimiter,
Eigen::Vector<double, 2>{left, right}, dt);
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
Eigen::Vector<double, 2> accels =
plant.A() * xAccelLimiter +
plant.B() * Eigen::Vector<double, 2>{left, right};
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)) /
trackwidth.value()};
@@ -123,19 +118,19 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
DifferentialDriveAccelerationLimiter accelLimiter{
plant, trackwidth, 1e3_mps_sq, 1e3_rad_per_s_sq};
Eigen::Vector<double, 2> x{0.0, 0.0};
Eigen::Vector<double, 2> xAccelLimiter{0.0, 0.0};
Vectord<2> x{0.0, 0.0};
Vectord<2> xAccelLimiter{0.0, 0.0};
// Forward
Eigen::Vector<double, 2> u{12.0, 12.0};
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(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
xAccelLimiter = plant.CalculateX(xAccelLimiter,
Eigen::Vector<double, 2>{left, right}, dt);
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
EXPECT_DOUBLE_EQ(x(0), xAccelLimiter(0));
EXPECT_DOUBLE_EQ(x(1), xAccelLimiter(1));
@@ -144,15 +139,15 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
// Backward
x.setZero();
xAccelLimiter.setZero();
u = Eigen::Vector<double, 2>{-12.0, -12.0};
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(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
xAccelLimiter = plant.CalculateX(xAccelLimiter,
Eigen::Vector<double, 2>{left, right}, dt);
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
EXPECT_DOUBLE_EQ(x(0), xAccelLimiter(0));
EXPECT_DOUBLE_EQ(x(1), xAccelLimiter(1));
@@ -161,15 +156,15 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
// Rotate CCW
x.setZero();
xAccelLimiter.setZero();
u = Eigen::Vector<double, 2>{-12.0, 12.0};
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(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
xAccelLimiter = plant.CalculateX(xAccelLimiter,
Eigen::Vector<double, 2>{left, right}, dt);
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
EXPECT_DOUBLE_EQ(x(0), xAccelLimiter(0));
EXPECT_DOUBLE_EQ(x(1), xAccelLimiter(1));