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);

frc971/control_loops/drivetrain/ssdrivetrain.cc

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);