commit 3250183ba1e2133b4911bdc9e7f8e1cb60bcf61f
author Austin Schuh <austin.linux@gmail.com> Sat Feb 25 18:32:56 2017 -0800
committer Austin Schuh <austin.linux@gmail.com> Sat Mar 04 17:55:14 2017 -0800
tree 8a86b454bfbe317f6f004c22ea6eee4f2407ee5a
parent c5fceb86fa7afa77119e57ba4fc824574f3a2328

Split StatespaceLoop into a Plant, Controller, and Observer.

This doesn't yet move any of the logic out of the Loop.

Change-Id: I2cb0ea6d1a75c7011576ba752c50e512eeff5890

frc971/control_loops/drivetrain/polydrivetrain.cc

diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index 5fc4b17..91c2fc9 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -163,7 +163,7 @@
   ::Eigen::Matrix<double, 1, 2> FF_sum = FF.colwise().sum();
   int min_FF_sum_index;
   const double min_FF_sum = FF_sum.minCoeff(&min_FF_sum_index);
-  const double min_K_sum = loop_->K().col(min_FF_sum_index).sum();
+  const double min_K_sum = loop_->controller().K().col(min_FF_sum_index).sum();
   const double high_min_FF_sum = FF_high.col(0).sum();
 
   const double adjusted_ff_voltage =
@@ -244,11 +244,13 @@
 
       // Construct a constraint on R by manipulating the constraint on U
       ::aos::controls::HVPolytope<2, 4, 4> R_poly_hv(
-          U_Poly_.static_H() * (loop_->K() + FF),
-          U_Poly_.static_k() + U_Poly_.static_H() * loop_->K() * loop_->X_hat(),
-          (loop_->K() + FF).inverse() *
-              ::aos::controls::ShiftPoints<2, 4>(U_Poly_.StaticVertices(),
-                                                 loop_->K() * loop_->X_hat()));
+          U_Poly_.static_H() * (loop_->controller().K() + FF),
+          U_Poly_.static_k() +
+              U_Poly_.static_H() * loop_->controller().K() * loop_->X_hat(),
+          (loop_->controller().K() + FF).inverse() *
+              ::aos::controls::ShiftPoints<2, 4>(
+                  U_Poly_.StaticVertices(),
+                  loop_->controller().K() * loop_->X_hat()));
 
       // Limit R back inside the box.
       loop_->mutable_R() =
@@ -257,7 +259,7 @@
 
     const Eigen::Matrix<double, 2, 1> FF_volts = FF * loop_->R();
     const Eigen::Matrix<double, 2, 1> U_ideal =
-        loop_->K() * (loop_->R() - loop_->X_hat()) + FF_volts;
+        loop_->controller().K() * (loop_->R() - loop_->X_hat()) + FF_volts;
 
     for (int i = 0; i < 2; i++) {
       loop_->mutable_U()[i] = ::aos::Clip(U_ideal[i], -12, 12);