mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
[wpilib] Add function to adjust LQR controller gain for pure time delay (#2878)
There were three options for where to put this function: 1. A free function in LinearQuadraticRegulator.h. Returning a K matrix means the user can't use the LinearQuadraticRegulator in a loop anymore. 2. A default argument added to ctors in LinearQuadraticRegulator for a time delay (default of 0). This has the smallest API footprint from the user perspective, but it bloats the already substantial constructor overload set of LinearQuadraticRegulator. 3. A member function in LinearQuadraticRegulator that modifies the internal K. This would still have to take in a LinearSystem or (A, B) pair because the ctor doesn't store it. Storing it internally feels like paying for what we don't use most of the time. I went with option 3. I verified the tests's expected values in Python with scipy.linalg.fractional_matrix_power(). Closes #2877.
This commit is contained in:
@@ -85,4 +85,27 @@ TEST(LinearQuadraticRegulatorTest, FourMotorElevator) {
|
||||
EXPECT_NEAR(0.69, controller.K(0, 1), 1e-1);
|
||||
}
|
||||
|
||||
TEST(LinearQuadraticRegulatorTest, LatencyCompensate) {
|
||||
LinearSystem<2, 1, 1> plant = [] {
|
||||
auto motors = DCMotor::Vex775Pro(4);
|
||||
|
||||
// Carriage mass
|
||||
constexpr auto m = 8_kg;
|
||||
|
||||
// Radius of pulley
|
||||
constexpr auto r = 0.75_in;
|
||||
|
||||
// Gear ratio
|
||||
constexpr double G = 14.67;
|
||||
|
||||
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
|
||||
}();
|
||||
LinearQuadraticRegulator<2, 1> controller{plant, {0.1, 0.2}, {12.0}, 0.02_s};
|
||||
|
||||
controller.LatencyCompensate(plant, 0.02_s, 0.01_s);
|
||||
|
||||
EXPECT_NEAR(8.97115941, controller.K(0, 0), 1e-3);
|
||||
EXPECT_NEAR(0.07904881, controller.K(0, 1), 1e-3);
|
||||
}
|
||||
|
||||
} // namespace frc
|
||||
|
||||
Reference in New Issue
Block a user