Split StatespaceLoop into a Plant, Controller, and Observer.
This doesn't yet move any of the logic out of the Loop.
Change-Id: I2cb0ea6d1a75c7011576ba752c50e512eeff5890
diff --git a/y2014/control_loops/shooter/shooter.cc b/y2014/control_loops/shooter/shooter.cc
index 9c6b182..70a6cad 100644
--- a/y2014/control_loops/shooter/shooter.cc
+++ b/y2014/control_loops/shooter/shooter.cc
@@ -58,11 +58,12 @@
double dx;
if (index() == 0) {
dx = (uncapped_voltage() - max_voltage_) /
- (K(0, 0) - plant().A(1, 0) * K(0, 2) / plant().A(1, 2));
+ (controller().K(0, 0) -
+ plant().A(1, 0) * controller().K(0, 2) / plant().A(1, 2));
mutable_R(0, 0) -= dx;
mutable_R(2, 0) -= -plant().A(1, 0) / plant().A(1, 2) * dx;
} else {
- dx = (uncapped_voltage() - max_voltage_) / K(0, 0);
+ dx = (uncapped_voltage() - max_voltage_) / controller().K(0, 0);
mutable_R(0, 0) -= dx;
}
capped_goal_ = true;
@@ -72,11 +73,12 @@
double dx;
if (index() == 0) {
dx = (uncapped_voltage() + max_voltage_) /
- (K(0, 0) - plant().A(1, 0) * K(0, 2) / plant().A(1, 2));
+ (controller().K(0, 0) -
+ plant().A(1, 0) * controller().K(0, 2) / plant().A(1, 2));
mutable_R(0, 0) -= dx;
mutable_R(2, 0) -= -plant().A(1, 0) / plant().A(1, 2) * dx;
} else {
- dx = (uncapped_voltage() + max_voltage_) / K(0, 0);
+ dx = (uncapped_voltage() + max_voltage_) / controller().K(0, 0);
mutable_R(0, 0) -= dx;
}
capped_goal_ = true;