mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +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:
@@ -113,4 +113,26 @@ public class LinearQuadraticRegulatorTest {
|
||||
assertEquals(19.16, k.get(0, 0), 0.1);
|
||||
assertEquals(3.32, k.get(0, 1), 0.1);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testLatencyCompensate() {
|
||||
var dt = 0.02;
|
||||
|
||||
var plant = LinearSystemId.createElevatorSystem(
|
||||
DCMotor.getVex775Pro(4),
|
||||
8.0,
|
||||
0.75 * 25.4 / 1000.0,
|
||||
14.67);
|
||||
|
||||
var regulator = new LinearQuadraticRegulator<>(
|
||||
plant,
|
||||
VecBuilder.fill(0.1, 0.2),
|
||||
VecBuilder.fill(12.0),
|
||||
dt);
|
||||
|
||||
regulator.latencyCompensate(plant, dt, 0.01);
|
||||
|
||||
assertEquals(8.97115941, regulator.getK().get(0, 0), 1e-3);
|
||||
assertEquals(0.07904881, regulator.getK().get(0, 1), 1e-3);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user