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/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index b7a68a9..4aeb74b 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -35,9 +35,11 @@
LOG_MATRIX(DEBUG, "U_uncapped", *U);
Eigen::Matrix<double, 2, 2> position_K;
- position_K << kf_->K(0, 0), kf_->K(0, 2), kf_->K(1, 0), kf_->K(1, 2);
+ position_K << kf_->controller().K(0, 0), kf_->controller().K(0, 2),
+ kf_->controller().K(1, 0), kf_->controller().K(1, 2);
Eigen::Matrix<double, 2, 2> velocity_K;
- velocity_K << kf_->K(0, 1), kf_->K(0, 3), kf_->K(1, 1), kf_->K(1, 3);
+ velocity_K << kf_->controller().K(0, 1), kf_->controller().K(0, 3),
+ kf_->controller().K(1, 1), kf_->controller().K(1, 3);
Eigen::Matrix<double, 2, 1> position_error;
position_error << error(0, 0), error(2, 0);
@@ -144,7 +146,7 @@
goal.right_velocity_goal, 0.0, 0.0, 0.0;
use_profile_ =
- !kf_->Kff().isZero(0) &&
+ !kf_->controller().Kff().isZero(0) &&
(goal.linear.max_velocity != 0.0 && goal.linear.max_acceleration != 0.0 &&
goal.angular.max_velocity != 0.0 &&
goal.angular.max_acceleration != 0.0);