diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 214fc6c..c4ef468 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -14,6 +14,13 @@
 namespace superstructure {
 
 namespace {
+constexpr double kIntakeEncoderIndexDifference =
+    constants::Values::kIntakeEncoderIndexDifference;
+constexpr double kWristEncoderIndexDifference =
+    constants::Values::kWristEncoderIndexDifference;
+constexpr double kShoulderEncoderIndexDifference =
+    constants::Values::kShoulderEncoderIndexDifference;
+
 constexpr double kZeroingVoltage = 4.0;
 }  // namespace
 
@@ -22,25 +29,74 @@
     : 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;
+bool Superstructure::IsArmNear(double shoulder_tolerance,
+                               double wrist_tolerance) {
+  return ((arm_.unprofiled_goal() - arm_.X_hat())
+              .block<2, 1>(0, 0)
+              .lpNorm<Eigen::Infinity>() < shoulder_tolerance) &&
+         ((arm_.unprofiled_goal() - arm_.X_hat())
+              .block<4, 1>(0, 0)
+              .lpNorm<Eigen::Infinity>() < wrist_tolerance) &&
+         ((arm_.unprofiled_goal() - arm_.goal())
+              .block<4, 1>(0, 0)
+              .lpNorm<Eigen::Infinity>() < 1e-6);
+}
+
+bool Superstructure::IsArmNear(double tolerance) {
+  return ((arm_.unprofiled_goal() - arm_.X_hat())
+              .block<4, 1>(0, 0)
+              .lpNorm<Eigen::Infinity>() < tolerance) &&
+         ((arm_.unprofiled_goal() - arm_.goal())
+              .block<4, 1>(0, 0)
+              .lpNorm<Eigen::Infinity>() < 1e-6);
+}
+
+bool Superstructure::IsIntakeNear(double tolerance) {
+  return ((intake_.unprofiled_goal() - intake_.X_hat())
+              .block<2, 1>(0, 0)
+              .lpNorm<Eigen::Infinity>() < tolerance);
+}
+
+double Superstructure::MoveButKeepAbove(double reference_angle,
+                                        double current_angle,
+                                        double move_distance) {
+  return -MoveButKeepBelow(-reference_angle, -current_angle, -move_distance);
+}
+
+double Superstructure::MoveButKeepBelow(double reference_angle,
+                                        double current_angle,
+                                        double move_distance) {
+  // There are 3 interesting places to move to.
+  const double small_negative_move = current_angle - move_distance;
+  const double small_positive_move = current_angle + move_distance;
+  // And the reference angle.
+
+  // Move the the highest one that is below reference_angle.
+  if (small_negative_move > reference_angle) {
+    return reference_angle;
+  } else if (small_positive_move > reference_angle) {
+    return small_negative_move;
   } else {
-    state_ = RUNNING;
+    return small_positive_move;
   }
 }
 
+constexpr double Superstructure::kShoulderMiddleAngle;
+constexpr double Superstructure::kLooseTolerance;
+constexpr double Superstructure::kIntakeUpperClear;
+constexpr double Superstructure::kIntakeLowerClear;
+constexpr double Superstructure::kShoulderUpAngle;
+constexpr double Superstructure::kShoulderLanded;
+constexpr double Superstructure::kTightTolerance;
+constexpr double Superstructure::kWristAlmostLevel;
+constexpr double Superstructure::kShoulderWristClearAngle;
+
 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) {
+  const State state_before_switch = state_;
   if (WasReset()) {
     LOG(ERROR, "WPILib reset, restarting\n");
     arm_.Reset();
@@ -54,58 +110,221 @@
   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.
+  // There are 2 main zeroing paths, HIGH_ARM_ZERO and LOW_ARM_ZERO.
+  //
+  // HIGH_ARM_ZERO works by lifting the arm all the way up so it is clear,
+  // moving the shooter to be horizontal, moving the intake out, and then moving
+  // the arm back down.
+  //
+  // LOW_ARM_ZERO works by moving the intake out of the way, lifting the arm up,
+  // leveling the shooter, and then moving back down.
 
   switch (state_) {
     case UNINITIALIZED:
-      LOG(DEBUG, "Uninitialized\n");
-      state_ = INITIALIZING;
+      // Wait in the uninitialized state until both the arm and intake are
+      // initialized.
+      LOG(DEBUG, "Uninitialized, waiting for intake and arm\n");
+      if (arm_.initialized() && intake_.initialized()) {
+        state_ = DISABLED_INITIALIZED;
+      }
       disable = true;
       break;
 
-    case INITIALIZING:
-      LOG(DEBUG, "Waiting for accurate initial position.\n");
+    case DISABLED_INITIALIZED:
+      // Wait here until we are either fully zeroed while disabled, or we become
+      // enabled.  At that point, figure out if we should HIGH_ARM_ZERO or
+      // LOW_ARM_ZERO.
+      if (disable) {
+        if (arm_.zeroed() && intake_.zeroed()) {
+          state_ = SLOW_RUNNING;
+        }
+      } else {
+        if (arm_.shoulder_angle() >= kShoulderMiddleAngle) {
+          state_ = HIGH_ARM_ZERO_LIFT_ARM;
+        } else {
+          state_ = LOW_ARM_ZERO_LOWER_INTAKE;
+        }
+      }
+
+      // Set the goals to where we are now so when we start back up, we don't
+      // jump.
+      intake_.ForceGoal(intake_.angle());
+      arm_.ForceGoal(arm_.shoulder_angle(), arm_.wrist_angle());
+      // Set up the profile to be the zeroing profile.
+      intake_.AdjustProfile(0.5, 10);
+      arm_.AdjustProfile(0.5, 10, 0.5, 10);
+
+      // We are not ready to start doing anything yet.
       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 HIGH_ARM_ZERO_LIFT_ARM:
+      if (disable) {
+        state_ = DISABLED_INITIALIZED;
+      } else {
+        // Raise the shoulder up out of the way.
+        arm_.set_unprofiled_goal(kShoulderUpAngle, arm_.wrist_angle());
+        if (IsArmNear(kLooseTolerance)) {
+          // Close enough, start the next move.
+          state_ = HIGH_ARM_ZERO_LEVEL_SHOOTER;
+        }
       }
       break;
 
-    case ZEROING_INTAKE:
-    case ZEROING_ARM:
-      // TODO(adam): Add your magic here.
-      state_ = RUNNING;
+    case HIGH_ARM_ZERO_LEVEL_SHOOTER:
+      if (disable) {
+        state_ = DISABLED_INITIALIZED;
+      } else {
+        // Move the shooter to be level.
+        arm_.set_unprofiled_goal(kShoulderUpAngle, 0.0);
+
+        if (IsArmNear(kLooseTolerance)) {
+          // Close enough, start the next move.
+          state_ = HIGH_ARM_ZERO_MOVE_INTAKE_OUT;
+        }
+      }
       break;
 
+    case HIGH_ARM_ZERO_MOVE_INTAKE_OUT:
+      if (disable) {
+        state_ = DISABLED_INITIALIZED;
+      } else {
+        // If we were just asked to move the intake, make sure it moves far
+        // enough.
+        if (last_state_ != HIGH_ARM_ZERO_MOVE_INTAKE_OUT) {
+          intake_.set_unprofiled_goal(
+              MoveButKeepBelow(kIntakeUpperClear, intake_.angle(),
+                               kIntakeEncoderIndexDifference * 2.5));
+        }
+
+        if (IsIntakeNear(kLooseTolerance)) {
+          // Close enough, start the next move.
+          state_ = HIGH_ARM_ZERO_LOWER_ARM;
+        }
+      }
+      break;
+
+    case HIGH_ARM_ZERO_LOWER_ARM:
+      if (disable) {
+        state_ = DISABLED_INITIALIZED;
+      } else {
+        // Land the shooter in the belly-pan.  It should be zeroed by the time
+        // it gets there.  If not, just estop.
+        arm_.set_unprofiled_goal(kShoulderLanded, 0.0);
+        if (arm_.zeroed() && intake_.zeroed()) {
+          state_ = RUNNING;
+        } else if (IsArmNear(kLooseTolerance)) {
+          LOG(ERROR,
+              "Failed to zero while executing the HIGH_ARM_ZERO sequence. "
+              "Arm: %d Intake %d\n",
+              arm_.zeroed(), intake_.zeroed());
+          state_ = ESTOP;
+        }
+      }
+      break;
+
+    case LOW_ARM_ZERO_LOWER_INTAKE:
+      if (disable) {
+        state_ = DISABLED_INITIALIZED;
+      } else {
+        // Move the intake down out of the way of the arm.  Make sure to move it
+        // far enough to zero.
+        if (last_state_ != LOW_ARM_ZERO_LOWER_INTAKE) {
+          intake_.set_unprofiled_goal(
+              MoveButKeepBelow(kIntakeLowerClear, intake_.angle(),
+                               kIntakeEncoderIndexDifference * 2.5));
+        }
+        if (IsIntakeNear(kLooseTolerance)) {
+          if (::std::abs(arm_.wrist_angle()) < kWristAlmostLevel) {
+            state_ = LOW_ARM_ZERO_MAYBE_LEVEL_SHOOTER;
+          } else {
+            state_ = LOW_ARM_ZERO_LIFT_SHOULDER;
+          }
+        }
+      }
+      break;
+
+    case LOW_ARM_ZERO_MAYBE_LEVEL_SHOOTER:
+      if (disable) {
+        state_ = DISABLED_INITIALIZED;
+      } else {
+        // If we are supposed to level the shooter, set it to level, and wait
+        // until it is very close to level.
+        arm_.set_unprofiled_goal(arm_.unprofiled_goal(0, 0), 0.0);
+        if (IsArmNear(kLooseTolerance, kTightTolerance)) {
+          state_ = LOW_ARM_ZERO_LIFT_SHOULDER;
+        }
+      }
+      break;
+
+    case LOW_ARM_ZERO_LIFT_SHOULDER:
+      if (disable) {
+        state_ = DISABLED_INITIALIZED;
+      } else {
+        // Decide where to move to.  We need to move far enough to see an index
+        // pulse, but must also get high enough that we can safely level the
+        // shooter.
+        if (last_state_ != LOW_ARM_ZERO_LIFT_SHOULDER) {
+          arm_.set_unprofiled_goal(
+              MoveButKeepAbove(kShoulderWristClearAngle, arm_.shoulder_angle(),
+                               ::std::max(kWristEncoderIndexDifference,
+                                          kShoulderEncoderIndexDifference) *
+                                   2.5),
+              arm_.unprofiled_goal(2, 0));
+        }
+
+        // Wait until we are level and then go for it.
+        if (IsArmNear(kLooseTolerance)) {
+          state_ = LOW_ARM_ZERO_LEVEL_SHOOTER;
+        }
+      }
+      break;
+
+    case LOW_ARM_ZERO_LEVEL_SHOOTER:
+      if (disable) {
+        state_ = DISABLED_INITIALIZED;
+      } else {
+        // Move the shooter level (and keep the same height).  We don't want to
+        // got to RUNNING until we are completely level so that we don't
+        // give control back in a weird case where we might crash.
+        arm_.set_unprofiled_goal(arm_.unprofiled_goal(0, 0), 0.0);
+        if (IsArmNear(kLooseTolerance)) {
+          if (arm_.zeroed() && intake_.zeroed()) {
+            state_ = RUNNING;
+          } else {
+            LOG(ERROR,
+                "Failed to zero while executing the LOW_ARM_ZERO sequence. "
+                "Arm: %d Intake %d\n",
+                arm_.zeroed(), intake_.zeroed());
+            state_ = ESTOP;
+          }
+        }
+      }
+      break;
+
+    case SLOW_RUNNING:
     case RUNNING:
+      // TODO(austin): Exit SLOW_RUNNING if we are not collided.
       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);
+            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);
+            unsafe_goal->max_angular_acceleration_intake);
 
         arm_.set_unprofiled_goal(unsafe_goal->angle_shoulder,
-                                 unsafe_goal->angle_wrist);
+            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;
+      // 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 || state_ == SLOW_RUNNING) {
+        // ESTOP if we hit the hard limits.
+        if ((arm_.CheckHardLimits() || intake_.CheckHardLimits()) && output) {
+          state_ = ESTOP;
+        }
       }
       break;
 
@@ -115,15 +334,6 @@
       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);
@@ -174,7 +384,7 @@
 
   status->state = state_;
 
-  last_state_ = state_;
+  last_state_ = state_before_switch;
 }
 
 }  // namespace superstructure
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index 9a04c3c..1350a15 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -24,23 +24,80 @@
   explicit Superstructure(
       control_loops::SuperstructureQueue *my_superstructure =
           &control_loops::superstructure_queue);
+
+  // This is the angle above which we will do a HIGH_ARM_ZERO, and below which
+  // we will do a LOW_ARM_ZERO.
+  static constexpr double kShoulderMiddleAngle = M_PI / 4.0;
+  // This is the large scale movement tolerance.
+  static constexpr double kLooseTolerance = 0.05;
+
+  // This is the small scale movement tolerance.
+  static constexpr double kTightTolerance = 0.01;
+
+  // This is the angle such that the intake will clear the arm when the shooter
+  // is level.
+  static constexpr double kIntakeUpperClear = 1.1;
+  // This is the angle such that the intake will clear the arm when the shooter
+  // is at almost any position.
+  static constexpr double kIntakeLowerClear = 0.5;
+
+  // This is the angle that the shoulder will go to when doing the
+  // HIGH_ARM_ZERO.
+  static constexpr double kShoulderUpAngle = M_PI / 2.0;
+
+  // This is the angle that the shoulder will go down to when landing in the
+  // bellypan.
+  static constexpr double kShoulderLanded = -0.02;
+
+  // This is the angle below which we consider the wrist close enough to level
+  // that we should move it to level before doing anything.
+  static constexpr double kWristAlmostLevel = 0.10;
+
+  // This is the angle that the shoulder will go down to when raising up before
+  // leveling the shooter for calibration.
+  static constexpr double kShoulderWristClearAngle = 0.6;
+
   enum State {
-    // Waiting to receive data before doing anything.
+    // Wait for all the filters to be ready before starting the initialization
+    // process.
     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,
+
+    // We now are ready to decide how to zero.  Decide what to do once we are
+    // enabled.
+    DISABLED_INITIALIZED = 1,
+
+    // Lift the arm up out of the way.
+    HIGH_ARM_ZERO_LIFT_ARM = 2,
+
+    HIGH_ARM_ZERO_LEVEL_SHOOTER = 3,
+
+    HIGH_ARM_ZERO_MOVE_INTAKE_OUT = 4,
+
+    HIGH_ARM_ZERO_LOWER_ARM = 6,
+
+    LOW_ARM_ZERO_LOWER_INTAKE = 7,
+    LOW_ARM_ZERO_MAYBE_LEVEL_SHOOTER = 8,
+    LOW_ARM_ZERO_LIFT_SHOULDER = 9,
+    LOW_ARM_ZERO_LEVEL_SHOOTER = 11,
+    // Run, but limit power to zeroing voltages.
+    SLOW_RUNNING = 12,
+    // Run with full power.
+    RUNNING = 13,
     // Internal error caused the superstructure to abort.
-    ESTOP = 5,
+    ESTOP = 14,
   };
 
   State state() const { return state_; }
 
+  // Returns the value to move the joint to such that it will stay below
+  // reference_angle starting at current_angle, but move at least move_distance
+  static double MoveButKeepBelow(double reference_angle, double current_angle,
+                                 double move_distance);
+  // Returns the value to move the joint to such that it will stay above
+  // reference_angle starting at current_angle, but move at least move_distance
+  static double MoveButKeepAbove(double reference_angle, double current_angle,
+                                 double move_distance);
+
  protected:
   virtual void RunIteration(
       const control_loops::SuperstructureQueue::Goal *unsafe_goal,
@@ -56,9 +113,11 @@
   State state_ = UNINITIALIZED;
   State last_state_ = UNINITIALIZED;
 
-  // Sets state_ to the correct state given the current state of the zeroing
-  // estimators.
-  void UpdateZeroingState();
+  // Returns true if the profile has finished, and the joint is within the
+  // specified tolerance.
+  bool IsArmNear(double tolerance);
+  bool IsArmNear(double shoulder_tolerance, double wrist_tolerance);
+  bool IsIntakeNear(double tolerance);
 
   DISALLOW_COPY_AND_ASSIGN(Superstructure);
 };
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index f3d8752..d20b345 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -27,21 +27,19 @@
 
   // Updates our estimator with the latest position.
   void Correct(::frc971::PotAndIndexPosition position);
+  // Runs the controller and profile generator for a cycle.
+  void Update(bool disabled);
+  // Sets the maximum voltage that will be commanded by the loop.
+  void set_max_voltage(double voltage);
 
   // 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();
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index 1bd4eca..78fae9d 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -80,7 +80,7 @@
   static constexpr double kNoiseScalar = 0.1;
   SuperstructureSimulation()
       : intake_plant_(new IntakePlant(MakeIntakePlant())),
-        plant_arm_(new ArmPlant(MakeArmPlant())),
+        arm_plant_(new ArmPlant(MakeArmPlant())),
         pot_encoder_intake_(constants::Values::kIntakeEncoderIndexDifference),
         pot_encoder_shoulder_(
             constants::Values::kShoulderEncoderIndexDifference),
@@ -103,16 +103,16 @@
   }
 
   void InitializeShoulderPosition(double start_pos) {
-    plant_arm_->mutable_X(0, 0) = start_pos;
-    plant_arm_->mutable_X(1, 0) = 0.0;
+    arm_plant_->mutable_X(0, 0) = start_pos;
+    arm_plant_->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;
+    arm_plant_->mutable_X(2, 0) = start_pos + arm_plant_->X(0, 0);
+    arm_plant_->mutable_X(3, 0) = 0.0;
 
     pot_encoder_wrist_.Initialize(start_pos, kNoiseScalar);
   }
@@ -129,13 +129,17 @@
     position.Send();
   }
 
+  double shoulder_angle() const { return arm_plant_->X(0, 0); }
+  double wrist_angle() const { return arm_plant_->X(2, 0); }
+  double intake_angle() const { return intake_plant_->X(0, 0); }
+
   // 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);
+    arm_plant_->set_shoulder_voltage_offset(power_error_shoulder);
+    arm_plant_->set_wrist_voltage_offset(power_error_wrist);
   }
 
   // Simulates for a single timestep.
@@ -146,19 +150,19 @@
     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(),
+    arm_plant_->mutable_U() << superstructure_queue_.output->voltage_shoulder +
+                                   arm_plant_->shoulder_voltage_offset(),
         superstructure_queue_.output->voltage_wrist +
-            plant_arm_->wrist_voltage_offset();
+            arm_plant_->wrist_voltage_offset();
 
     // Use the plant to generate the next physical state given the voltages to
     // the motors.
     intake_plant_->Update();
-    plant_arm_->Update();
+    arm_plant_->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);
+    const double angle_shoulder = arm_plant_->Y(0, 0);
+    const double angle_wrist = arm_plant_->Y(1, 0);
 
     // Use the physical state to simulate sensor readings.
     pot_encoder_intake_.MoveTo(angle_intake);
@@ -176,7 +180,7 @@
 
  private:
   ::std::unique_ptr<IntakePlant> intake_plant_;
-  ::std::unique_ptr<ArmPlant> plant_arm_;
+  ::std::unique_ptr<ArmPlant> arm_plant_;
 
   PositionSensorSimulator pot_encoder_intake_;
   PositionSensorSimulator pot_encoder_shoulder_;
@@ -209,6 +213,13 @@
                 superstructure_queue_.status->shoulder.angle, 0.001);
     EXPECT_NEAR(superstructure_queue_.goal->angle_wrist,
                 superstructure_queue_.status->wrist.angle, 0.001);
+
+    EXPECT_NEAR(superstructure_queue_.goal->angle_intake,
+                superstructure_plant_.intake_angle(), 0.001);
+    EXPECT_NEAR(superstructure_queue_.goal->angle_shoulder,
+                superstructure_plant_.shoulder_angle(), 0.001);
+    EXPECT_NEAR(superstructure_queue_.goal->angle_wrist,
+                superstructure_plant_.wrist_angle(), 0.001);
   }
 
   // Runs one iteration of the whole simulation and checks that separation
@@ -349,7 +360,7 @@
                   .max_angular_acceleration_wrist(20)
                   .Send());
 
-  RunForTime(Time::InSeconds(5));
+  RunForTime(Time::InSeconds(10));
 
   VerifyNearGoal();
 }
@@ -375,7 +386,7 @@
                   .angle_wrist(constants::Values::kWristRange.lower)
                   .Send());
   // We have to wait for it to put the elevator in a safe position as well.
-  RunForTime(Time::InSeconds(5));
+  RunForTime(Time::InSeconds(10));
 
   VerifyNearGoal();
 }
@@ -394,7 +405,7 @@
                   .angle_wrist(constants::Values::kWristRange.lower)
                   .Send());
   // We have to wait for it to put the elevator in a safe position as well.
-  RunForTime(Time::InSeconds(5));
+  RunForTime(Time::InSeconds(10));
 
   VerifyNearGoal();
 }
@@ -413,14 +424,14 @@
           .angle_shoulder(constants::Values::kShoulderRange.lower + 0.3)
           .angle_wrist(constants::Values::kWristRange.lower + 0.3)
           .Send());
-  RunForTime(Time::InSeconds(5));
+  RunForTime(Time::InSeconds(10));
 
   EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
   VerifyNearGoal();
   SimulateSensorReset();
   RunForTime(Time::InMS(100));
   EXPECT_NE(Superstructure::RUNNING, superstructure_.state());
-  RunForTime(Time::InMS(6000));
+  RunForTime(Time::InMS(10000));
   EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
   VerifyNearGoal();
 }
@@ -446,6 +457,14 @@
   EXPECT_NE(0.0, superstructure_.arm_.goal(2, 0));
 }
 
+// Tests that MoveButKeepBelow returns sane values.
+TEST_F(SuperstructureTest, MoveButKeepBelowTest) {
+  EXPECT_EQ(1.0, Superstructure::MoveButKeepBelow(1.0, 10.0, 1.0));
+  EXPECT_EQ(1.0, Superstructure::MoveButKeepBelow(1.0, 2.0, 1.0));
+  EXPECT_EQ(0.0, Superstructure::MoveButKeepBelow(1.0, 1.0, 1.0));
+  EXPECT_EQ(1.0, Superstructure::MoveButKeepBelow(1.0, 0.0, 1.0));
+}
+
 // Tests that the integrators works.
 TEST_F(SuperstructureTest, IntegratorTest) {
   superstructure_plant_.InitializeIntakePosition(