Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add full state support to LinearSystemId functions (#6554)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Nicholas Armstrong
2024-05-15 09:23:22 -04:00
committed by GitHub
parent 7fbbecb5b7
commit 7fc17811fa
29 changed files with 343 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wpimath/src/test/native/cpp/StateSpaceTest.cpp
View File

@@ -36,7 +36,7 @@ class StateSpaceTest : public testing::Test {
// Gear ratio
constexpr double G = 40.0 / 40.0;
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
}();
LinearQuadraticRegulator<2, 1> controller{plant, {0.02, 0.4}, {12.0}, kDt};
KalmanFilter<2, 1, 1> observer{plant, {0.05, 1.0}, {0.0001}, kDt};

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

@@ -28,7 +28,7 @@ TEST(LinearQuadraticRegulatorTest, ElevatorGains) {
// Gear ratio
constexpr double G = 40.0 / 40.0;
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
}();
Matrixd<1, 2> K =
LinearQuadraticRegulator<2, 1>{plant, {0.02, 0.4}, {12.0}, 5.05_ms}.K();
@@ -50,8 +50,9 @@ TEST(LinearQuadraticRegulatorTest, ArmGains) {
// Gear ratio
constexpr double G = 100.0;
return frc::LinearSystemId::SingleJointedArmSystem(
motors, 1.0 / 3.0 * m * r * r, G);
return frc::LinearSystemId::SingleJointedArmSystem(motors,
1.0 / 3.0 * m * r * r, G)
.Slice(0);
}();
Matrixd<1, 2> K =
@@ -75,7 +76,7 @@ TEST(LinearQuadraticRegulatorTest, FourMotorElevator) {
// Gear ratio
constexpr double G = 14.67;
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
return frc::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();
@@ -177,7 +178,7 @@ TEST(LinearQuadraticRegulatorTest, LatencyCompensate) {
// Gear ratio
constexpr double G = 14.67;
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
}();
LinearQuadraticRegulator<2, 1> controller{plant, {0.1, 0.2}, {12.0}, 0.02_s};

3
wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
View File

@@ -28,7 +28,8 @@ TEST(LinearSystemIDTest, IdentifyDrivetrainVelocitySystem) {
TEST(LinearSystemIDTest, ElevatorSystem) {
auto model = frc::LinearSystemId::ElevatorSystem(frc::DCMotor::NEO(2), 5_kg,
0.05_m, 12);
0.05_m, 12)
.Slice(0);
ASSERT_TRUE(model.A().isApprox(
frc::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -99.05473}}, 0.001));
ASSERT_TRUE(model.B().isApprox(frc::Matrixd<2, 1>{0.0, 20.8}, 0.001));