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/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 91e7ae7..4d09bbc 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -259,8 +259,10 @@
status->estimator_state = this->EstimatorState(0);
Eigen::Matrix<double, 3, 1> error = this->controller().error();
- status->position_power = this->controller().K(0, 0) * error(0, 0);
- status->velocity_power = this->controller().K(0, 1) * error(1, 0);
+ status->position_power =
+ this->controller().controller().K(0, 0) * error(0, 0);
+ status->velocity_power =
+ this->controller().controller().K(0, 1) * error(1, 0);
}
template <class ZeroingEstimator>