diff --git a/WORKSPACE b/WORKSPACE
index 01187a5..4db4ba8 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -21,8 +21,8 @@
 new_http_archive(
   name = 'arm_frc_linux_gnueabi_repo',
   build_file = 'tools/cpp/arm-frc-linux-gnueabi/arm-frc-linux-gnueabi.BUILD',
-  sha256 = '9e93b0712e90d11895444f720f0c90c649fb9becb4ca28bb50749d9074eb1306',
-  url = 'http://frc971.org/Build-Dependencies/roborio-compiler-2016.tar.gz',
+  sha256 = '9e2e58f0a668c22e46486a76df8b9da08f526cd8bf4e579f19b461f70a358bf8',
+  url = 'http://frc971.org/Build-Dependencies/roborio-compiler-2017.tar.xz',
 )
 
 # Recompressed version of the one downloaded from Linaro at
diff --git a/aos/common/messages/robot_state.q b/aos/common/messages/robot_state.q
index 3ecb653..1e9b936 100644
--- a/aos/common/messages/robot_state.q
+++ b/aos/common/messages/robot_state.q
@@ -4,8 +4,8 @@
   // A bitmask of the button state.
   uint16_t buttons;
 
-  // The 4 joystick axes.
-  double[4] axis;
+  // The 6 joystick axes.
+  double[6] axis;
 
   // The POV axis.
   int32_t pov;
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 8ceeace..8f8b381 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -148,6 +148,16 @@
   ],
 )
 
+queue_library(
+  name = 'profiled_subsystem_queue',
+  srcs = [
+    'profiled_subsystem.q',
+  ],
+  deps = [
+    ':queues',
+  ],
+)
+
 cc_library(
   name = 'profiled_subsystem',
   srcs = [
@@ -157,6 +167,7 @@
     'profiled_subsystem.h',
   ],
   deps = [
+    ':profiled_subsystem_queue',
     ':simple_capped_state_feedback_loop',
     ':state_feedback_loop',
     '//aos/common/controls:control_loop',
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 9444b66..7dbd1a8 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -2,6 +2,7 @@
 #define FRC971_CONTROL_LOOPS_PROFILED_SUBSYSTEM_H_
 
 #include <array>
+#include <chrono>
 #include <memory>
 #include <utility>
 
@@ -9,9 +10,12 @@
 
 #include "aos/common/controls/control_loop.h"
 #include "aos/common/util/trapezoid_profile.h"
+#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/profiled_subsystem.q.h"
 #include "frc971/control_loops/simple_capped_state_feedback_loop.h"
 #include "frc971/control_loops/state_feedback_loop.h"
 #include "frc971/zeroing/zeroing.h"
+#include "frc971/constants.h"
 
 namespace frc971 {
 namespace control_loops {
@@ -142,28 +146,34 @@
   // Runs the controller and profile generator for a cycle.
   void Update(bool disabled);
 
+  // Fills out the ProfiledJointStatus structure with the current state.
+  void PopulateStatus(ProfiledJointStatus *status);
+
   // Forces the current goal to the provided goal, bypassing the profiler.
   void ForceGoal(double goal);
   // Sets the unprofiled goal.  The profiler will generate a profile to go to
   // this goal.
   void set_unprofiled_goal(double unprofiled_goal);
   // Limits our profiles to a max velocity and acceleration for proper motion.
+  void AdjustProfile(const ::frc971::ProfileParameters &profile_parameters);
   void AdjustProfile(double max_angular_velocity,
                      double max_angular_acceleration);
 
   // Returns true if we have exceeded any hard limits.
   bool CheckHardLimits();
 
-  // Returns the requested intake voltage.
-  double intake_voltage() const { return loop_->U(0, 0); }
+  // Returns the requested voltage.
+  double voltage() const { return loop_->U(0, 0); }
 
   // Returns the current position.
-  double angle() const { return Y_(0, 0); }
+  double position() const { return Y_(0, 0); }
 
   // For testing:
   // Triggers an estimator error.
   void TriggerEstimatorError() { estimators_[0].TriggerError(); }
 
+  const ::frc971::constants::Range &range() const { return range_; }
+
  private:
   // Limits the provided goal to the soft limits.  Prints "name" when it fails
   // to aid debugging.
@@ -182,6 +192,8 @@
 
   const double default_velocity_;
   const double default_acceleration_;
+
+  double last_position_ = 0;
 };
 
 namespace internal {
@@ -213,6 +225,7 @@
 
   loop_->mutable_X_hat()(0, 0) += doffset;
   Y_(0, 0) += doffset;
+  last_position_ += doffset;
   loop_->mutable_R(0, 0) += doffset;
 
   profile_.MoveGoal(doffset);
@@ -222,12 +235,40 @@
 }
 
 template <class ZeroingEstimator>
+void SingleDOFProfiledSubsystem<ZeroingEstimator>::PopulateStatus(
+    ProfiledJointStatus *status) {
+  status->zeroed = zeroed();
+  status->state = -1;
+  // We don't know, so default to the bad case.
+  status->estopped = true;
+
+  status->position = X_hat(0, 0);
+  status->velocity = X_hat(1, 0);
+  status->goal_position = goal(0, 0);
+  status->goal_velocity = goal(1, 0);
+  status->unprofiled_goal_position = unprofiled_goal(0, 0);
+  status->unprofiled_goal_velocity = unprofiled_goal(1, 0);
+  status->voltage_error = X_hat(2, 0);
+  status->calculated_velocity =
+      (position() - last_position_) /
+      ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+          ::aos::controls::kLoopFrequency)
+          .count();
+
+  status->estimator_state = EstimatorState(0);
+
+  Eigen::Matrix<double, 3, 1> error = controller().error();
+  status->position_power = controller().K(0, 0) * error(0, 0);
+  status->velocity_power = controller().K(0, 1) * error(1, 0);
+}
+
+template <class ZeroingEstimator>
 void SingleDOFProfiledSubsystem<ZeroingEstimator>::Correct(
-    typename ZeroingEstimator::Position position) {
-  estimators_[0].UpdateEstimate(position);
+    typename ZeroingEstimator::Position new_position) {
+  estimators_[0].UpdateEstimate(new_position);
 
   if (estimators_[0].error()) {
-    LOG(ERROR, "zeroing error with intake_estimator\n");
+    LOG(ERROR, "zeroing error\n");
     return;
   }
 
@@ -243,7 +284,8 @@
     set_zeroed(0, true);
   }
 
-  Y_ << position.encoder;
+  last_position_ = position();
+  Y_ << new_position.encoder;
   Y_ += offset_;
   loop_->Correct(Y_);
 }
@@ -251,14 +293,14 @@
 template <class ZeroingEstimator>
 void SingleDOFProfiledSubsystem<ZeroingEstimator>::CapGoal(
     const char *name, Eigen::Matrix<double, 3, 1> *goal) {
-  // Limit the goal to min/max allowable angles.
+  // Limit the goal to min/max allowable positions.
   if ((*goal)(0, 0) > range_.upper) {
-    LOG(WARNING, "Intake goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
+    LOG(WARNING, "Goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
         range_.upper);
     (*goal)(0, 0) = range_.upper;
   }
   if ((*goal)(0, 0) < range_.lower) {
-    LOG(WARNING, "Intake goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
+    LOG(WARNING, "Goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
         range_.lower);
     (*goal)(0, 0) = range_.lower;
   }
@@ -305,10 +347,10 @@
 bool SingleDOFProfiledSubsystem<ZeroingEstimator>::CheckHardLimits() {
   // Returns whether hard limits have been exceeded.
 
-  if (angle() > range_.upper_hard || angle() < range_.lower_hard) {
+  if (position() > range_.upper_hard || position() < range_.lower_hard) {
     LOG(ERROR,
         "SingleDOFProfiledSubsystem at %f out of bounds [%f, %f], ESTOPing\n",
-        angle(), range_.lower_hard, range_.upper_hard);
+        position(), range_.lower_hard, range_.upper_hard);
     return true;
   }
 
@@ -317,6 +359,13 @@
 
 template <class ZeroingEstimator>
 void SingleDOFProfiledSubsystem<ZeroingEstimator>::AdjustProfile(
+    const ::frc971::ProfileParameters &profile_parameters) {
+  AdjustProfile(profile_parameters.max_velocity,
+                profile_parameters.max_acceleration);
+}
+
+template <class ZeroingEstimator>
+void SingleDOFProfiledSubsystem<ZeroingEstimator>::AdjustProfile(
     double max_angular_velocity, double max_angular_acceleration) {
   profile_.set_maximum_velocity(
       internal::UseUnlessZero(max_angular_velocity, default_velocity_));
diff --git a/frc971/control_loops/profiled_subsystem.q b/frc971/control_loops/profiled_subsystem.q
new file mode 100644
index 0000000..11d641d
--- /dev/null
+++ b/frc971/control_loops/profiled_subsystem.q
@@ -0,0 +1,41 @@
+package frc971.control_loops;
+
+import "frc971/control_loops/control_loops.q";
+
+struct ProfiledJointStatus {
+  // Is the turret zeroed?
+  bool zeroed;
+
+  // The state of the subsystem, if applicable.  -1 otherwise.
+  int32_t state;
+
+  // If true, we have aborted.
+  bool estopped;
+
+  // Position of the joint.
+  float position;
+  // Velocity of the joint in units/second.
+  float velocity;
+  // Profiled goal position of the joint.
+  float goal_position;
+  // Profiled goal velocity of the joint in units/second.
+  float goal_velocity;
+  // Unprofiled goal position from absoulte zero  of the joint.
+  float unprofiled_goal_position;
+  // Unprofiled goal velocity of the joint in units/second.
+  float unprofiled_goal_velocity;
+
+  // The estimated voltage error.
+  float voltage_error;
+
+  // The calculated velocity with delta x/delta t
+  float calculated_velocity;
+
+  // Components of the control loop output
+  float position_power;
+  float velocity_power;
+  float feedforwards_power;
+
+  // State of the estimator.
+  .frc971.EstimatorState estimator_state;
+};
diff --git a/frc971/wpilib/joystick_sender.cc b/frc971/wpilib/joystick_sender.cc
index b4866e9..a490e67 100644
--- a/frc971/wpilib/joystick_sender.cc
+++ b/frc971/wpilib/joystick_sender.cc
@@ -47,7 +47,7 @@
 
     for (int i = 0; i < 4; ++i) {
       new_state->joysticks[i].buttons = ds->GetStickButtons(i);
-      for (int j = 0; j < 4; ++j) {
+      for (int j = 0; j < 6; ++j) {
         new_state->joysticks[i].axis[j] = ds->GetStickAxis(i, j);
       }
       new_state->joysticks[i].pov = ds->GetStickPOV(i, 0);
diff --git a/y2012/control_loops/python/polydrivetrain.py b/y2012/control_loops/python/polydrivetrain.py
index c19c888..944c09b 100755
--- a/y2012/control_loops/python/polydrivetrain.py
+++ b/y2012/control_loops/python/polydrivetrain.py
@@ -134,7 +134,7 @@
 
     self.G_high = self._drivetrain.G_high
     self.G_low = self._drivetrain.G_low
-    self.R = self._drivetrain.R
+    self.resistance = self._drivetrain.R
     self.r = self._drivetrain.r
     self.Kv = self._drivetrain.Kv
     self.Kt = self._drivetrain.Kt
@@ -184,6 +184,10 @@
         [[0.0],
          [0.0]])
 
+    self.U_ideal = numpy.matrix(
+        [[0.0],
+         [0.0]])
+
     # ttrust is the comprimise between having full throttle negative inertia,
     # and having no throttle negative inertia.  A value of 0 is full throttle
     # inertia.  A value of 1 is no throttle negative inertia.
@@ -239,9 +243,9 @@
                  self.CurrentDrivetrain().r)
     #print gear_name, "Motor Energy Difference.", 0.5 * 0.000140032647 * (low_omega * low_omega - high_omega * high_omega), "joules"
     high_torque = ((12.0 - high_omega / self.CurrentDrivetrain().Kv) *
-                   self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().R)
+                   self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
     low_torque = ((12.0 - low_omega / self.CurrentDrivetrain().Kv) *
-                  self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().R)
+                  self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
     high_power = high_torque * high_omega
     low_power = low_torque * low_omega
     #if should_print:
@@ -407,27 +411,26 @@
 
 
 def main(argv):
-  argv = FLAGS(argv)
-
   vdrivetrain = VelocityDrivetrain()
 
-  if len(argv) != 5:
-    glog.fatal('Expected .h file name and .cc file name')
-  else:
-    namespaces = ['y2012', 'control_loops', 'drivetrain']
-    dog_loop_writer = control_loop.ControlLoopWriter(
-        "VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
-                       vdrivetrain.drivetrain_low_high,
-                       vdrivetrain.drivetrain_high_low,
-                       vdrivetrain.drivetrain_high_high],
-                       namespaces=namespaces)
+  if not FLAGS.plot:
+    if len(argv) != 5:
+      glog.fatal('Expected .h file name and .cc file name')
+    else:
+      namespaces = ['y2012', 'control_loops', 'drivetrain']
+      dog_loop_writer = control_loop.ControlLoopWriter(
+          "VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
+                         vdrivetrain.drivetrain_low_high,
+                         vdrivetrain.drivetrain_high_low,
+                         vdrivetrain.drivetrain_high_high],
+                         namespaces=namespaces)
 
-    dog_loop_writer.Write(argv[1], argv[2])
+      dog_loop_writer.Write(argv[1], argv[2])
 
-    cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()])
+      cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()])
 
-    cim_writer.Write(argv[3], argv[4])
-    return
+      cim_writer.Write(argv[3], argv[4])
+      return
 
   vl_plot = []
   vr_plot = []
@@ -475,22 +478,6 @@
     else:
       radius_plot.append(turn_velocity / fwd_velocity)
 
-  cim_velocity_plot = []
-  cim_voltage_plot = []
-  cim_time = []
-  cim = CIM()
-  R = numpy.matrix([[300]])
-  for t in numpy.arange(0, 0.5, cim.dt):
-    U = numpy.clip(cim.K * (R - cim.X) + R / cim.Kv, cim.U_min, cim.U_max)
-    cim.Update(U)
-    cim_velocity_plot.append(cim.X[0, 0])
-    cim_voltage_plot.append(U[0, 0] * 10)
-    cim_time.append(t)
-  pylab.plot(cim_time, cim_velocity_plot, label='cim spinup')
-  pylab.plot(cim_time, cim_voltage_plot, label='cim voltage')
-  pylab.legend()
-  pylab.show()
-
   # TODO(austin):
   # Shifting compensation.
 
@@ -509,4 +496,6 @@
   return 0
 
 if __name__ == '__main__':
-  sys.exit(main(sys.argv))
+  argv = FLAGS(sys.argv)
+  glog.init()
+  sys.exit(main(argv))
diff --git a/y2014/control_loops/python/polydrivetrain.py b/y2014/control_loops/python/polydrivetrain.py
index 387fd78..83fff45 100755
--- a/y2014/control_loops/python/polydrivetrain.py
+++ b/y2014/control_loops/python/polydrivetrain.py
@@ -134,7 +134,7 @@
 
     self.G_high = self._drivetrain.G_high
     self.G_low = self._drivetrain.G_low
-    self.R = self._drivetrain.R
+    self.resistance = self._drivetrain.R
     self.r = self._drivetrain.r
     self.Kv = self._drivetrain.Kv
     self.Kt = self._drivetrain.Kt
@@ -184,6 +184,10 @@
         [[0.0],
          [0.0]])
 
+    self.U_ideal = numpy.matrix(
+        [[0.0],
+         [0.0]])
+
     # ttrust is the comprimise between having full throttle negative inertia,
     # and having no throttle negative inertia.  A value of 0 is full throttle
     # inertia.  A value of 1 is no throttle negative inertia.
@@ -239,9 +243,9 @@
                  self.CurrentDrivetrain().r)
     #print gear_name, "Motor Energy Difference.", 0.5 * 0.000140032647 * (low_omega * low_omega - high_omega * high_omega), "joules"
     high_torque = ((12.0 - high_omega / self.CurrentDrivetrain().Kv) *
-                   self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().R)
+                   self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
     low_torque = ((12.0 - low_omega / self.CurrentDrivetrain().Kv) *
-                  self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().R)
+                  self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
     high_power = high_torque * high_omega
     low_power = low_torque * low_omega
     #if should_print:
@@ -407,27 +411,26 @@
 
 
 def main(argv):
-  argv = FLAGS(argv)
-
   vdrivetrain = VelocityDrivetrain()
 
-  if len(argv) != 5:
-    glog.fatal('Expected .h file name and .cc file name')
-  else:
-    namespaces = ['y2014', 'control_loops', 'drivetrain']
-    dog_loop_writer = control_loop.ControlLoopWriter(
-        "VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
-                       vdrivetrain.drivetrain_low_high,
-                       vdrivetrain.drivetrain_high_low,
-                       vdrivetrain.drivetrain_high_high],
-                       namespaces=namespaces)
+  if not FLAGS.plot:
+    if len(argv) != 5:
+      glog.fatal('Expected .h file name and .cc file name')
+    else:
+      namespaces = ['y2014', 'control_loops', 'drivetrain']
+      dog_loop_writer = control_loop.ControlLoopWriter(
+          "VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
+                         vdrivetrain.drivetrain_low_high,
+                         vdrivetrain.drivetrain_high_low,
+                         vdrivetrain.drivetrain_high_high],
+                         namespaces=namespaces)
 
-    dog_loop_writer.Write(argv[1], argv[2])
+      dog_loop_writer.Write(argv[1], argv[2])
 
-    cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()])
+      cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()])
 
-    cim_writer.Write(argv[3], argv[4])
-    return
+      cim_writer.Write(argv[3], argv[4])
+      return
 
   vl_plot = []
   vr_plot = []
@@ -474,7 +477,6 @@
       radius_plot.append(turn_velocity)
     else:
       radius_plot.append(turn_velocity / fwd_velocity)
-
   cim_velocity_plot = []
   cim_voltage_plot = []
   cim_time = []
@@ -509,4 +511,6 @@
   return 0
 
 if __name__ == '__main__':
-  sys.exit(main(sys.argv))
+  argv = FLAGS(sys.argv)
+  glog.init()
+  sys.exit(main(argv))
diff --git a/y2014_bot3/control_loops/python/polydrivetrain.py b/y2014_bot3/control_loops/python/polydrivetrain.py
index 1d426d9..9bffa2f 100755
--- a/y2014_bot3/control_loops/python/polydrivetrain.py
+++ b/y2014_bot3/control_loops/python/polydrivetrain.py
@@ -411,8 +411,6 @@
 
 
 def main(argv):
-  argv = FLAGS(argv)
-
   vdrivetrain = VelocityDrivetrain()
 
   if not FLAGS.plot:
@@ -498,4 +496,6 @@
   return 0
 
 if __name__ == '__main__':
-  sys.exit(main(sys.argv))
+  argv = FLAGS(sys.argv)
+  glog.init()
+  sys.exit(main(argv))
diff --git a/y2015/control_loops/python/polydrivetrain.py b/y2015/control_loops/python/polydrivetrain.py
index 9a5b48b..29a55a6 100755
--- a/y2015/control_loops/python/polydrivetrain.py
+++ b/y2015/control_loops/python/polydrivetrain.py
@@ -412,8 +412,6 @@
 
 
 def main(argv):
-  argv = FLAGS(argv)
-
   vdrivetrain = VelocityDrivetrain()
 
   if not FLAGS.plot:
@@ -515,4 +513,6 @@
   return 0
 
 if __name__ == '__main__':
-  sys.exit(main(sys.argv))
+  argv = FLAGS(sys.argv)
+  glog.init()
+  sys.exit(main(argv))
diff --git a/y2015_bot3/control_loops/python/polydrivetrain.py b/y2015_bot3/control_loops/python/polydrivetrain.py
index e85d131..1d2a74e 100755
--- a/y2015_bot3/control_loops/python/polydrivetrain.py
+++ b/y2015_bot3/control_loops/python/polydrivetrain.py
@@ -412,8 +412,6 @@
 
 
 def main(argv):
-  argv = FLAGS(argv)
-
   vdrivetrain = VelocityDrivetrain()
 
   if not FLAGS.plot:
@@ -482,22 +480,6 @@
     else:
       radius_plot.append(turn_velocity / fwd_velocity)
 
-  cim_velocity_plot = []
-  cim_voltage_plot = []
-  cim_time = []
-  cim = CIM()
-  R = numpy.matrix([[300]])
-  for t in numpy.arange(0, 0.5, cim.dt):
-    U = numpy.clip(cim.K * (R - cim.X) + R / cim.Kv, cim.U_min, cim.U_max)
-    cim.Update(U)
-    cim_velocity_plot.append(cim.X[0, 0])
-    cim_voltage_plot.append(U[0, 0] * 10)
-    cim_time.append(t)
-  pylab.plot(cim_time, cim_velocity_plot, label='cim spinup')
-  pylab.plot(cim_time, cim_voltage_plot, label='cim voltage')
-  pylab.legend()
-  pylab.show()
-
   # TODO(austin):
   # Shifting compensation.
 
@@ -516,4 +498,6 @@
   return 0
 
 if __name__ == '__main__':
-  sys.exit(main(sys.argv))
+  argv = FLAGS(sys.argv)
+  glog.init()
+  sys.exit(main(argv))
diff --git a/y2016/control_loops/python/polydrivetrain.py b/y2016/control_loops/python/polydrivetrain.py
index 62a4895..c131326 100755
--- a/y2016/control_loops/python/polydrivetrain.py
+++ b/y2016/control_loops/python/polydrivetrain.py
@@ -411,8 +411,6 @@
 
 
 def main(argv):
-  argv = FLAGS(argv)
-
   vdrivetrain = VelocityDrivetrain()
 
   if not FLAGS.plot:
@@ -498,4 +496,6 @@
   return 0
 
 if __name__ == '__main__':
-  sys.exit(main(sys.argv))
+  argv = FLAGS(sys.argv)
+  glog.init()
+  sys.exit(main(argv))
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index cb09b42..3bf0966 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -41,7 +41,7 @@
 
   double shoulder_angle = arm_->shoulder_angle();
   double wrist_angle = arm_->wrist_angle();
-  double intake_angle = intake_->angle();
+  double intake_angle = intake_->position();
 
   // TODO(phil): This may need tuning to account for bounciness in the limbs or
   // some other thing that I haven't thought of. At the very least,
@@ -163,7 +163,7 @@
 
 bool CollisionAvoidance::collided() const {
   return collided_with_given_angles(arm_->shoulder_angle(), arm_->wrist_angle(),
-                                    intake_->angle());
+                                    intake_->position());
 }
 
 bool CollisionAvoidance::collided_with_given_angles(double shoulder_angle,
@@ -345,7 +345,7 @@
 
       // Set the goals to where we are now so when we start back up, we don't
       // jump.
-      intake_.ForceGoal(intake_.angle());
+      intake_.ForceGoal(intake_.position());
       arm_.ForceGoal(arm_.shoulder_angle(), arm_.wrist_angle());
       // Set up the profile to be the zeroing profile.
       intake_.AdjustProfile(0.5, 10);
@@ -390,7 +390,7 @@
         // enough.
         if (last_state_ != HIGH_ARM_ZERO_MOVE_INTAKE_OUT) {
           intake_.set_unprofiled_goal(
-              MoveButKeepBelow(kIntakeUpperClear, intake_.angle(),
+              MoveButKeepBelow(kIntakeUpperClear, intake_.position(),
                                kIntakeEncoderIndexDifference * 2.5));
         }
 
@@ -428,7 +428,7 @@
         // far enough to zero.
         if (last_state_ != LOW_ARM_ZERO_LOWER_INTAKE) {
           intake_.set_unprofiled_goal(
-              MoveButKeepBelow(kIntakeLowerClear, intake_.angle(),
+              MoveButKeepBelow(kIntakeLowerClear, intake_.position(),
                                kIntakeEncoderIndexDifference * 2.5));
         }
         if (IsIntakeNear(kLooseTolerance)) {
@@ -539,7 +539,7 @@
         }
 
         // Reset the profile to the current position so it moves well from here.
-        intake_.ForceGoal(intake_.angle());
+        intake_.ForceGoal(intake_.position());
         arm_.ForceGoal(arm_.shoulder_angle(), arm_.wrist_angle());
       } else {
         // If we are in slow_running and are no longer collided, let 'er rip.
@@ -687,7 +687,7 @@
 
   // Write out all the voltages.
   if (output) {
-    output->voltage_intake = intake_.intake_voltage();
+    output->voltage_intake = intake_.voltage();
     output->voltage_shoulder = arm_.shoulder_voltage();
     output->voltage_wrist = arm_.wrist_voltage();
 
@@ -764,7 +764,7 @@
   status->intake.unprofiled_goal_angular_velocity =
       intake_.unprofiled_goal(1, 0);
   status->intake.calculated_velocity =
-      (intake_.angle() - last_intake_angle_) / 0.005;
+      (intake_.position() - last_intake_angle_) / 0.005;
   status->intake.voltage_error = intake_.X_hat(2, 0);
   status->intake.estimator_state = intake_.EstimatorState(0);
   status->intake.feedforwards_power = intake_.controller().ff_U(0, 0);
@@ -773,7 +773,7 @@
 
   last_shoulder_angle_ = arm_.shoulder_angle();
   last_wrist_angle_ = arm_.wrist_angle();
-  last_intake_angle_ = intake_.angle();
+  last_intake_angle_ = intake_.position();
 
   status->estopped = (state_ == ESTOP);
 
diff --git a/y2016_bot3/control_loops/python/polydrivetrain.py b/y2016_bot3/control_loops/python/polydrivetrain.py
index aed8ebe..ed799f4 100755
--- a/y2016_bot3/control_loops/python/polydrivetrain.py
+++ b/y2016_bot3/control_loops/python/polydrivetrain.py
@@ -411,8 +411,6 @@
 
 
 def main(argv):
-  argv = FLAGS(argv)
-
   vdrivetrain = VelocityDrivetrain()
 
   if not FLAGS.plot:
@@ -498,4 +496,6 @@
   return 0
 
 if __name__ == '__main__':
-  sys.exit(main(sys.argv))
+  argv = FLAGS(sys.argv)
+  glog.init()
+  sys.exit(main(argv))
diff --git a/y2016_bot4/control_loops/python/polydrivetrain.py b/y2016_bot4/control_loops/python/polydrivetrain.py
index 6c70946..e0f94d0 100755
--- a/y2016_bot4/control_loops/python/polydrivetrain.py
+++ b/y2016_bot4/control_loops/python/polydrivetrain.py
@@ -411,8 +411,6 @@
 
 
 def main(argv):
-  argv = FLAGS(argv)
-
   vdrivetrain = VelocityDrivetrain()
 
   if not FLAGS.plot:
@@ -498,4 +496,6 @@
   return 0
 
 if __name__ == '__main__':
-  sys.exit(main(sys.argv))
+  argv = FLAGS(sys.argv)
+  glog.init()
+  sys.exit(main(argv))
diff --git a/y2017/control_loops/python/hood.py b/y2017/control_loops/python/hood.py
index 5be85cd..376a22a 100755
--- a/y2017/control_loops/python/hood.py
+++ b/y2017/control_loops/python/hood.py
@@ -42,8 +42,10 @@
     self.G2 = self.G1 * (14.0 / 36.0)
     # Third axle gear ratio off the motor
     self.G3 = self.G2 * (14.0 / 36.0)
+    # The last gear reduction (encoder -> hood angle)
+    self.last_G = (18.0 / 345.0)
     # Gear ratio
-    self.G = (12.0 / 60.0) * (14.0 / 36.0) * (14.0 / 36.0) * (18.0 / 345.0)
+    self.G = (12.0 / 60.0) * (14.0 / 36.0) * (14.0 / 36.0) * self.last_G
 
     # 36 tooth gear inertia in kg * m^2
     self.big_gear_inertia = 0.5 * 0.039 * ((36.0 / 40.0 * 0.025) ** 2)
@@ -319,6 +321,8 @@
     integral_hood = IntegralHood('IntegralHood')
     integral_loop_writer = control_loop.ControlLoopWriter('IntegralHood', [integral_hood],
                                                           namespaces=namespaces)
+    integral_loop_writer.AddConstant(control_loop.Constant('kLastReduction', '%f',
+          integral_hood.last_G))
     integral_loop_writer.Write(argv[3], argv[4])
 
 
diff --git a/y2017/control_loops/python/polydrivetrain.py b/y2017/control_loops/python/polydrivetrain.py
index 29cb209..df1f18f 100755
--- a/y2017/control_loops/python/polydrivetrain.py
+++ b/y2017/control_loops/python/polydrivetrain.py
@@ -411,8 +411,6 @@
 
 
 def main(argv):
-  argv = FLAGS(argv)
-
   vdrivetrain = VelocityDrivetrain()
 
   if not FLAGS.plot:
@@ -498,4 +496,6 @@
   return 0
 
 if __name__ == '__main__':
-  sys.exit(main(sys.argv))
+  argv = FLAGS(sys.argv)
+  glog.init()
+  sys.exit(main(argv))
diff --git a/y2017/control_loops/superstructure/BUILD b/y2017/control_loops/superstructure/BUILD
index 61fb001..d0603ba 100644
--- a/y2017/control_loops/superstructure/BUILD
+++ b/y2017/control_loops/superstructure/BUILD
@@ -9,6 +9,7 @@
   ],
   deps = [
     '//aos/common/controls:control_loop_queues',
+    '//frc971/control_loops:profiled_subsystem_queue',
     '//frc971/control_loops:queues',
   ],
 )
diff --git a/y2017/control_loops/superstructure/superstructure.q b/y2017/control_loops/superstructure/superstructure.q
index 5fe207e..4f854b7 100644
--- a/y2017/control_loops/superstructure/superstructure.q
+++ b/y2017/control_loops/superstructure/superstructure.q
@@ -1,36 +1,10 @@
 package y2017.control_loops;
 
 import "aos/common/controls/control_loops.q";
-import "frc971/control_loops/control_loops.q";
-
-struct JointState {
-  // Position of the joint.
-  float position;
-  // Velocity of the joint in units/second.
-  float velocity;
-  // Profiled goal position of the joint.
-  float goal_position;
-  // Profiled goal velocity of the joint in units/second.
-  float goal_velocity;
-  // Unprofiled goal position from absoulte zero  of the joint.
-  float unprofiled_goal_position;
-  // Unprofiled goal velocity of the joint in units/second.
-  float unprofiled_goal_velocity;
-
-  // The estimated voltage error.
-  float voltage_error;
-
-  // The calculated velocity with delta x/delta t
-  float calculated_velocity;
-
-  // Components of the control loop output
-  float position_power;
-  float velocity_power;
-  float feedforwards_power;
-
-  // State of the estimator.
-  .frc971.EstimatorState estimator_state;
-};
+import "frc971/control_loops/profiled_subsystem.q";
+// TODO(austin): Add this back in when the queue compiler supports diamond
+// inheritance.
+//import "frc971/control_loops/control_loops.q";
 
 struct IntakeGoal {
   // Zero on the intake is when the intake is retracted inside the robot,
@@ -43,7 +17,7 @@
   .frc971.ProfileParameters profile_params;
 
   // Voltage to send to the rollers. Positive is sucking in.
-  float voltage_rollers;
+  double voltage_rollers;
 };
 
 struct SerializerGoal {
@@ -65,7 +39,7 @@
 
 struct HoodGoal {
   // Angle the hood is currently at
-  double angle_hood;
+  double angle;
 
   // Caps on velocity/acceleration for profiling. 0 for the default.
   .frc971.ProfileParameters profile_params;
@@ -76,17 +50,6 @@
   double angular_velocity;
 };
 
-struct IntakeStatus {
-  // Is it zeroed?
-  bool zeroed;
-
-  // Estimated position and velocities.
-  JointState joint_state;
-
-  // If true, we have aborted.
-  bool estopped;
-};
-
 struct SerializerStatus {
   // The current average velocity in radians/second.
   double avg_angular_velocity;
@@ -102,28 +65,6 @@
   bool estopped;
 };
 
-struct TurretStatus {
-  // Is the turret zeroed?
-  bool zeroed;
-
-  // If true, we have aborted.
-  bool estopped;
-
-  // Estimate angles and angular velocities.
-  JointState turret;
-};
-
-struct HoodStatus {
-  // Is the turret zeroed?
-  bool zeroed;
-
-  // If true, we have aborted.
-  bool estopped;
-
-  // Estimate angles and angular velocities.
-  JointState hood;
-};
-
 struct ShooterStatus {
   // The current average velocity in radians/second.
   double avg_angular_velocity;
@@ -158,27 +99,31 @@
     bool estopped;
 
     // Each subsystems status.
-    IntakeStatus intake;
+    .frc971.control_loops.ProfiledJointStatus intake;
+    .frc971.control_loops.ProfiledJointStatus turret;
+    .frc971.control_loops.ProfiledJointStatus hood;
     SerializerStatus serializer;
-    TurretStatus turret;
-    HoodStatus hood;
     ShooterStatus shooter;
   };
 
   message Position {
+    // TODO(austin): The turret and intake really should be absolute.  Switch
+    // them over when that class is ready.
+
     // Position of the intake, zero when the intake is in, positive when it is
     // out.
-    .frc971.PotAndAbsolutePosition intake;
+    .frc971.PotAndIndexPosition intake;
 
     // Serializer angle in radians.
     double theta_serializer;
 
     // The sensor readings for the turret. The units and sign are defined the
     // same as what's in the Goal message.
-    .frc971.PotAndAbsolutePosition turret;
+    .frc971.PotAndIndexPosition turret;
 
-    // Position of the hood in radians
-    double theta_hood;
+    // The sensor readings for the hood. The units and sign are defined the
+    // same as what's in the Goal message.
+    .frc971.PotAndIndexPosition hood;
 
     // Shooter wheel angle in radians.
     double theta_shooter;
@@ -186,17 +131,17 @@
 
   message Output {
     // Voltages for some of the subsystems.
-    float voltage_intake;
-    float voltage_serializer;
-    float voltage_shooter;
+    double voltage_intake;
+    double voltage_serializer;
+    double voltage_shooter;
 
     // Rollers on the intake.
-    float voltage_intake_rollers;
+    double voltage_intake_rollers;
     // Roller on the serializer
-    float voltage_serializer_rollers;
+    double voltage_serializer_rollers;
 
-    float voltage_turret;
-    float voltage_hood;
+    double voltage_turret;
+    double voltage_hood;
   };
 
   queue Goal goal;