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/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 3bf0966..2366fb7 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -670,16 +670,22 @@
// Calculate the loops for a cycle.
{
Eigen::Matrix<double, 3, 1> error = intake_.controller().error();
- status->intake.position_power = intake_.controller().K(0, 0) * error(0, 0);
- status->intake.velocity_power = intake_.controller().K(0, 1) * error(1, 0);
+ status->intake.position_power =
+ intake_.controller().controller().K(0, 0) * error(0, 0);
+ status->intake.velocity_power =
+ intake_.controller().controller().K(0, 1) * error(1, 0);
}
{
Eigen::Matrix<double, 6, 1> error = arm_.controller().error();
- status->shoulder.position_power = arm_.controller().K(0, 0) * error(0, 0);
- status->shoulder.velocity_power = arm_.controller().K(0, 1) * error(1, 0);
- status->wrist.position_power = arm_.controller().K(0, 2) * error(2, 0);
- status->wrist.velocity_power = arm_.controller().K(0, 3) * error(3, 0);
+ status->shoulder.position_power =
+ arm_.controller().controller().K(0, 0) * error(0, 0);
+ status->shoulder.velocity_power =
+ arm_.controller().controller().K(0, 1) * error(1, 0);
+ status->wrist.position_power =
+ arm_.controller().controller().K(0, 2) * error(2, 0);
+ status->wrist.velocity_power =
+ arm_.controller().controller().K(0, 3) * error(3, 0);
}
arm_.Update(disable);
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index 95f04f8..5d8c85a 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -28,14 +28,16 @@
const Eigen::Matrix<double, 2, 1> ControllerOutput() override {
const Eigen::Matrix<double, 2, 1> accelerating_ff =
- controller(0).Kff * (next_R() - plant().coefficients(0).A * R());
+ controller().coefficients(0).Kff *
+ (next_R() - plant().coefficients(0).A * R());
const Eigen::Matrix<double, 2, 1> accelerating_controller =
- controller(0).K * error() + accelerating_ff;
+ controller().coefficients(0).K * error() + accelerating_ff;
const Eigen::Matrix<double, 2, 1> decelerating_ff =
- controller(1).Kff * (next_R() - plant().coefficients(1).A * R());
+ controller().coefficients(1).Kff *
+ (next_R() - plant().coefficients(1).A * R());
const Eigen::Matrix<double, 2, 1> decelerating_controller =
- controller(1).K * error() + decelerating_ff;
+ controller().coefficients(1).K * error() + decelerating_ff;
const double bemf_voltage = X_hat(1, 0) / kV_shoulder;
bool use_accelerating_controller = true;
@@ -66,18 +68,18 @@
if (U(0, 0) > max_voltage(0)) {
const double overage_amount = U(0, 0) - max_voltage(0);
mutable_U(0, 0) = max_voltage(0);
- const double coupled_amount =
- (Kff().block<1, 2>(1, 2) * plant().B().block<2, 1>(2, 0))(0, 0) *
- overage_amount;
+ const double coupled_amount = (controller().Kff().block<1, 2>(1, 2) *
+ plant().B().block<2, 1>(2, 0))(0, 0) *
+ overage_amount;
LOG(DEBUG, "Removing coupled amount %f\n", coupled_amount);
mutable_U(1, 0) += coupled_amount;
}
if (U(0, 0) < min_voltage(0)) {
const double under_amount = U(0, 0) - min_voltage(0);
mutable_U(0, 0) = min_voltage(0);
- const double coupled_amount =
- (Kff().block<1, 2>(1, 2) * plant().B().block<2, 1>(2, 0))(0, 0) *
- under_amount;
+ const double coupled_amount = (controller().Kff().block<1, 2>(1, 2) *
+ plant().B().block<2, 1>(2, 0))(0, 0) *
+ under_amount;
LOG(DEBUG, "Removing coupled amount %f\n", coupled_amount);
mutable_U(1, 0) += coupled_amount;
}