diff --git a/y2016/control_loops/superstructure/BUILD b/y2016/control_loops/superstructure/BUILD
index 183c3c0..b9628f6 100644
--- a/y2016/control_loops/superstructure/BUILD
+++ b/y2016/control_loops/superstructure/BUILD
@@ -29,32 +29,17 @@
 )
 
 genrule(
-  name = 'genrule_shoulder',
+  name = 'genrule_arm',
   visibility = ['//visibility:private'],
-  cmd = '$(location //y2016/control_loops/python:shoulder) $(OUTS)',
+  cmd = '$(location //y2016/control_loops/python:arm) $(OUTS)',
   tools = [
-    '//y2016/control_loops/python:shoulder',
+    '//y2016/control_loops/python:arm',
   ],
   outs = [
-    'shoulder_plant.h',
-    'shoulder_plant.cc',
-    'integral_shoulder_plant.h',
-    'integral_shoulder_plant.cc',
-  ],
-)
-
-genrule(
-  name = 'genrule_wrist',
-  visibility = ['//visibility:private'],
-  cmd = '$(location //y2016/control_loops/python:wrist) $(OUTS)',
-  tools = [
-    '//y2016/control_loops/python:wrist',
-  ],
-  outs = [
-    'wrist_plant.h',
-    'wrist_plant.cc',
-    'integral_wrist_plant.h',
-    'integral_wrist_plant.cc',
+    'arm_plant.h',
+    'arm_plant.cc',
+    'integral_arm_plant.h',
+    'integral_arm_plant.cc',
   ],
 )
 
@@ -62,19 +47,15 @@
   name = 'superstructure_plants',
   srcs = [
     'intake_plant.cc',
-    'shoulder_plant.cc',
-    'wrist_plant.cc',
+    'arm_plant.cc',
     'integral_intake_plant.cc',
-    'integral_shoulder_plant.cc',
-    'integral_wrist_plant.cc',
+    'integral_arm_plant.cc',
   ],
   hdrs = [
     'intake_plant.h',
-    'shoulder_plant.h',
-    'wrist_plant.h',
+    'arm_plant.h',
     'integral_intake_plant.h',
-    'integral_shoulder_plant.h',
-    'integral_wrist_plant.h',
+    'integral_arm_plant.h',
   ],
   deps = [
     '//frc971/control_loops:state_feedback_loop',
@@ -91,8 +72,12 @@
   ],
   deps = [
     ':superstructure_queue',
+    ':superstructure_plants',
     '//aos/common/controls:control_loop',
+    '//aos/common/util:trapezoid_profile',
     '//frc971/control_loops:state_feedback_loop',
+    '//frc971/zeroing',
+    '//y2016:constants',
   ],
 )
 
@@ -107,6 +92,10 @@
     '//aos/testing:googletest',
     '//aos/common:queues',
     '//aos/common/controls:control_loop_test',
+    '//aos/common:math',
+    '//aos/common:time',
+    '//frc971/control_loops:position_sensor_sim',
+    '//frc971/control_loops:team_number_test_environment',
   ],
 )
 
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index db31ffc..d286fa8 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -3,24 +3,534 @@
 #include "aos/common/controls/control_loops.q.h"
 #include "aos/common/logging/logging.h"
 
+#include "y2016/control_loops/superstructure/integral_intake_plant.h"
+#include "y2016/control_loops/superstructure/integral_arm_plant.h"
+
+#include "y2016/constants.h"
+
 namespace y2016 {
 namespace control_loops {
+namespace superstructure {
 
-Superstructure::Superstructure(
-    control_loops::SuperstructureQueue *my_superstructure)
-    : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(
-          my_superstructure) {}
+namespace {
+constexpr double kZeroingVoltage = 4.0;
 
-void Superstructure::RunIteration(
-    const control_loops::SuperstructureQueue::Goal *goal,
-    const control_loops::SuperstructureQueue::Position *position,
-    ::aos::control_loops::Output *output,
-    control_loops::SuperstructureQueue::Status *status) {
-  (void)goal;
-  (void)position;
-  (void)output;
-  (void)status;
+double UseUnlessZero(double target_value, double default_value) {
+  if (target_value != 0.0) {
+    return target_value;
+  } else {
+    return default_value;
+  }
 }
 
+}  // namespace
+
+void SimpleCappedStateFeedbackLoop::CapU() {
+  mutable_U(0, 0) = ::std::min(U(0, 0), max_voltage_);
+  mutable_U(0, 0) = ::std::max(U(0, 0), -max_voltage_);
+}
+
+void DoubleCappedStateFeedbackLoop::CapU() {
+  mutable_U(0, 0) = ::std::min(U(0, 0), shoulder_max_voltage_);
+  mutable_U(0, 0) = ::std::max(U(0, 0), -shoulder_max_voltage_);
+  mutable_U(1, 0) = ::std::min(U(1, 0), wrist_max_voltage_);
+  mutable_U(1, 0) = ::std::max(U(1, 0), -wrist_max_voltage_);
+}
+
+// Intake
+Intake::Intake()
+    : loop_(new SimpleCappedStateFeedbackLoop(StateFeedbackLoop<3, 1, 1>(
+          ::y2016::control_loops::superstructure::MakeIntegralIntakeLoop()))),
+      estimator_(constants::GetValues().intake.zeroing),
+      profile_(::aos::controls::kLoopFrequency) {
+  Y_.setZero();
+  unprofiled_goal_.setZero();
+  offset_.setZero();
+  AdjustProfile(0.0, 0.0);
+}
+
+void Intake::UpdateIntakeOffset(double offset) {
+  const double doffset = offset - offset_(0, 0);
+  LOG(INFO, "Adjusting Intake offset from %f to %f\n", offset_(0, 0), offset);
+
+  loop_->mutable_X_hat()(0, 0) += doffset;
+  Y_(0, 0) += doffset;
+  loop_->mutable_R(0, 0) += doffset;
+
+  profile_.MoveGoal(doffset);
+  offset_(0, 0) = offset;
+
+  CapGoal("R", &loop_->mutable_R());
+}
+
+
+void Intake::Correct(PotAndIndexPosition position) {
+  estimator_.UpdateEstimate(position);
+
+  if (!initialized_) {
+    if (estimator_.offset_ready()) {
+      UpdateIntakeOffset(estimator_.offset());
+      initialized_ = true;
+    }
+  }
+
+  Y_ << position.encoder;
+  Y_ += offset_;
+  loop_->Correct(Y_);
+}
+
+void Intake::CapGoal(const char *name, Eigen::Matrix<double, 3, 1> *goal) {
+  const auto &values = constants::GetValues();
+
+  // Limit the goal to min/max allowable angles.
+  if ((*goal)(0, 0) >= values.intake.limits.upper) {
+    LOG(WARNING, "Intake goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
+        values.intake.limits.upper);
+    (*goal)(0, 0) = values.intake.limits.upper;
+  }
+  if ((*goal)(0, 0) <= values.intake.limits.lower) {
+    LOG(WARNING, "Intake goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
+        values.intake.limits.lower);
+    (*goal)(0, 0) = values.intake.limits.lower;
+  }
+}
+
+void Intake::ForceGoal(double goal) {
+  set_unprofiled_goal(goal);
+  loop_->mutable_R() = unprofiled_goal_;
+}
+
+void Intake::set_unprofiled_goal(double unprofiled_goal) {
+  unprofiled_goal_(0, 0) = unprofiled_goal;
+  unprofiled_goal_(1, 0) = 0.0;
+  unprofiled_goal_(2, 0) = 0.0;
+  CapGoal("unprofiled R", &unprofiled_goal_);
+}
+
+void Intake::Update(bool disable) {
+  if (!disable) {
+    ::Eigen::Matrix<double, 2, 1> goal_state =
+        profile_.Update(unprofiled_goal_(0, 0), unprofiled_goal_(1, 0));
+
+    loop_->mutable_next_R(0, 0) = goal_state(0, 0);
+    loop_->mutable_next_R(1, 0) = goal_state(1, 0);
+    loop_->mutable_next_R(2, 0) = 0.0;
+    CapGoal("next R", &loop_->mutable_next_R());
+  }
+
+  loop_->Update(disable);
+
+  if (!disable && loop_->U(0, 0) != loop_->U_uncapped(0, 0)) {
+    ::Eigen::Matrix<double, 2, 1> new_state;
+    new_state << loop_->R(0, 0), loop_->R(1, 0);
+    profile_.MoveCurrentState(new_state);
+  }
+}
+
+bool Intake::CheckHardLimits() {
+  const auto &values = constants::GetValues();
+  // Returns whether hard limits have been exceeded.
+
+  if (angle() >= values.intake.limits.upper_hard ||
+      angle() <= values.intake.limits.lower_hard) {
+    LOG(ERROR, "Intake at %f out of bounds [%f, %f], ESTOPing\n", angle(),
+        values.intake.limits.lower_hard, values.intake.limits.upper_hard);
+    return true;
+  }
+
+  return false;
+}
+
+void Intake::set_max_voltage(double voltage) { loop_->set_max_voltage(voltage); }
+
+void Intake::AdjustProfile(double max_angular_velocity,
+                           double max_angular_acceleration) {
+  profile_.set_maximum_velocity(UseUnlessZero(max_angular_velocity, 10.0));
+  profile_.set_maximum_acceleration(UseUnlessZero(max_angular_acceleration, 10.0));
+}
+
+void Intake::Reset() {
+  estimator_.Reset();
+  initialized_ = false;
+  zeroed_ = false;
+}
+
+EstimatorState Intake::IntakeEstimatorState() {
+  EstimatorState estimator_state;
+  ::frc971::zeroing::PopulateEstimatorState(estimator_, &estimator_state);
+
+  return estimator_state;
+}
+
+Arm::Arm()
+    : loop_(new DoubleCappedStateFeedbackLoop(
+          ::y2016::control_loops::superstructure::MakeIntegralArmLoop())),
+      shoulder_profile_(::aos::controls::kLoopFrequency),
+      wrist_profile_(::aos::controls::kLoopFrequency),
+      shoulder_estimator_(constants::GetValues().shoulder.zeroing),
+      wrist_estimator_(constants::GetValues().wrist.zeroing) {
+  Y_.setZero();
+  offset_.setZero();
+  unprofiled_goal_.setZero();
+  AdjustProfile(0.0, 0.0, 0.0, 0.0);
+}
+
+void Arm::UpdateWristOffset(double offset) {
+  const double doffset = offset - offset_(1, 0);
+  LOG(INFO, "Adjusting Wrist offset from %f to %f\n", offset_(1, 0), offset);
+
+  loop_->mutable_X_hat()(2, 0) += doffset;
+  Y_(1, 0) += doffset;
+  loop_->mutable_R(2, 0) += doffset;
+
+  wrist_profile_.MoveGoal(doffset);
+  offset_(1, 0) = offset;
+
+  CapGoal("R", &loop_->mutable_R());
+}
+
+void Arm::UpdateShoulderOffset(double offset) {
+  const double doffset = offset - offset_(0, 0);
+  LOG(INFO, "Adjusting Shoulder offset from %f to %f\n", offset_(0, 0),
+      offset);
+
+  loop_->mutable_X_hat()(0, 0) += doffset;
+  loop_->mutable_X_hat()(2, 0) += doffset;
+  Y_(0, 0) += doffset;
+  loop_->mutable_R(0, 0) += doffset;
+  loop_->mutable_R(2, 0) += doffset;
+
+  shoulder_profile_.MoveGoal(doffset);
+  offset_(0, 0) = offset;
+
+  CapGoal("R", &loop_->mutable_R());
+}
+
+// TODO(austin): Handle zeroing errors.
+
+void Arm::Correct(PotAndIndexPosition position_shoulder,
+                  PotAndIndexPosition position_wrist) {
+  shoulder_estimator_.UpdateEstimate(position_shoulder);
+  wrist_estimator_.UpdateEstimate(position_wrist);
+
+  if (!initialized_) {
+    if (shoulder_estimator_.offset_ready() && wrist_estimator_.offset_ready()) {
+      UpdateShoulderOffset(shoulder_estimator_.offset());
+      UpdateWristOffset(wrist_estimator_.offset());
+      initialized_ = true;
+    }
+  }
+
+  if (!shoulder_zeroed_ && shoulder_estimator_.zeroed()) {
+    UpdateShoulderOffset(shoulder_estimator_.offset());
+    shoulder_zeroed_ = true;
+  }
+  if (!wrist_zeroed_ && wrist_estimator_.zeroed()) {
+    UpdateWristOffset(wrist_estimator_.offset());
+    wrist_zeroed_ = true;
+  }
+
+  {
+    Y_ << position_shoulder.encoder, position_wrist.encoder;
+    Y_ += offset_;
+    loop_->Correct(Y_);
+  }
+}
+
+void Arm::CapGoal(const char *name, Eigen::Matrix<double, 6, 1> *goal) {
+  // Limit the goals to min/max allowable angles.
+  const auto &values = constants::GetValues();
+
+  if ((*goal)(0, 0) >= values.shoulder.limits.upper) {
+    LOG(WARNING, "Shoulder goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
+        values.shoulder.limits.upper);
+    (*goal)(0, 0) = values.shoulder.limits.upper;
+  }
+  if ((*goal)(0, 0) <= values.shoulder.limits.lower) {
+    LOG(WARNING, "Shoulder goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
+        values.shoulder.limits.lower);
+    (*goal)(0, 0) = values.shoulder.limits.lower;
+  }
+
+  const double wrist_goal_angle_ungrounded = (*goal)(2, 0) - (*goal)(0, 0);
+
+  if (wrist_goal_angle_ungrounded >= values.wrist.limits.upper) {
+    LOG(WARNING, "Wrist goal %s above limit, %f > %f\n", name,
+        wrist_goal_angle_ungrounded, values.wrist.limits.upper);
+    (*goal)(2, 0) = values.wrist.limits.upper + (*goal)(0, 0);
+  }
+  if (wrist_goal_angle_ungrounded <= values.wrist.limits.lower) {
+    LOG(WARNING, "Wrist goal %s below limit, %f < %f\n", name,
+        wrist_goal_angle_ungrounded, values.wrist.limits.lower);
+    (*goal)(2, 0) = values.wrist.limits.lower + (*goal)(0, 0);
+  }
+}
+
+void Arm::ForceGoal(double goal_shoulder, double goal_wrist) {
+  set_unprofiled_goal(goal_shoulder, goal_wrist);
+  loop_->mutable_R() = unprofiled_goal_;
+}
+
+void Arm::set_unprofiled_goal(double unprofiled_goal_shoulder,
+                              double unprofiled_goal_wrist) {
+  unprofiled_goal_ << unprofiled_goal_shoulder, 0.0, unprofiled_goal_wrist, 0.0,
+      0.0, 0.0;
+  CapGoal("unprofiled R", &unprofiled_goal_);
+}
+
+void Arm::AdjustProfile(double max_angular_velocity_shoulder,
+                        double max_angular_acceleration_shoulder,
+                        double max_angular_velocity_wrist,
+                        double max_angular_acceleration_wrist) {
+  shoulder_profile_.set_maximum_velocity(
+      UseUnlessZero(max_angular_velocity_shoulder, 10.0));
+  shoulder_profile_.set_maximum_acceleration(
+      UseUnlessZero(max_angular_acceleration_shoulder, 10.0));
+  wrist_profile_.set_maximum_velocity(
+      UseUnlessZero(max_angular_velocity_wrist, 10.0));
+  wrist_profile_.set_maximum_acceleration(
+      UseUnlessZero(max_angular_acceleration_wrist, 10.0));
+}
+
+bool Arm::CheckHardLimits() {
+  const auto &values = constants::GetValues();
+  if (shoulder_angle() >= values.shoulder.limits.upper_hard ||
+      shoulder_angle() <= values.shoulder.limits.lower_hard) {
+    LOG(ERROR, "Shoulder at %f out of bounds [%f, %f], ESTOPing\n",
+        shoulder_angle(), values.shoulder.limits.lower_hard,
+        values.shoulder.limits.upper_hard);
+    return true;
+  }
+
+  if (wrist_angle() - shoulder_angle() >= values.wrist.limits.upper_hard ||
+      wrist_angle() - shoulder_angle() <= values.wrist.limits.lower_hard) {
+    LOG(ERROR, "Wrist at %f out of bounds [%f, %f], ESTOPing\n",
+        wrist_angle() - shoulder_angle(), values.wrist.limits.lower_hard,
+        values.wrist.limits.upper_hard);
+    return true;
+  }
+
+  return false;
+}
+
+void Arm::Update(bool disable) {
+  if (!disable) {
+    // Compute next goal.
+    ::Eigen::Matrix<double, 2, 1> goal_state_shoulder =
+        shoulder_profile_.Update(unprofiled_goal_(0, 0),
+                                 unprofiled_goal_(1, 0));
+    loop_->mutable_next_R(0, 0) = goal_state_shoulder(0, 0);
+    loop_->mutable_next_R(1, 0) = goal_state_shoulder(1, 0);
+
+    ::Eigen::Matrix<double, 2, 1> goal_state_wrist =
+        wrist_profile_.Update(unprofiled_goal_(2, 0), unprofiled_goal_(3, 0));
+    loop_->mutable_next_R(2, 0) = goal_state_wrist(0, 0);
+    loop_->mutable_next_R(3, 0) = goal_state_wrist(1, 0);
+
+    loop_->mutable_next_R(4, 0) = unprofiled_goal_(4, 0);
+    loop_->mutable_next_R(5, 0) = unprofiled_goal_(5, 0);
+    CapGoal("next R", &loop_->mutable_next_R());
+  }
+
+  // Move loop
+  loop_->Update(disable);
+
+  // Shoulder saturated
+  if (!disable && loop_->U(0, 0) != loop_->U_uncapped(0, 0)) {
+    ::Eigen::Matrix<double, 2, 1> new_shoulder_state;
+    new_shoulder_state << loop_->R(0, 0), loop_->R(1, 0);
+    shoulder_profile_.MoveCurrentState(new_shoulder_state);
+  }
+
+  // Wrist saturated
+  if (!disable && loop_->U(1, 0) != loop_->U_uncapped(1, 0)) {
+    ::Eigen::Matrix<double, 2, 1> new_wrist_state;
+    new_wrist_state << loop_->R(2, 0), loop_->R(3, 0);
+    wrist_profile_.MoveCurrentState(new_wrist_state);
+  }
+}
+
+void Arm::set_max_voltage(double shoulder_max_voltage,
+                          double wrist_max_voltage) {
+  loop_->set_max_voltage(shoulder_max_voltage, wrist_max_voltage);
+}
+
+void Arm::Reset() {
+  shoulder_estimator_.Reset();
+  wrist_estimator_.Reset();
+  initialized_ = false;
+  shoulder_zeroed_ = false;
+  wrist_zeroed_ = false;
+}
+
+EstimatorState Arm::ShoulderEstimatorState() {
+  EstimatorState estimator_state;
+  ::frc971::zeroing::PopulateEstimatorState(shoulder_estimator_,
+                                            &estimator_state);
+
+  return estimator_state;
+}
+
+EstimatorState Arm::WristEstimatorState() {
+  EstimatorState estimator_state;
+  ::frc971::zeroing::PopulateEstimatorState(wrist_estimator_, &estimator_state);
+
+  return estimator_state;
+}
+
+// ///// Superstructure /////
+Superstructure::Superstructure(
+    control_loops::SuperstructureQueue *superstructure_queue)
+    : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(
+          superstructure_queue) {}
+
+void Superstructure::UpdateZeroingState() {
+  // TODO(austin): Explicit state transitions instead of this.
+  // TODO(adam): Change this once we have zeroing written.
+  if (!arm_.initialized() || !intake_.initialized()) {
+    state_ = INITIALIZING;
+  } else if (!intake_.zeroed()) {
+    state_ = ZEROING_INTAKE;
+  } else if (!arm_.zeroed()) {
+    state_ = ZEROING_ARM;
+  } else {
+    state_ = RUNNING;
+  }
+}
+
+void Superstructure::RunIteration(
+    const control_loops::SuperstructureQueue::Goal *unsafe_goal,
+    const control_loops::SuperstructureQueue::Position *position,
+    control_loops::SuperstructureQueue::Output *output,
+    control_loops::SuperstructureQueue::Status *status) {
+  if (WasReset()) {
+    LOG(ERROR, "WPILib reset, restarting\n");
+    arm_.Reset();
+    intake_.Reset();
+    state_ = UNINITIALIZED;
+  }
+
+  // Bool to track if we should turn the motors on or not.
+  bool disable = output == nullptr;
+
+  arm_.Correct(position->shoulder, position->wrist);
+  intake_.Correct(position->intake);
+
+  // Zeroing will work as follows:
+  // Start with the intake. Move it towards the center. Once zeroed, move it
+  // back to the bottom. Rotate the shoulder towards the center. Once zeroed,
+  // move it up enough to rotate the wrist towards the center.
+
+  // We'll then need code to do sanity checking on values.
+
+  switch (state_) {
+    case UNINITIALIZED:
+      LOG(DEBUG, "Uninitialized\n");
+      state_ = INITIALIZING;
+      disable = true;
+      break;
+
+    case INITIALIZING:
+      LOG(DEBUG, "Waiting for accurate initial position.\n");
+      disable = true;
+      // Update state_ to accurately represent the state of the zeroing
+      // estimators.
+      UpdateZeroingState();
+      if (state_ != INITIALIZING) {
+        // Set the goals to where we are now.
+        intake_.ForceGoal(intake_.angle());
+        arm_.ForceGoal(arm_.shoulder_angle(), arm_.wrist_angle());
+      }
+      break;
+
+    case ZEROING_INTAKE:
+    case ZEROING_ARM:
+      // TODO(adam): Add your magic here.
+      state_ = RUNNING;
+      break;
+
+    case RUNNING:
+      if (unsafe_goal) {
+        arm_.AdjustProfile(unsafe_goal->max_angular_velocity_shoulder,
+                           unsafe_goal->max_angular_acceleration_shoulder,
+                           unsafe_goal->max_angular_velocity_wrist,
+                           unsafe_goal->max_angular_acceleration_wrist);
+        intake_.AdjustProfile(unsafe_goal->max_angular_velocity_wrist,
+                              unsafe_goal->max_angular_acceleration_intake);
+
+        arm_.set_unprofiled_goal(unsafe_goal->angle_shoulder,
+                                 unsafe_goal->angle_wrist);
+        intake_.set_unprofiled_goal(unsafe_goal->angle_intake);
+      }
+
+      // Update state_ to accurately represent the state of the zeroing
+      // estimators.
+
+      if (state_ != RUNNING && state_ != ESTOP) {
+        state_ = UNINITIALIZED;
+      }
+      break;
+
+    case ESTOP:
+      LOG(ERROR, "Estop\n");
+      disable = true;
+      break;
+  }
+
+  // ESTOP if we hit any of the limits.  It is safe(ish) to hit the limits while
+  // zeroing since we use such low power.
+  if (state_ == RUNNING) {
+    // ESTOP if we hit the hard limits.
+    if ((arm_.CheckHardLimits() || intake_.CheckHardLimits()) && output) {
+      state_ = ESTOP;
+    }
+  }
+
+  // Set the voltage limits.
+  const double max_voltage = state_ == RUNNING ? 12.0 : kZeroingVoltage;
+  arm_.set_max_voltage(max_voltage, max_voltage);
+  intake_.set_max_voltage(max_voltage);
+
+  // Calculate the loops for a cycle.
+  arm_.Update(disable);
+  intake_.Update(disable);
+
+  // Write out all the voltages.
+  if (output) {
+    output->voltage_intake = intake_.intake_voltage();
+    output->voltage_shoulder = arm_.shoulder_voltage();
+    output->voltage_wrist = arm_.wrist_voltage();
+  }
+
+  // Save debug/internal state.
+  // TODO(austin): Save the voltage errors.
+  status->zeroed = state_ == RUNNING;
+
+  status->shoulder.angle = arm_.X_hat(0, 0);
+  status->shoulder.angular_velocity = arm_.X_hat(1, 0);
+  status->shoulder.goal_angle = arm_.goal(0, 0);
+  status->shoulder.goal_angular_velocity = arm_.goal(1, 0);
+  status->shoulder.estimator_state = arm_.ShoulderEstimatorState();
+
+  status->wrist.angle = arm_.X_hat(2, 0);
+  status->wrist.angular_velocity = arm_.X_hat(3, 0);
+  status->wrist.goal_angle = arm_.goal(2, 0);
+  status->wrist.goal_angular_velocity = arm_.goal(3, 0);
+  status->wrist.estimator_state = arm_.WristEstimatorState();
+
+  status->intake.angle = intake_.X_hat(0, 0);
+  status->intake.angular_velocity = intake_.X_hat(1, 0);
+  status->intake.goal_angle = intake_.goal(0, 0);
+  status->intake.goal_angular_velocity = intake_.goal(1, 0);
+  status->intake.estimator_state = intake_.IntakeEstimatorState();
+
+  status->estopped = (state_ == ESTOP);
+
+  status->state = state_;
+
+  last_state_ = state_;
+}
+
+}  // namespace superstructure
 }  // namespace control_loops
 }  // namespace y2016
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index f53629c..aa43758 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -5,11 +5,208 @@
 
 #include "aos/common/controls/control_loop.h"
 #include "frc971/control_loops/state_feedback_loop.h"
+#include "aos/common/util/trapezoid_profile.h"
 
+#include "frc971/zeroing/zeroing.h"
 #include "y2016/control_loops/superstructure/superstructure.q.h"
 
 namespace y2016 {
 namespace control_loops {
+namespace superstructure {
+namespace testing {
+class SuperstructureTest_DisabledGoalTest_Test;
+}  // namespace testing
+
+using ::frc971::PotAndIndexPosition;
+using ::frc971::EstimatorState;
+
+class SimpleCappedStateFeedbackLoop : public StateFeedbackLoop<3, 1, 1> {
+ public:
+  SimpleCappedStateFeedbackLoop(StateFeedbackLoop<3, 1, 1> &&loop)
+      : StateFeedbackLoop<3, 1, 1>(::std::move(loop)), max_voltage_(12.0) {}
+
+  void set_max_voltage(double max_voltage) {
+    max_voltage_ = ::std::max(0.0, ::std::min(12.0, max_voltage));
+  }
+
+  void CapU() override;
+
+ private:
+  double max_voltage_;
+};
+
+class DoubleCappedStateFeedbackLoop : public StateFeedbackLoop<6, 2, 2> {
+ public:
+  DoubleCappedStateFeedbackLoop(StateFeedbackLoop<6, 2, 2> &&loop)
+      : StateFeedbackLoop<6, 2, 2>(::std::move(loop)),
+        shoulder_max_voltage_(12.0),
+        wrist_max_voltage_(12.0) {}
+
+  void set_max_voltage(double shoulder_max_voltage, double wrist_max_voltage) {
+    shoulder_max_voltage_ = ::std::max(0.0, ::std::min(12.0, shoulder_max_voltage));
+    wrist_max_voltage_ = ::std::max(0.0, ::std::min(12.0, wrist_max_voltage));
+  }
+
+  void CapU() override;
+
+ private:
+  double shoulder_max_voltage_;
+  double wrist_max_voltage_;
+};
+
+class Intake {
+ public:
+  Intake();
+  // Returns whether the estimators have been initialized and zeroed.
+  bool initialized() const { return initialized_; }
+  bool zeroed() const { return zeroed_; }
+
+  // Updates our estimator with the latest position.
+  void Correct(PotAndIndexPosition position);
+
+  // 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);
+
+  // Runs the controller and profile generator for a cycle.
+  void Update(bool disabled);
+
+  // Limits our profiles to a max velocity and acceleration for proper motion.
+  void AdjustProfile(double max_angular_velocity,
+                     double max_angular_acceleration);
+  // Sets the maximum voltage that will be commanded by the loop.
+  void set_max_voltage(double voltage);
+
+  // Returns true if we have exceeded any hard limits.
+  bool CheckHardLimits();
+  // Resets the internal state.
+  void Reset();
+
+  // Returns the current internal estimator state for logging.
+  EstimatorState IntakeEstimatorState();
+
+  // Returns the requested intake voltage.
+  double intake_voltage() const { return loop_->U(0, 0); }
+
+  // Returns the current position.
+  double angle() const { return Y_(0, 0); }
+
+  // Returns the filtered goal.
+  const Eigen::Matrix<double, 3, 1> &goal() const { return loop_->R(); }
+  double goal(int row, int col) const { return loop_->R(row, col); }
+
+  // Returns the current state estimate.
+  const Eigen::Matrix<double, 3, 1> &X_hat() const { return loop_->X_hat(); }
+  double X_hat(int row, int col) const { return loop_->X_hat(row, col); }
+
+ private:
+  // Limits the provided goal to the soft limits.  Prints "name" when it fails
+  // to aid debugging.
+  void CapGoal(const char *name, Eigen::Matrix<double, 3, 1> *goal);
+
+  void UpdateIntakeOffset(double offset);
+
+  ::std::unique_ptr<SimpleCappedStateFeedbackLoop> loop_;
+
+  ::frc971::zeroing::ZeroingEstimator estimator_;
+  aos::util::TrapezoidProfile profile_;
+
+  // Current measurement.
+  Eigen::Matrix<double, 1, 1> Y_;
+  // Current offset.  Y_ = offset_ + raw_sensor;
+  Eigen::Matrix<double, 1, 1> offset_;
+
+  // The goal that the profile tries to reach.
+  Eigen::Matrix<double, 3, 1> unprofiled_goal_;
+
+  bool initialized_ = false;
+  bool zeroed_ = false;
+};
+
+class Arm {
+ public:
+  Arm();
+  // Returns whether the estimators have been initialized and zeroed.
+  bool initialized() const { return initialized_; }
+  bool zeroed() const { return shoulder_zeroed_ && wrist_zeroed_; }
+  bool shoulder_zeroed() const { return shoulder_zeroed_; }
+  bool wrist_zeroed() const { return wrist_zeroed_; }
+
+  // Updates our estimator with the latest position.
+  void Correct(PotAndIndexPosition position_shoulder,
+               PotAndIndexPosition position_wrist);
+
+  // Forces the goal to be the provided goal.
+  void ForceGoal(double unprofiled_goal_shoulder, double unprofiled_goal_wrist);
+  // Sets the unprofiled goal.  The profiler will generate a profile to go to
+  // this goal.
+  void set_unprofiled_goal(double unprofiled_goal_shoulder,
+                           double unprofiled_goal_wrist);
+
+  // Runs the controller and profile generator for a cycle.
+  void Update(bool disabled);
+
+  // Limits our profiles to a max velocity and acceleration for proper motion.
+  void AdjustProfile(double max_angular_velocity_shoulder,
+                     double max_angular_acceleration_shoulder,
+                     double max_angular_velocity_wrist,
+                     double max_angular_acceleration_wrist);
+  void set_max_voltage(double shoulder_max_voltage, double wrist_max_voltage);
+
+  // Returns true if we have exceeded any hard limits.
+  bool CheckHardLimits();
+  // Resets the internal state.
+  void Reset();
+
+  // Returns the current internal estimator state for logging.
+  EstimatorState ShoulderEstimatorState();
+  EstimatorState WristEstimatorState();
+
+  // Returns the requested shoulder and wrist voltages.
+  double shoulder_voltage() const { return loop_->U(0, 0); }
+  double wrist_voltage() const { return loop_->U(1, 0); }
+
+  // Returns the current positions.
+  double shoulder_angle() const { return Y_(0, 0); }
+  double wrist_angle() const { return Y_(1, 0) + Y_(0, 0); }
+
+  // Returns the filtered goal.
+  const Eigen::Matrix<double, 6, 1> &goal() const { return loop_->R(); }
+  double goal(int row, int col) const { return loop_->R(row, col); }
+
+  // Returns the current state estimate.
+  const Eigen::Matrix<double, 6, 1> &X_hat() const { return loop_->X_hat(); }
+  double X_hat(int row, int col) const { return loop_->X_hat(row, col); }
+
+ private:
+  // Limits the provided goal to the soft limits.  Prints "name" when it fails
+  // to aid debugging.
+  void CapGoal(const char *name, Eigen::Matrix<double, 6, 1> *goal);
+
+  // Updates the offset
+  void UpdateWristOffset(double offset);
+  void UpdateShoulderOffset(double offset);
+
+  friend class testing::SuperstructureTest_DisabledGoalTest_Test;
+  ::std::unique_ptr<DoubleCappedStateFeedbackLoop> loop_;
+
+  aos::util::TrapezoidProfile shoulder_profile_, wrist_profile_;
+  ::frc971::zeroing::ZeroingEstimator shoulder_estimator_, wrist_estimator_;
+
+  // Current measurement.
+  Eigen::Matrix<double, 2, 1> Y_;
+  // Current offset.  Y_ = offset_ + raw_sensor;
+  Eigen::Matrix<double, 2, 1> offset_;
+
+  // The goal that the profile tries to reach.
+  Eigen::Matrix<double, 6, 1> unprofiled_goal_;
+
+  bool initialized_ = false;
+  bool shoulder_zeroed_ = false;
+  bool wrist_zeroed_ = false;
+};
 
 class Superstructure
     : public ::aos::controls::ControlLoop<control_loops::SuperstructureQueue> {
@@ -17,18 +214,46 @@
   explicit Superstructure(
       control_loops::SuperstructureQueue *my_superstructure =
           &control_loops::superstructure_queue);
+  enum State {
+    // Waiting to receive data before doing anything.
+    UNINITIALIZED = 0,
+    // Estimating the starting location.
+    INITIALIZING = 1,
+    // Moving the intake to find an index pulse.
+    ZEROING_INTAKE = 2,
+    // Moving the arm to find an index pulse.
+    ZEROING_ARM = 3,
+    // All good!
+    RUNNING = 4,
+    // Internal error caused the superstructure to abort.
+    ESTOP = 5,
+  };
+
+  State state() const { return state_; }
 
  protected:
   virtual void RunIteration(
-      const control_loops::SuperstructureQueue::Goal *goal,
+      const control_loops::SuperstructureQueue::Goal *unsafe_goal,
       const control_loops::SuperstructureQueue::Position *position,
-      ::aos::control_loops::Output *output,
+      control_loops::SuperstructureQueue::Output *output,
       control_loops::SuperstructureQueue::Status *status) override;
 
  private:
+  friend class testing::SuperstructureTest_DisabledGoalTest_Test;
+  Intake intake_;
+  Arm arm_;
+
+  State state_ = UNINITIALIZED;
+  State last_state_ = UNINITIALIZED;
+
+  // Sets state_ to the correct state given the current state of the zeroing
+  // estimators.
+  void UpdateZeroingState();
+
   DISALLOW_COPY_AND_ASSIGN(Superstructure);
 };
 
+}  // namespace superstructure
 }  // namespace control_loops
 }  // namespace y2016
 
diff --git a/y2016/control_loops/superstructure/superstructure.q b/y2016/control_loops/superstructure/superstructure.q
index f7a93f8..96594e1 100644
--- a/y2016/control_loops/superstructure/superstructure.q
+++ b/y2016/control_loops/superstructure/superstructure.q
@@ -3,24 +3,89 @@
 import "aos/common/controls/control_loops.q";
 import "frc971/control_loops/control_loops.q";
 
+struct JointState {
+  // Angle of the joint in radians.
+  double angle;
+  // Angular velocity of the joint in radians/second.
+  float angular_velocity;
+  // Profiled goal angle of the joint in radians.
+  double goal_angle;
+  // Profiled goal angular velocity of the joint in radians/second.
+  double goal_angular_velocity;
+
+  // State of the estimator.
+  .frc971.EstimatorState estimator_state;
+};
+
 queue_group SuperstructureQueue {
   implements aos.control_loops.ControlLoop;
 
   message Goal {
-    double value;
+    // Zero on the intake is when the horizontal tube stock members are level
+    // with the ground.  This will be essentially when we are in the intaking
+    // position.  Positive is up.  The angle is measured relative to the top
+    // of the robot frame.
+    // Zero on the shoulder is horizontal.  Positive is up.  The angle is
+    // measured relative to the top of the robot frame.
+    // Zero on the wrist is horizontal and landed in the bellypan.  Positive is
+    // up.  The angle is measured relative to the top of the robot frame.
+
+    // Goal angles and angular velocities of the superstructure subsystems.
+    double angle_intake;
+    double angle_shoulder;
+    // In relation to the ground plane.
+    double angle_wrist;
+
+    // Caps on velocity/acceleration for profiling. 0 for the default.
+    float max_angular_velocity_intake;
+    float max_angular_velocity_shoulder;
+    float max_angular_velocity_wrist;
+
+    float max_angular_acceleration_intake;
+    float max_angular_acceleration_shoulder;
+    float max_angular_acceleration_wrist;
   };
 
   message Status {
-    double value;
+    // Are the superstructure subsystems zeroed?
+    bool zeroed;
+
+    // If true, we have aborted.
+    bool estopped;
+
+    // The internal state of the state machine.
+    int32_t state;
+
+
+    // Estimated angles and angular velocities of the superstructure subsystems.
+    JointState intake;
+    JointState shoulder;
+    JointState wrist;
   };
 
   message Position {
-    double value;
+    // Zero for the intake potentiometer value is horizontal, and positive is
+    // up.
+    // Zero for the shoulder potentiometer value is horizontal, and positive is
+    // up.
+    // Zero for the wrist potentiometer value is parallel to the arm and with
+    // the shooter wheels pointed towards the shoulder joint.  This is measured
+    // relative to the arm, not the ground, not like the world we actually
+    // present to users.
+    .frc971.PotAndIndexPosition intake;
+    .frc971.PotAndIndexPosition shoulder;
+    .frc971.PotAndIndexPosition wrist;
+  };
+
+  message Output {
+    double voltage_intake;
+    double voltage_shoulder;
+    double voltage_wrist;
   };
 
   queue Goal goal;
   queue Position position;
-  queue aos.control_loops.Output output;
+  queue Output output;
   queue Status status;
 };
 
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index 3a843cd..f168fb3 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -7,30 +7,181 @@
 #include "gtest/gtest.h"
 #include "aos/common/queue.h"
 #include "aos/common/controls/control_loop_test.h"
+#include "aos/common/commonmath.h"
+#include "aos/common/time.h"
+#include "frc971/control_loops/position_sensor_sim.h"
+#include "frc971/control_loops/team_number_test_environment.h"
 #include "y2016/control_loops/superstructure/superstructure.q.h"
+#include "y2016/control_loops/superstructure/intake_plant.h"
+#include "y2016/control_loops/superstructure/arm_plant.h"
+#include "y2016/constants.h"
 
 using ::aos::time::Time;
+using ::frc971::control_loops::PositionSensorSimulator;
 
 namespace y2016 {
 namespace control_loops {
+namespace superstructure {
 namespace testing {
 
+class ArmPlant : public StateFeedbackPlant<4, 2, 2> {
+ public:
+  explicit ArmPlant(StateFeedbackPlant<4, 2, 2> &&other)
+      : StateFeedbackPlant<4, 2, 2>(::std::move(other)) {}
+
+  void CheckU() override {
+    assert(U(0, 0) <= U_max(0, 0) + 0.00001 + shoulder_voltage_offset_);
+    assert(U(0, 0) >= U_min(0, 0) - 0.00001 + shoulder_voltage_offset_);
+    assert(U(1, 0) <= U_max(1, 0) + 0.00001 + wrist_voltage_offset_);
+    assert(U(1, 0) >= U_min(1, 0) - 0.00001 + wrist_voltage_offset_);
+  }
+
+  double shoulder_voltage_offset() const { return shoulder_voltage_offset_; }
+  void set_shoulder_voltage_offset(double shoulder_voltage_offset) {
+    shoulder_voltage_offset_ = shoulder_voltage_offset;
+  }
+
+  double wrist_voltage_offset() const { return wrist_voltage_offset_; }
+  void set_wrist_voltage_offset(double wrist_voltage_offset) {
+    wrist_voltage_offset_ = wrist_voltage_offset;
+  }
+
+ private:
+  double shoulder_voltage_offset_ = 0.0;
+  double wrist_voltage_offset_ = 0.0;
+};
+
+class IntakePlant : public StateFeedbackPlant<2, 1, 1> {
+ public:
+  explicit IntakePlant(StateFeedbackPlant<2, 1, 1> &&other)
+      : StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
+
+  void CheckU() override {
+    for (int i = 0; i < kNumInputs; ++i) {
+      assert(U(i, 0) <= U_max(i, 0) + 0.00001 + voltage_offset_);
+      assert(U(i, 0) >= U_min(i, 0) - 0.00001 + voltage_offset_);
+    }
+  }
+
+  double voltage_offset() const { return voltage_offset_; }
+  void set_voltage_offset(double voltage_offset) {
+    voltage_offset_ = voltage_offset;
+  }
+
+ private:
+  double voltage_offset_ = 0.0;
+};
+
 // Class which simulates the superstructure and sends out queue messages with
 // the position.
 class SuperstructureSimulation {
  public:
+  static constexpr double kNoiseScalar = 0.1;
   SuperstructureSimulation()
-      : superstructure_queue_(".y2016.control_loops.superstructure", 0x0,
-                               ".y2016.control_loops.superstructure.goal",
-                               ".y2016.control_loops.superstructure.status",
-                               ".y2016.control_loops.superstructure.output",
-                               ".y2016.control_loops.superstructure.status") {
+      : intake_plant_(new IntakePlant(MakeIntakePlant())),
+        plant_arm_(new ArmPlant(MakeArmPlant())),
+        pot_encoder_intake_(0.0),
+        pot_encoder_shoulder_(0.0),
+        pot_encoder_wrist_(0.0),
+        superstructure_queue_(".y2016.control_loops.superstructure", 0x0,
+                              ".y2016.control_loops.superstructure.goal",
+                              ".y2016.control_loops.superstructure.status",
+                              ".y2016.control_loops.superstructure.output",
+                              ".y2016.control_loops.superstructure.status") {
+    InitializeIntakePosition(0.0);
+    InitializeShoulderPosition(0.0);
+    InitializeWristPosition(0.0);
   }
 
-  // Simulates superstructure for a single timestep.
-  void Simulate() { EXPECT_TRUE(superstructure_queue_.output.FetchLatest()); }
+  void InitializeIntakePosition(double start_pos) {
+    intake_plant_->mutable_X(0, 0) = start_pos;
+    intake_plant_->mutable_X(1, 0) = 0.0;
+
+    pot_encoder_intake_.Initialize(start_pos, kNoiseScalar);
+  }
+
+  void InitializeShoulderPosition(double start_pos) {
+    plant_arm_->mutable_X(0, 0) = start_pos;
+    plant_arm_->mutable_X(1, 0) = 0.0;
+
+    pot_encoder_shoulder_.Initialize(start_pos, kNoiseScalar);
+  }
+
+  // Must be called after any changes to InitializeShoulderPosition.
+  void InitializeWristPosition(double start_pos) {
+    plant_arm_->mutable_X(2, 0) = start_pos + plant_arm_->X(0, 0);
+    plant_arm_->mutable_X(3, 0) = 0.0;
+
+    pot_encoder_wrist_.Initialize(start_pos, kNoiseScalar);
+  }
+
+  // Sends a queue message with the position.
+  void SendPositionMessage() {
+    ::aos::ScopedMessagePtr<control_loops::SuperstructureQueue::Position>
+        position = superstructure_queue_.position.MakeMessage();
+
+    pot_encoder_intake_.GetSensorValues(&position->intake);
+    pot_encoder_shoulder_.GetSensorValues(&position->shoulder);
+    pot_encoder_wrist_.GetSensorValues(&position->wrist);
+
+    position.Send();
+  }
+
+  // Sets the difference between the commanded and applied powers.
+  // This lets us test that the integrators work.
+  void set_power_error(double power_error_intake, double power_error_shoulder,
+                       double power_error_wrist) {
+    intake_plant_->set_voltage_offset(power_error_intake);
+    plant_arm_->set_shoulder_voltage_offset(power_error_shoulder);
+    plant_arm_->set_wrist_voltage_offset(power_error_wrist);
+  }
+
+  // Simulates for a single timestep.
+  void Simulate() {
+    EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
+
+    // Feed voltages into physics simulation.
+    intake_plant_->mutable_U() << superstructure_queue_.output->voltage_intake +
+                                      intake_plant_->voltage_offset();
+
+    plant_arm_->mutable_U() << superstructure_queue_.output->voltage_shoulder +
+                                   plant_arm_->shoulder_voltage_offset(),
+        superstructure_queue_.output->voltage_wrist +
+            plant_arm_->wrist_voltage_offset();
+
+    // Use the plant to generate the next physical state given the voltages to
+    // the motors.
+    intake_plant_->Update();
+    plant_arm_->Update();
+
+    const double angle_intake = intake_plant_->Y(0, 0);
+    const double angle_shoulder = plant_arm_->Y(0, 0);
+    const double angle_wrist = plant_arm_->Y(1, 0);
+
+    // Use the physical state to simulate sensor readings.
+    pot_encoder_intake_.MoveTo(angle_intake);
+    pot_encoder_shoulder_.MoveTo(angle_shoulder);
+    pot_encoder_wrist_.MoveTo(angle_wrist);
+
+    // Validate that everything is within range.
+    EXPECT_GE(angle_intake, constants::GetValues().intake.limits.lower_hard);
+    EXPECT_LE(angle_intake, constants::GetValues().intake.limits.upper_hard);
+    EXPECT_GE(angle_shoulder,
+              constants::GetValues().shoulder.limits.lower_hard);
+    EXPECT_LE(angle_shoulder,
+              constants::GetValues().shoulder.limits.upper_hard);
+    EXPECT_GE(angle_wrist, constants::GetValues().wrist.limits.lower_hard);
+    EXPECT_LE(angle_wrist, constants::GetValues().wrist.limits.upper_hard);
+  }
 
  private:
+  ::std::unique_ptr<IntakePlant> intake_plant_;
+  ::std::unique_ptr<ArmPlant> plant_arm_;
+
+  PositionSensorSimulator pot_encoder_intake_;
+  PositionSensorSimulator pot_encoder_shoulder_;
+  PositionSensorSimulator pot_encoder_wrist_;
+
   SuperstructureQueue superstructure_queue_;
 };
 
@@ -38,21 +189,37 @@
  protected:
   SuperstructureTest()
       : superstructure_queue_(".y2016.control_loops.superstructure", 0x0,
-                               ".y2016.control_loops.superstructure.goal",
-                               ".y2016.control_loops.superstructure.status",
-                               ".y2016.control_loops.superstructure.output",
-                               ".y2016.control_loops.superstructure.status"),
+                              ".y2016.control_loops.superstructure.goal",
+                              ".y2016.control_loops.superstructure.status",
+                              ".y2016.control_loops.superstructure.output",
+                              ".y2016.control_loops.superstructure.status"),
         superstructure_(&superstructure_queue_),
         superstructure_plant_() {}
 
   void VerifyNearGoal() {
-    // TODO(phil): Implement a check here.
+    superstructure_queue_.goal.FetchLatest();
+    superstructure_queue_.status.FetchLatest();
+
+    EXPECT_TRUE(superstructure_queue_.goal.get() != nullptr);
+    EXPECT_TRUE(superstructure_queue_.status.get() != nullptr);
+
+    EXPECT_NEAR(superstructure_queue_.goal->angle_intake,
+                superstructure_queue_.status->intake.angle, 0.001);
+    EXPECT_NEAR(superstructure_queue_.goal->angle_shoulder,
+                superstructure_queue_.status->shoulder.angle, 0.001);
+    EXPECT_NEAR(superstructure_queue_.goal->angle_wrist,
+                superstructure_queue_.status->wrist.angle, 0.001);
   }
 
   // Runs one iteration of the whole simulation and checks that separation
   // remains reasonable.
   void RunIteration(bool enabled = true) {
     SendMessages(enabled);
+
+    superstructure_plant_.SendPositionMessage();
+    superstructure_.Iterate();
+    superstructure_plant_.Simulate();
+
     TickTime();
   }
 
@@ -76,11 +243,227 @@
 
 // Tests that the superstructure does nothing when the goal is zero.
 TEST_F(SuperstructureTest, DoesNothing) {
+  ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+                  .angle_intake(0.0)
+                  .angle_shoulder(0.0)
+                  .angle_wrist(0.0)
+                  .max_angular_velocity_intake(20)
+                  .max_angular_velocity_shoulder(20)
+                  .max_angular_velocity_wrist(20)
+                  .max_angular_acceleration_intake(20)
+                  .max_angular_acceleration_shoulder(20)
+                  .max_angular_acceleration_wrist(20)
+                  .Send());
+
   // TODO(phil): Send a goal of some sort.
-  RunIteration();
+  RunForTime(Time::InSeconds(5));
+  VerifyNearGoal();
+}
+
+// Tests that the loop can reach a goal.
+TEST_F(SuperstructureTest, ReachesGoal) {
+  // Set a reasonable goal.
+  ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+                  .angle_intake(M_PI / 4.0)
+                  .angle_shoulder(M_PI / 4.0)
+                  .angle_wrist(M_PI / 4.0)
+                  .max_angular_velocity_intake(20)
+                  .max_angular_acceleration_intake(20)
+                  .max_angular_velocity_shoulder(20)
+                  .max_angular_acceleration_shoulder(20)
+                  .max_angular_velocity_wrist(20)
+                  .max_angular_acceleration_wrist(20)
+                  .Send());
+
+  // Give it a lot of time to get there.
+  RunForTime(Time::InSeconds(5));
+
+  VerifyNearGoal();
+}
+
+// Tests that the loop doesn't try and go beyond the physical range of the
+// mechanisms.
+TEST_F(SuperstructureTest, RespectsRange) {
+  // Set some ridiculous goals to test upper limits.
+  ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+                  .angle_intake(M_PI * 10)
+                  .angle_shoulder(M_PI * 10)
+                  .angle_wrist(M_PI * 10)
+                  .max_angular_velocity_intake(20)
+                  .max_angular_acceleration_intake(20)
+                  .max_angular_velocity_shoulder(20)
+                  .max_angular_acceleration_shoulder(20)
+                  .max_angular_velocity_wrist(20)
+                  .max_angular_acceleration_wrist(20)
+                  .Send());
+
+  RunForTime(Time::InSeconds(10));
+
+  // Check that we are near our soft limit.
+  superstructure_queue_.status.FetchLatest();
+  const auto &values = constants::GetValues();
+  EXPECT_NEAR(values.intake.limits.upper,
+              superstructure_queue_.status->intake.angle, 0.001);
+  EXPECT_NEAR(values.shoulder.limits.upper,
+              superstructure_queue_.status->shoulder.angle, 0.001);
+  EXPECT_NEAR(values.wrist.limits.upper + values.shoulder.limits.upper,
+              superstructure_queue_.status->wrist.angle, 0.001);
+
+  // Set some ridiculous goals to test lower limits.
+  ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+                  .angle_intake(-M_PI * 10)
+                  .angle_shoulder(-M_PI * 10)
+                  .angle_wrist(-M_PI * 10)
+                  .max_angular_velocity_intake(20)
+                  .max_angular_acceleration_intake(20)
+                  .max_angular_velocity_shoulder(20)
+                  .max_angular_acceleration_shoulder(20)
+                  .max_angular_velocity_wrist(20)
+                  .max_angular_acceleration_wrist(20)
+                  .Send());
+
+  RunForTime(Time::InSeconds(10));
+
+  // Check that we are near our soft limit.
+  superstructure_queue_.status.FetchLatest();
+  EXPECT_NEAR(values.intake.limits.lower,
+              superstructure_queue_.status->intake.angle, 0.001);
+  EXPECT_NEAR(values.shoulder.limits.lower,
+              superstructure_queue_.status->shoulder.angle, 0.001);
+  EXPECT_NEAR(values.wrist.limits.lower + values.shoulder.limits.lower,
+              superstructure_queue_.status->wrist.angle, 0.001);
+}
+
+// Tests that the loop zeroes when run for a while.
+TEST_F(SuperstructureTest, ZeroTest) {
+  ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+                  .angle_intake(0.0)
+                  .angle_shoulder(0.0)
+                  .angle_wrist(0.0)
+                  .max_angular_velocity_intake(20)
+                  .max_angular_acceleration_intake(20)
+                  .max_angular_velocity_shoulder(20)
+                  .max_angular_acceleration_shoulder(20)
+                  .max_angular_velocity_wrist(20)
+                  .max_angular_acceleration_wrist(20)
+                  .Send());
+
+  RunForTime(Time::InSeconds(5));
+
+  VerifyNearGoal();
+}
+
+// Tests that the loop zeroes when run for a while without a goal.
+TEST_F(SuperstructureTest, ZeroNoGoal) {
+  RunForTime(Time::InSeconds(5));
+
+  EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
+}
+
+// Tests that starting at the lower hardstops doesn't cause an abort.
+TEST_F(SuperstructureTest, LowerHardstopStartup) {
+  superstructure_plant_.InitializeIntakePosition(
+      constants::GetValues().intake.limits.lower);
+  superstructure_plant_.InitializeShoulderPosition(
+      constants::GetValues().shoulder.limits.lower);
+  superstructure_plant_.InitializeWristPosition(
+      constants::GetValues().wrist.limits.lower);
+  ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+                  .angle_intake(0.0)
+                  .angle_shoulder(0.0)
+                  .angle_wrist(0.0)
+                  .Send());
+  // We have to wait for it to put the elevator in a safe position as well.
+  RunForTime(Time::InSeconds(5));
+
+  VerifyNearGoal();
+}
+
+// Tests that starting at the upper hardstops doesn't cause an abort.
+TEST_F(SuperstructureTest, UpperHardstopStartup) {
+  superstructure_plant_.InitializeIntakePosition(
+      constants::GetValues().intake.limits.upper);
+  superstructure_plant_.InitializeShoulderPosition(
+      constants::GetValues().shoulder.limits.upper);
+  superstructure_plant_.InitializeWristPosition(
+      constants::GetValues().wrist.limits.upper);
+  ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+                  .angle_intake(0.0)
+                  .angle_shoulder(0.0)
+                  .angle_wrist(0.0)
+                  .Send());
+  // We have to wait for it to put the elevator in a safe position as well.
+  RunForTime(Time::InSeconds(5));
+
+  VerifyNearGoal();
+}
+
+// Tests that resetting WPILib results in a rezero.
+TEST_F(SuperstructureTest, ResetTest) {
+  superstructure_plant_.InitializeIntakePosition(
+      constants::GetValues().intake.limits.upper);
+  superstructure_plant_.InitializeShoulderPosition(
+      constants::GetValues().shoulder.limits.upper);
+  superstructure_plant_.InitializeWristPosition(
+      constants::GetValues().wrist.limits.upper);
+  ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+                  .angle_intake(0.3)
+                  .angle_shoulder(0.3)
+                  .angle_wrist(0.3)
+                  .Send());
+  RunForTime(Time::InSeconds(5));
+
+  EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
+  VerifyNearGoal();
+  SimulateSensorReset();
+  RunForTime(Time::InMS(100));
+  EXPECT_NE(Superstructure::RUNNING, superstructure_.state());
+  RunForTime(Time::InMS(6000));
+  EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
+  VerifyNearGoal();
+}
+
+// Tests that the internal goals don't change while disabled.
+TEST_F(SuperstructureTest, DisabledGoalTest) {
+  ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+                  .angle_intake(0.03)
+                  .angle_shoulder(0.03)
+                  .angle_wrist(0.03)
+                  .Send());
+
+  RunForTime(Time::InMS(100), false);
+  EXPECT_EQ(0.0, superstructure_.intake_.goal(0, 0));
+  EXPECT_EQ(0.0, superstructure_.arm_.goal(0, 0));
+  EXPECT_EQ(0.0, superstructure_.arm_.goal(2, 0));
+
+  // Now make sure they move correctly
+  RunForTime(Time::InMS(4000), true);
+  EXPECT_NE(0.0, superstructure_.intake_.goal(0, 0));
+  EXPECT_NE(0.0, superstructure_.arm_.goal(0, 0));
+  EXPECT_NE(0.0, superstructure_.arm_.goal(2, 0));
+}
+
+// Tests that the integrators works.
+TEST_F(SuperstructureTest, IntegratorTest) {
+  superstructure_plant_.InitializeIntakePosition(
+      constants::GetValues().intake.limits.lower);
+  superstructure_plant_.InitializeShoulderPosition(
+      constants::GetValues().shoulder.limits.lower);
+  superstructure_plant_.InitializeWristPosition(
+      constants::GetValues().wrist.limits.lower);
+  superstructure_plant_.set_power_error(1.0, 1.0, 1.0);
+  superstructure_queue_.goal.MakeWithBuilder()
+    .angle_intake(0.0)
+    .angle_shoulder(0.0)
+    .angle_wrist(0.0)
+    .Send();
+
+  RunForTime(Time::InSeconds(8));
+
   VerifyNearGoal();
 }
 
 }  // namespace testing
+}  // namespace superstructure
 }  // namespace control_loops
 }  // namespace frc971
diff --git a/y2016/control_loops/superstructure/superstructure_main.cc b/y2016/control_loops/superstructure/superstructure_main.cc
index d825eeb..f4987fb 100644
--- a/y2016/control_loops/superstructure/superstructure_main.cc
+++ b/y2016/control_loops/superstructure/superstructure_main.cc
@@ -4,7 +4,7 @@
 
 int main() {
   ::aos::Init();
-  ::y2016::control_loops::Superstructure superstructure;
+  ::y2016::control_loops::superstructure::Superstructure superstructure;
   superstructure.Run();
   ::aos::Cleanup();
   return 0;