commit 473a5652493bf7bda719e2f35e8a0caeb8505337
author Austin Schuh <austin.linux@gmail.com> Sun Feb 05 01:30:42 2017 -0800
committer Brian Silverman <bsilver16384@gmail.com> Sun Feb 05 15:48:00 2017 -0800
tree e3d7d9fb6ec069255eda9c1bb1858cf5e6e14bca
parent 48d60c18aa8fb192e3902634d348ffdc885cfea7

Factored out 1 DOF profiled subsystem from 2016

This should make all the motion profiled zeroing easy.

Change-Id: I1941ee8e5b4e14e611bb2f0df86c60e9c055d7af

y2016/control_loops/superstructure/superstructure.cc

diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 0c1ac6a..cb09b42 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -648,11 +648,11 @@
     }
   }
   arm_.set_max_voltage(
-      kill_shoulder_ ? 0.0 : max_voltage,
-      kill_shoulder_ ? (arm_.X_hat(0, 0) < 0.05 ? kShooterHangingLowVoltage
-                                                : kShooterHangingVoltage)
-                     : max_voltage);
-  intake_.set_max_voltage(max_voltage);
+      {{kill_shoulder_ ? 0.0 : max_voltage,
+        kill_shoulder_ ? (arm_.X_hat(0, 0) < 0.05 ? kShooterHangingLowVoltage
+                                                  : kShooterHangingVoltage)
+                       : max_voltage}});
+  intake_.set_max_voltage({{max_voltage}});
 
   if (IsRunning() && !kill_shoulder_) {
     // We don't want lots of negative voltage when we are near the bellypan on
@@ -743,7 +743,7 @@
   status->shoulder.voltage_error = arm_.X_hat(4, 0);
   status->shoulder.calculated_velocity =
       (arm_.shoulder_angle() - last_shoulder_angle_) / 0.005;
-  status->shoulder.estimator_state = arm_.ShoulderEstimatorState();
+  status->shoulder.estimator_state = arm_.EstimatorState(0);
 
   status->wrist.angle = arm_.X_hat(2, 0);
   status->wrist.angular_velocity = arm_.X_hat(3, 0);
@@ -754,7 +754,7 @@
   status->wrist.voltage_error = arm_.X_hat(5, 0);
   status->wrist.calculated_velocity =
       (arm_.wrist_angle() - last_wrist_angle_) / 0.005;
-  status->wrist.estimator_state = arm_.WristEstimatorState();
+  status->wrist.estimator_state = arm_.EstimatorState(1);
 
   status->intake.angle = intake_.X_hat(0, 0);
   status->intake.angular_velocity = intake_.X_hat(1, 0);
@@ -766,7 +766,7 @@
   status->intake.calculated_velocity =
       (intake_.angle() - last_intake_angle_) / 0.005;
   status->intake.voltage_error = intake_.X_hat(2, 0);
-  status->intake.estimator_state = intake_.IntakeEstimatorState();
+  status->intake.estimator_state = intake_.EstimatorState(0);
   status->intake.feedforwards_power = intake_.controller().ff_U(0, 0);
 
   status->shoulder_controller_index = arm_.controller_index();