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

[wpimath] Remove LinearSystem from LinearSystemLoop (#2968)

Browse Source 
The system wasn't being used internally, and as LinearSystem is stateless, it doesn't need to be held by LinearSystemLoop.
This commit is contained in:
Matt
2020-12-28 10:35:51 -08:00
committed by GitHub
parent aa89744c95
commit 254931b9a8
4 changed files with 43 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files

44
wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearSystemLoopTest.java
View File

@@ -29,29 +29,25 @@ public class LinearSystemLoopTest {
public static final double kDt = 0.00505;
private static final double kPositionStddev = 0.0001;
private static final Random random = new Random();
private final LinearSystemLoop<N2, N1, N1> m_loop;
LinearSystem<N2, N1, N1> m_plant = LinearSystemId.createElevatorSystem(DCMotor.getVex775Pro(2), 5,
0.0181864, 1.0);
KalmanFilter<N2, N1, N1> m_observer = new KalmanFilter<>(Nat.N2(), Nat.N1(), m_plant,
VecBuilder.fill(0.05, 1.0),
VecBuilder.fill(0.0001), kDt);
LinearQuadraticRegulator<N2, N1, N1> m_controller = new LinearQuadraticRegulator<>(
m_plant, VecBuilder.fill(0.02, 0.4), VecBuilder.fill(12.0),
0.00505);
private final LinearSystemLoop<N2, N1, N1> m_loop =
new LinearSystemLoop<>(m_plant, m_controller, m_observer, 12, 0.00505);
@SuppressWarnings("LocalVariableName")
public LinearSystemLoopTest() {
LinearSystem<N2, N1, N1> plant = LinearSystemId.createElevatorSystem(DCMotor.getVex775Pro(2), 5,
0.0181864, 1.0);
KalmanFilter<N2, N1, N1> observer = new KalmanFilter<>(Nat.N2(), Nat.N1(), plant,
VecBuilder.fill(0.05, 1.0),
VecBuilder.fill(0.0001), kDt);
var qElms = VecBuilder.fill(0.02, 0.4);
var rElms = VecBuilder.fill(12.0);
var dt = 0.00505;
var controller = new LinearQuadraticRegulator<>(
plant, qElms, rElms, dt);
m_loop = new LinearSystemLoop<>(plant, controller, observer, 12, dt);
}
@SuppressWarnings("LocalVariableName")
private static void updateTwoState(LinearSystemLoop<N2, N1, N1> loop, double noise) {
Matrix<N1, N1> y = loop.getPlant().calculateY(loop.getXHat(), loop.getU()).plus(
private static void updateTwoState(LinearSystem<N2, N1, N1> plant, LinearSystemLoop<N2, N1, N1>
loop, double noise) {
Matrix<N1, N1> y = plant.calculateY(loop.getXHat(), loop.getU()).plus(
VecBuilder.fill(noise)
);
@@ -78,7 +74,7 @@ public class LinearSystemLoopTest {
state = profile.calculate(kDt);
m_loop.setNextR(VecBuilder.fill(state.position, state.velocity));
updateTwoState(m_loop, (random.nextGaussian()) * kPositionStddev);
updateTwoState(m_plant, m_loop, (random.nextGaussian()) * kPositionStddev);
var u = m_loop.getU(0);
assertTrue(u >= -12.1 && u <= 12.1, "U out of bounds! Got " + u);
@@ -107,7 +103,7 @@ public class LinearSystemLoopTest {
var feedforward = new LinearPlantInversionFeedforward<>(plant, kDt);
var loop = new LinearSystemLoop<>(plant, controller, feedforward, observer, 12);
var loop = new LinearSystemLoop<>(controller, feedforward, observer, 12);
loop.reset(VecBuilder.fill(0.0));
var references = VecBuilder.fill(3000 / 60d * 2 * Math.PI);
@@ -129,7 +125,7 @@ public class LinearSystemLoopTest {
loop.setNextR(references);
Matrix<N1, N1> y = loop.getPlant().calculateY(loop.getXHat(), loop.getU()).plus(
Matrix<N1, N1> y = plant.calculateY(loop.getXHat(), loop.getU()).plus(
VecBuilder.fill(random.nextGaussian() * kPositionStddev)
);