commit c5fceb86fa7afa77119e57ba4fc824574f3a2328
author Austin Schuh <austin.linux@gmail.com> Sat Feb 25 16:24:12 2017 -0800
committer Austin Schuh <austin.linux@gmail.com> Sat Mar 04 17:55:14 2017 -0800
tree 2f292592e5e88b5ca74f84ddebfe7cf7ec6ef1e1
parent 71b5f13de3846bf9363ba238e2e7716242481003

Put a plant inside the loop and moved A,B,etc there.

Change-Id: I9cb3a1a16bd0ccda0c9287514577b3c3861bc42f

y2016/control_loops/superstructure/superstructure_controls.h

diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index 6f853d6..95f04f8 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -28,12 +28,12 @@
 
   const Eigen::Matrix<double, 2, 1> ControllerOutput() override {
     const Eigen::Matrix<double, 2, 1> accelerating_ff =
-        controller(0).Kff * (next_R() - controller(0).plant.A * R());
+        controller(0).Kff * (next_R() - plant().coefficients(0).A * R());
     const Eigen::Matrix<double, 2, 1> accelerating_controller =
         controller(0).K * error() + accelerating_ff;
 
     const Eigen::Matrix<double, 2, 1> decelerating_ff =
-        controller(1).Kff * (next_R() - controller(1).plant.A * R());
+        controller(1).Kff * (next_R() - plant().coefficients(1).A * R());
     const Eigen::Matrix<double, 2, 1> decelerating_controller =
         controller(1).K * error() + decelerating_ff;
 
@@ -48,11 +48,11 @@
       use_accelerating_controller = false;
     }
     if (use_accelerating_controller) {
+      set_index(0);
       ff_U_ = accelerating_ff;
-      set_controller_index(0);
       return accelerating_controller;
     } else {
-      set_controller_index(1);
+      set_index(1);
       ff_U_ = decelerating_ff;
       return decelerating_controller;
     }
@@ -67,7 +67,7 @@
       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) * B().block<2, 1>(2, 0))(0, 0) *
+          (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;
@@ -76,7 +76,7 @@
       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) * B().block<2, 1>(2, 0))(0, 0) *
+          (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;