commit cb091717cd465dfca867172266b68108c74b8251
author Austin Schuh <austin.linux@gmail.com> Wed Feb 21 20:01:55 2018 -0800
committer Austin Schuh <austin.linux@gmail.com> Sat Mar 03 00:00:56 2018 -0800
tree c0f957acb0e86cf94e4f2b4089963e767922e371
parent f853bded2f6c029448a01389fd481aeec43a0db1

Arm works

Added gravity and calibrated it.  Terifying...

Change-Id: I70babb1cd3b83ddd7a81f06fb2a75cefd55bcdb8

y2018/control_loops/superstructure/superstructure.cc

diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index 157a46e..25cc500 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -31,6 +31,7 @@
     LOG(ERROR, "WPILib reset, restarting\n");
     intake_left_.Reset();
     intake_right_.Reset();
+    arm_.Reset();
   }
 
   intake_left_.Iterate(unsafe_goal != nullptr
@@ -47,10 +48,26 @@
                         output != nullptr ? &(output->intake.right) : nullptr,
                         &(status->right_intake));
 
-  status->estopped =
-      status->left_intake.estopped || status->right_intake.estopped;
+  intake_right_.Iterate(unsafe_goal != nullptr
+                            ? &(unsafe_goal->intake.right_intake_angle)
+                            : nullptr,
+                        &(position->intake.right),
+                        output != nullptr ? &(output->intake.right) : nullptr,
+                        &(status->left_intake));
 
-  status->zeroed = status->left_intake.zeroed && status->right_intake.zeroed;
+  arm_.Iterate(
+      unsafe_goal != nullptr ? &(unsafe_goal->arm_goal_position) : nullptr,
+      &(position->arm),
+      output != nullptr ? &(output->voltage_proximal) : nullptr,
+      output != nullptr ? &(output->voltage_distal) : nullptr,
+      output != nullptr ? &(output->release_arm_brake) : nullptr,
+      &(status->arm));
+
+  status->estopped = status->left_intake.estopped ||
+                     status->right_intake.estopped || status->arm.estopped;
+
+  status->zeroed = status->left_intake.zeroed && status->right_intake.zeroed &&
+                   status->arm.zeroed;
 
   if (output && unsafe_goal) {
     output->intake.left.voltage_rollers = ::std::max(