diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 6ffc1b8..fe224cc 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -105,17 +105,17 @@
   const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_max() const {
     return coefficients().U_max;
   }
-  Scalar U_max(int i, int j) const { return U_max()(i, j); }
+  Scalar U_max(int i, int j = 0) const { return U_max()(i, j); }
 
   const Eigen::Matrix<Scalar, number_of_states, 1> &X() const { return X_; }
-  Scalar X(int i, int j) const { return X()(i, j); }
+  Scalar X(int i, int j = 0) const { return X()(i, j); }
   const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y() const { return Y_; }
-  Scalar Y(int i, int j) const { return Y()(i, j); }
+  Scalar Y(int i, int j = 0) const { return Y()(i, j); }
 
   Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X() { return X_; }
-  Scalar &mutable_X(int i, int j) { return mutable_X()(i, j); }
+  Scalar &mutable_X(int i, int j = 0) { return mutable_X()(i, j); }
   Eigen::Matrix<Scalar, number_of_outputs, 1> &mutable_Y() { return Y_; }
-  Scalar &mutable_Y(int i, int j) { return mutable_Y()(i, j); }
+  Scalar &mutable_Y(int i, int j = 0) { return mutable_Y()(i, j); }
 
   const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
                                        number_of_outputs, Scalar>
@@ -400,7 +400,7 @@
   const Eigen::Matrix<Scalar, number_of_states, 1> &X_hat() const {
     return observer().X_hat();
   }
-  Scalar X_hat(int i, int j) const { return X_hat()(i, j); }
+  Scalar X_hat(int i, int j = 0) const { return X_hat()(i, j); }
   const Eigen::Matrix<Scalar, number_of_states, 1> &R() const { return R_; }
   Scalar R(int i, int j) const { return R()(i, j); }
   const Eigen::Matrix<Scalar, number_of_states, 1> &next_R() const {
@@ -421,7 +421,7 @@
   Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X_hat() {
     return observer_.mutable_X_hat();
   }
-  Scalar &mutable_X_hat(int i, int j) { return mutable_X_hat()(i, j); }
+  Scalar &mutable_X_hat(int i, int j = 0) { return mutable_X_hat()(i, j); }
   Eigen::Matrix<Scalar, number_of_states, 1> &mutable_R() { return R_; }
   Scalar &mutable_R(int i, int j) { return mutable_R()(i, j); }
   Eigen::Matrix<Scalar, number_of_states, 1> &mutable_next_R() {
diff --git a/y2018/constants.cc b/y2018/constants.cc
index 42872b9..5326d68 100644
--- a/y2018/constants.cc
+++ b/y2018/constants.cc
@@ -31,40 +31,91 @@
 
 const Values *DoGetValuesForTeam(uint16_t team) {
   Values *const r = new Values();
-  Values::Intake *const intake = &r->intake;
+  Values::IntakeSide *const left_intake = &r->left_intake;
+  Values::IntakeSide *const right_intake = &r->right_intake;
   Values::Proximal *const proximal = &r->proximal;
   Values::Distal *const distal = &r->distal;
 
+  left_intake->zeroing.average_filter_size = Values::kZeroingSampleSize();
+  left_intake->zeroing.one_revolution_distance =
+      M_PI * 2.0 * constants::Values::kIntakeMotorEncoderRatio();
+  left_intake->zeroing.zeroing_threshold = 0.0005;
+  left_intake->zeroing.moving_buffer_size = 20;
+  left_intake->zeroing.allowable_encoder_error = 1.9;
+
+  *right_intake = *left_intake;
+
+  proximal->zeroing.average_filter_size = Values::kZeroingSampleSize();
+  proximal->zeroing.one_revolution_distance =
+      M_PI * 2.0 * constants::Values::kProximalEncoderRatio();
+  proximal->zeroing.zeroing_threshold = 0.0005;
+  proximal->zeroing.moving_buffer_size = 20;
+  proximal->zeroing.allowable_encoder_error = 0.9;
+
+  distal->zeroing.average_filter_size = Values::kZeroingSampleSize();
+  distal->zeroing.one_revolution_distance =
+      M_PI * 2.0 * constants::Values::kDistalEncoderRatio();
+  distal->zeroing.zeroing_threshold = 0.0005;
+  distal->zeroing.moving_buffer_size = 20;
+  distal->zeroing.allowable_encoder_error = 0.9;
+
   switch (team) {
     // A set of constants for tests.
     case 1:
-      r->down_error = 0;
       r->vision_name = "test";
       r->vision_error = -0.030;
-      intake->left_pot_offset = 0;
-      intake->right_pot_offset = 0;
-      proximal->pot_offset = 0;
-      distal->pot_offset = 0;
+
+      left_intake->zeroing.measured_absolute_position = 0.0;
+      left_intake->potentiometer_offset = 0.0;
+      left_intake->spring_offset = 0.0;
+
+      right_intake->zeroing.measured_absolute_position = 0.0;
+      right_intake->potentiometer_offset = 0.0;
+      right_intake->spring_offset = 0.0;
+
+      proximal->zeroing.measured_absolute_position = 0.0;
+      proximal->potentiometer_offset = 0.0;
+
+      distal->zeroing.measured_absolute_position = 0.0;
+      distal->potentiometer_offset = 0.0;
       break;
 
     case kCompTeamNumber:
-      r->down_error = 0;
       r->vision_name = "competition";
       r->vision_error = 0.0;
-      intake->left_pot_offset = 0;
-      intake->right_pot_offset = 0;
-      proximal->pot_offset = 0;
-      distal->pot_offset = 0;
+
+      left_intake->zeroing.measured_absolute_position = 0.0;
+      left_intake->potentiometer_offset = 0.0;
+      left_intake->spring_offset = 0.0;
+
+      right_intake->zeroing.measured_absolute_position = 0.0;
+      right_intake->potentiometer_offset = 0.0;
+      right_intake->spring_offset = 0.0;
+
+      proximal->zeroing.measured_absolute_position = 0.0;
+      proximal->potentiometer_offset = 0.0;
+
+      distal->zeroing.measured_absolute_position = 0.0;
+      distal->potentiometer_offset = 0.0;
       break;
 
     case kPracticeTeamNumber:
-      r->down_error = 0;
       r->vision_name = "practice";
       r->vision_error = 0.0;
-      intake->left_pot_offset = 0;
-      intake->right_pot_offset = 0;
-      proximal->pot_offset = 0;
-      distal->pot_offset = 0;
+
+      left_intake->zeroing.measured_absolute_position = 0.0;
+      left_intake->potentiometer_offset = 0.0;
+      left_intake->spring_offset = 0.0;
+
+      right_intake->zeroing.measured_absolute_position = 0.0;
+      right_intake->potentiometer_offset = 0.0;
+      right_intake->spring_offset = 0.0;
+
+      proximal->zeroing.measured_absolute_position = 0.0;
+      proximal->potentiometer_offset = 0.0;
+
+      distal->zeroing.measured_absolute_position = 0.0;
+      distal->potentiometer_offset = 0.0;
       break;
 
     default:
diff --git a/y2018/constants.h b/y2018/constants.h
index a576f96..73d485a 100644
--- a/y2018/constants.h
+++ b/y2018/constants.h
@@ -24,7 +24,7 @@
 // All ratios are from the encoder shaft to the output units.
 
 struct Values {
-  static constexpr int kZeroingSampleSize() { return 200; }
+  static constexpr size_t kZeroingSampleSize() { return 200; }
 
   static constexpr double kDrivetrainCyclesPerRevolution() { return 512.0; }
   static constexpr double kDrivetrainEncoderCountsPerRevolution() {
@@ -80,23 +80,31 @@
            kIntakeMotorEncoderCountsPerRevolution();
   }
 
-  struct Intake {
-    double left_pot_offset;
-    double right_pot_offset;
+  static constexpr ::frc971::constants::Range kIntakeRange() {
+    return ::frc971::constants::Range{(-0.75 * M_PI), (1.25 * M_PI),
+                                      (-2.0 / 3.0 * M_PI), M_PI};
+  }
+
+  struct IntakeSide {
+    ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants zeroing;
+    double potentiometer_offset;
+    double spring_offset;
   };
-  Intake intake;
+  IntakeSide left_intake;
+  IntakeSide right_intake;
 
   struct Proximal {
-    double pot_offset;
+    ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants zeroing;
+    double potentiometer_offset;
   };
   Proximal proximal;
 
   struct Distal {
-    double pot_offset;
+    ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants zeroing;
+    double potentiometer_offset;
   };
   Distal distal;
 
-  double down_error;
   const char *vision_name;
 
   double vision_error;
diff --git a/y2018/control_loops/superstructure/BUILD b/y2018/control_loops/superstructure/BUILD
index 275d46a..c8b0b84 100644
--- a/y2018/control_loops/superstructure/BUILD
+++ b/y2018/control_loops/superstructure/BUILD
@@ -1,17 +1,62 @@
-package(default_visibility = ['//visibility:public'])
+package(default_visibility = ["//visibility:public"])
 
-load('//aos/build:queues.bzl', 'queue_library')
-
+load("//aos/build:queues.bzl", "queue_library")
 
 queue_library(
-  name = 'superstructure_queue',
-  srcs = [
-    'superstructure.q',
-  ],
-  deps = [
-    '//aos/common/controls:control_loop_queues',
-    '//frc971/control_loops:profiled_subsystem_queue',
-    '//frc971/control_loops:queues',
-  ],
+    name = "superstructure_queue",
+    srcs = [
+        "superstructure.q",
+    ],
+    deps = [
+        "//aos/common/controls:control_loop_queues",
+        "//frc971/control_loops:queues",
+    ],
 )
 
+cc_library(
+    name = "superstructure_lib",
+    srcs = [
+        "superstructure.cc",
+    ],
+    hdrs = [
+        "superstructure.h",
+    ],
+    deps = [
+        ":superstructure_queue",
+        "//aos/common/controls:control_loop",
+        "//frc971/control_loops:queues",
+        "//y2018:constants",
+        "//y2018/control_loops/superstructure/intake",
+    ],
+)
+
+cc_test(
+    name = "superstructure_lib_test",
+    srcs = [
+        "superstructure_lib_test.cc",
+    ],
+    deps = [
+        ":superstructure_lib",
+        ":superstructure_queue",
+        "//aos/common:math",
+        "//aos/common:queues",
+        "//aos/common:time",
+        "//aos/common/controls:control_loop_test",
+        "//aos/testing:googletest",
+        "//frc971/control_loops:position_sensor_sim",
+        "//frc971/control_loops:team_number_test_environment",
+        "//y2018/control_loops/superstructure/intake:intake_plants",
+    ],
+)
+
+cc_binary(
+    name = "superstructure",
+    srcs = [
+        "superstructure_main.cc",
+    ],
+    deps = [
+        ":superstructure_lib",
+        ":superstructure_queue",
+        "//aos/linux_code:init",
+    ],
+)
diff --git a/y2018/control_loops/superstructure/intake/BUILD b/y2018/control_loops/superstructure/intake/BUILD
index bb6fec7..ff2f16e 100644
--- a/y2018/control_loops/superstructure/intake/BUILD
+++ b/y2018/control_loops/superstructure/intake/BUILD
@@ -27,3 +27,24 @@
         "//frc971/control_loops:state_feedback_loop",
     ],
 )
+
+cc_library(
+    name = "intake",
+    srcs = [
+        "intake.cc",
+    ],
+    hdrs = [
+        "intake.h",
+    ],
+    visibility = ["//visibility:public"],
+    deps = [
+        ":intake_plants",
+        "//aos/common:math",
+        "//aos/common/controls:control_loop",
+        "//aos/common/logging:queue_logging",
+        "//frc971/control_loops:queues",
+        "//frc971/zeroing",
+        "//y2018:constants",
+        "//y2018/control_loops/superstructure:superstructure_queue",
+    ],
+)
diff --git a/y2018/control_loops/superstructure/intake/intake.cc b/y2018/control_loops/superstructure/intake/intake.cc
new file mode 100644
index 0000000..7212fd3
--- /dev/null
+++ b/y2018/control_loops/superstructure/intake/intake.cc
@@ -0,0 +1,172 @@
+#include "y2018/control_loops/superstructure/intake/intake.h"
+
+#include <chrono>
+
+#include "aos/common/commonmath.h"
+#include "aos/common/controls/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
+
+#include "y2018/constants.h"
+#include "y2018/control_loops/superstructure/intake/intake_delayed_plant.h"
+#include "y2018/control_loops/superstructure/intake/intake_plant.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace intake {
+
+namespace chrono = ::std::chrono;
+using ::aos::monotonic_clock;
+
+constexpr double IntakeController::kDt;
+
+IntakeController::IntakeController()
+    : loop_(new StateFeedbackLoop<5, 1, 2, double, StateFeedbackPlant<5, 1, 2>,
+                                  StateFeedbackObserver<5, 1, 2>>(
+          superstructure::intake::MakeDelayedIntakeLoop())),
+      intake_range_(::y2018::constants::Values::kIntakeRange()) {
+  Y_.setZero();
+}
+
+void IntakeController::set_position(double spring_angle,
+                                    double output_position) {
+  // Update position in the model.
+  Y_ << spring_angle, (output_position + offset_);
+}
+
+double IntakeController::voltage() const { return loop_->U(0, 0); }
+
+void IntakeController::Reset() { reset_ = true; }
+
+void IntakeController::UpdateOffset(double offset) {
+  const double doffset = offset - offset_;
+  offset_ = offset;
+
+  loop_->mutable_X_hat(0) += doffset;
+  loop_->mutable_X_hat(2) += doffset;
+}
+
+double IntakeController::goal_angle(const double *unsafe_goal) {
+  if (unsafe_goal == nullptr) {
+    return 0;
+  } else {
+    return ::aos::Clip(*unsafe_goal, intake_range_.lower, intake_range_.upper);
+  }
+}
+
+void IntakeController::Update(bool disabled, const double *unsafe_goal) {
+  if (reset_) {
+    loop_->mutable_X_hat().setZero();
+    loop_->mutable_X_hat(0) = Y_(0) + Y_(1);
+    loop_->mutable_X_hat(2) = Y_(1);
+    reset_ = false;
+  }
+
+  double goal_velocity;
+  loop_->Correct(Y_);
+
+  if (unsafe_goal == nullptr) {
+    disabled = true;
+    goal_velocity = 0.0;
+  } else {
+    goal_velocity = ::aos::Clip(
+        ((goal_angle(unsafe_goal) - loop_->X_hat(0, 0)) * 6.0), -10.0, 10.0);
+  }
+  // Computes the goal.
+  loop_->mutable_R() << 0.0, goal_velocity, 0.0, goal_velocity,
+      (goal_velocity / (kGearRatio * kMotorVelocityConstant));
+
+  loop_->Update(disabled);
+}
+
+void IntakeController::SetStatus(IntakeSideStatus *status,
+                                 const double *unsafe_goal) {
+  status->goal_position = goal_angle(unsafe_goal);
+  status->goal_velocity = loop_->R(1, 0);
+  status->spring_position = loop_->X_hat(0);
+  status->spring_velocity = loop_->X_hat(1);
+  status->motor_position = loop_->X_hat(2);
+  status->motor_velocity = loop_->X_hat(3);
+  status->delayed_voltage = loop_->X_hat(4);
+}
+
+IntakeSide::IntakeSide(
+    const ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
+        &zeroing_constants)
+    : zeroing_estimator_(zeroing_constants) {}
+
+void IntakeSide::Reset() { state_ = State::UNINITIALIZED; }
+
+void IntakeSide::Iterate(const double *unsafe_goal,
+                         const control_loops::IntakeElasticSensors *position,
+                         control_loops::IntakeVoltage *output,
+                         control_loops::IntakeSideStatus *status) {
+  double intake_last_position_ = status->estimator_state.position;
+  zeroing_estimator_.UpdateEstimate(position->motor_position);
+
+  switch (state_) {
+    case State::UNINITIALIZED:
+      // Wait in the uninitialized state until the intake is initialized.
+      LOG(DEBUG, "Uninitialized, waiting for intake\n");
+      zeroing_estimator_.Reset();
+      controller_.Reset();
+      state_ = State::ZEROING;
+      break;
+
+    case State::ZEROING:
+      // Zero by not moving.
+      if (zeroing_estimator_.zeroed()) {
+        LOG(INFO, "Now zeroed\n");
+        controller_.UpdateOffset(zeroing_estimator_.offset());
+        state_ = State::RUNNING;
+      }
+      break;
+
+    case State::RUNNING:
+      if (!(zeroing_estimator_.zeroed())) {
+        LOG(ERROR, "Zeroing estimator is no longer zeroed\n");
+        state_ = State::UNINITIALIZED;
+      }
+      if (zeroing_estimator_.error()) {
+        LOG(ERROR, "Zeroing estimator error\n");
+        state_ = State::UNINITIALIZED;
+      }
+      // ESTOP if we hit the hard limits.
+      if ((status->motor_position) > controller_.intake_range_.upper ||
+          (status->motor_position) < controller_.intake_range_.lower) {
+        LOG(ERROR, "Hit hard limits\n");
+        state_ = State::ESTOP;
+      }
+      break;
+
+    case State::ESTOP:
+      LOG(ERROR, "Estop\n");
+      break;
+  }
+
+  const bool disable = (output == nullptr) || state_ != State::RUNNING;
+  controller_.set_position(position->spring_angle,
+                           position->motor_position.encoder);
+
+  controller_.Update(disable, unsafe_goal);
+
+  if (output) {
+    output->voltage_elastic = controller_.voltage();
+  }
+
+  // Save debug/internal state.
+  status->estimator_state = zeroing_estimator_.GetEstimatorState();
+  controller_.SetStatus(status, unsafe_goal);
+  status->calculated_velocity =
+      (status->estimator_state.position - intake_last_position_) /
+      controller_.kDt;
+  status->zeroed = zeroing_estimator_.zeroed();
+  status->estopped = (state_ == State::ESTOP);
+  status->state = static_cast<int32_t>(state_);
+}
+
+}  // namespace intake
+}  // namespace superstructure
+}  // namespace control_loops
+}  // namespace y2018
diff --git a/y2018/control_loops/superstructure/intake/intake.h b/y2018/control_loops/superstructure/intake/intake.h
new file mode 100644
index 0000000..d46d90e
--- /dev/null
+++ b/y2018/control_loops/superstructure/intake/intake.h
@@ -0,0 +1,107 @@
+#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_INTAKE_INTAKE_H_
+#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_INTAKE_INTAKE_H_
+
+#include "aos/common/commonmath.h"
+#include "aos/common/controls/control_loop.h"
+#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/zeroing/zeroing.h"
+#include "y2018/constants.h"
+#include "y2018/control_loops/superstructure/intake/intake_delayed_plant.h"
+#include "y2018/control_loops/superstructure/intake/intake_plant.h"
+#include "y2018/control_loops/superstructure/superstructure.q.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace intake {
+
+class IntakeController {
+ public:
+  IntakeController();
+
+  // Sets the current encoder position in radians
+  void set_position(double spring_angle, double output_position);
+
+  // Populates the status structure.
+  void SetStatus(control_loops::IntakeSideStatus *status,
+                 const double *unsafe_goal);
+
+  // Returns the control loop calculated voltage.
+  double voltage() const;
+
+  // Executes the control loop for a cycle.
+  void Update(bool disabled, const double *unsafe_goal);
+
+  // Resets the kalman filter and any other internal state.
+  void Reset();
+
+  // Sets the goal angle from unsafe_goal.
+  double goal_angle(const double *unsafe_goal);
+
+  // The control loop.
+  ::std::unique_ptr<
+      StateFeedbackLoop<5, 1, 2, double, StateFeedbackPlant<5, 1, 2>,
+                        StateFeedbackObserver<5, 1, 2>>>
+      loop_;
+
+  constexpr static double kDt =
+      ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+          ::aos::controls::kLoopFrequency)
+          .count();
+
+  // Sets the offset of the controller to be the zeroing estimator offset when
+  // possible otherwise zero.
+  void UpdateOffset(double offset);
+
+  const ::frc971::constants::Range intake_range_;
+
+  // Stores the current zeroing estimator offset.
+  double offset_ = 0.0;
+
+ private:
+  bool reset_ = true;
+
+  // The current sensor measurement.
+  Eigen::Matrix<double, 2, 1> Y_;
+
+  DISALLOW_COPY_AND_ASSIGN(IntakeController);
+};
+
+class IntakeSide {
+ public:
+  IntakeSide(const ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
+                 &zeroing_constants);
+
+  // The operating voltage.
+  static constexpr double kOperatingVoltage() { return 12.0; }
+
+  void Iterate(const double *unsafe_goal,
+               const control_loops::IntakeElasticSensors *position,
+               control_loops::IntakeVoltage *output,
+               control_loops::IntakeSideStatus *status);
+
+  void Reset();
+
+  enum class State : int32_t {
+    UNINITIALIZED,
+    ZEROING,
+    RUNNING,
+    ESTOP,
+  };
+
+  State state() const { return state_; }
+
+ private:
+  IntakeController controller_;
+
+  State state_ = State::UNINITIALIZED;
+
+  ::frc971::zeroing::PotAndAbsEncoderZeroingEstimator zeroing_estimator_;
+};
+
+}  // namespace intake
+}  // namespace superstructure
+}  // namespace control_loops
+}  // namespace y2018
+
+#endif  // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_INTAKE_INTAKE_H_
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
new file mode 100644
index 0000000..157a46e
--- /dev/null
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -0,0 +1,67 @@
+#include "y2018/control_loops/superstructure/superstructure.h"
+
+#include "aos/common/controls/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+#include "frc971/control_loops/control_loops.q.h"
+#include "y2018/constants.h"
+#include "y2018/control_loops/superstructure/intake/intake.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+
+namespace {
+// The maximum voltage the intake roller will be allowed to use.
+constexpr double kMaxIntakeRollerVoltage = 12.0;
+}  // namespace
+
+Superstructure::Superstructure(
+    control_loops::SuperstructureQueue *superstructure_queue)
+    : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(
+          superstructure_queue),
+      intake_left_(constants::GetValues().left_intake.zeroing),
+      intake_right_(constants::GetValues().right_intake.zeroing) {}
+
+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");
+    intake_left_.Reset();
+    intake_right_.Reset();
+  }
+
+  intake_left_.Iterate(unsafe_goal != nullptr
+                           ? &(unsafe_goal->intake.left_intake_angle)
+                           : nullptr,
+                       &(position->intake.left),
+                       output != nullptr ? &(output->intake.left) : nullptr,
+                       &(status->left_intake));
+
+  intake_right_.Iterate(unsafe_goal != nullptr
+                            ? &(unsafe_goal->intake.right_intake_angle)
+                            : nullptr,
+                        &(position->intake.right),
+                        output != nullptr ? &(output->intake.right) : nullptr,
+                        &(status->right_intake));
+
+  status->estopped =
+      status->left_intake.estopped || status->right_intake.estopped;
+
+  status->zeroed = status->left_intake.zeroed && status->right_intake.zeroed;
+
+  if (output && unsafe_goal) {
+    output->intake.left.voltage_rollers = ::std::max(
+        -kMaxIntakeRollerVoltage, ::std::min(unsafe_goal->intake.roller_voltage,
+                                             kMaxIntakeRollerVoltage));
+    output->intake.right.voltage_rollers = ::std::max(
+        -kMaxIntakeRollerVoltage, ::std::min(unsafe_goal->intake.roller_voltage,
+                                             kMaxIntakeRollerVoltage));
+  }
+}
+
+}  // namespace superstructure
+}  // namespace control_loops
+}  // namespace y2018
diff --git a/y2018/control_loops/superstructure/superstructure.h b/y2018/control_loops/superstructure/superstructure.h
new file mode 100644
index 0000000..0cc504d
--- /dev/null
+++ b/y2018/control_loops/superstructure/superstructure.h
@@ -0,0 +1,43 @@
+#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+
+#include <memory>
+
+#include "aos/common/controls/control_loop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "y2018/control_loops/superstructure/intake/intake.h"
+#include "y2018/control_loops/superstructure/superstructure.q.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+
+class Superstructure
+    : public ::aos::controls::ControlLoop<control_loops::SuperstructureQueue> {
+ public:
+  explicit Superstructure(
+      control_loops::SuperstructureQueue *my_superstructure =
+          &control_loops::superstructure_queue);
+
+  const intake::IntakeSide &intake_left() const { return intake_left_; }
+  const intake::IntakeSide &intake_right() const { return intake_right_; }
+
+ protected:
+  virtual void RunIteration(
+      const control_loops::SuperstructureQueue::Goal *unsafe_goal,
+      const control_loops::SuperstructureQueue::Position *position,
+      control_loops::SuperstructureQueue::Output *output,
+      control_loops::SuperstructureQueue::Status *status) override;
+
+ private:
+  intake::IntakeSide intake_left_;
+  intake::IntakeSide intake_right_;
+
+  DISALLOW_COPY_AND_ASSIGN(Superstructure);
+};
+
+}  // namespace superstructure
+}  // namespace control_loops
+}  // namespace y2018
+
+#endif  // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
diff --git a/y2018/control_loops/superstructure/superstructure.q b/y2018/control_loops/superstructure/superstructure.q
index 75900fa..3eaeb7e 100644
--- a/y2018/control_loops/superstructure/superstructure.q
+++ b/y2018/control_loops/superstructure/superstructure.q
@@ -13,10 +13,15 @@
   // If true, we have aborted.
   bool estopped;
 
+  // Estimated position of the spring.
+  float spring_position;
+  // Estimated velocity of the spring in units/second.
+  float spring_velocity;
+
   // Estimated position of the joint.
-  float position;
+  float motor_position;
   // Estimated velocity of the joint in units/second.
-  float velocity;
+  float motor_velocity;
 
   // Goal position of the joint.
   float goal_position;
@@ -26,12 +31,15 @@
   // The calculated velocity with delta x/delta t
   float calculated_velocity;
 
+  // The voltage given last cycle;
+  float delayed_voltage;
+
   // State of the estimator.
   .frc971.AbsoluteEstimatorState estimator_state;
 };
 
 struct IntakeGoal {
-  float roller_voltage;
+  double roller_voltage;
 
   // Goal angle in radians of the intake.
   // Zero radians is where the intake is pointing straight out, with positive
diff --git a/y2018/control_loops/superstructure/superstructure_lib_test.cc b/y2018/control_loops/superstructure/superstructure_lib_test.cc
new file mode 100644
index 0000000..2fa19ca
--- /dev/null
+++ b/y2018/control_loops/superstructure/superstructure_lib_test.cc
@@ -0,0 +1,414 @@
+#include "y2018/control_loops/superstructure/superstructure.h"
+
+#include <unistd.h>
+
+#include <chrono>
+#include <memory>
+
+#include "aos/common/controls/control_loop_test.h"
+#include "aos/common/queue.h"
+#include "frc971/control_loops/position_sensor_sim.h"
+#include "frc971/control_loops/team_number_test_environment.h"
+#include "gtest/gtest.h"
+#include "y2018/constants.h"
+#include "y2018/control_loops/superstructure/intake/intake_plant.h"
+
+using ::frc971::control_loops::PositionSensorSimulator;
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace testing {
+namespace {
+constexpr double kNoiseScalar = 0.01;
+}  // namespace
+
+namespace chrono = ::std::chrono;
+using ::aos::monotonic_clock;
+
+class IntakePlant : public StateFeedbackPlant<5, 1, 2> {
+ public:
+  explicit IntakePlant(StateFeedbackPlant<5, 1, 2> &&other)
+      : StateFeedbackPlant<5, 1, 2>(::std::move(other)) {}
+
+  void CheckU(const ::Eigen::Matrix<double, 1, 1> &U) override {
+    EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
+    EXPECT_GE(U(0, 0), U_min(0, 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 IntakeSideSimulation {
+ public:
+  explicit IntakeSideSimulation(
+      StateFeedbackPlant<5, 1, 2> &&other,
+      const ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
+          &zeroing_constants)
+      : plant_(::std::move(other)),
+        pot_encoder_(M_PI * 2.0 *
+                     constants::Values::kIntakeMotorEncoderRatio()),
+        zeroing_constants_(zeroing_constants) {}
+
+  void InitializePosition(double start_pos) {
+    plant_.mutable_X().setZero();
+    plant_.mutable_X(0) = start_pos;
+    plant_.mutable_X(2) = start_pos;
+
+    pot_encoder_.Initialize(
+        start_pos, kNoiseScalar, 0.0,
+        zeroing_constants_.measured_absolute_position);
+  }
+
+  void GetSensorValues(IntakeElasticSensors *position) {
+    pot_encoder_.GetSensorValues(&position->motor_position);
+    position->spring_angle = plant_.Y(0);
+  }
+
+  double spring_position() const { return plant_.X(0); }
+  double spring_velocity() const { return plant_.X(1); }
+  double motor_position() const { return plant_.X(2); }
+  double motor_velocity() const { return plant_.X(3); }
+
+  void set_voltage_offset(double voltage_offset) {
+    plant_.set_voltage_offset(voltage_offset);
+  }
+
+  void Simulate(const IntakeVoltage &intake_voltage) {
+    const double voltage_check =
+        superstructure::intake::IntakeSide::kOperatingVoltage();
+
+    CHECK_LE(::std::abs(intake_voltage.voltage_elastic), voltage_check);
+
+    ::Eigen::Matrix<double, 1, 1> U;
+    U << intake_voltage.voltage_elastic + plant_.voltage_offset();
+
+    plant_.Update(U);
+
+    const double position_intake = plant_.Y(1);
+
+    pot_encoder_.MoveTo(position_intake);
+    EXPECT_GE(position_intake,
+              constants::Values::kIntakeRange().lower_hard);
+    EXPECT_LE(position_intake,
+              constants::Values::kIntakeRange().upper_hard);
+  }
+
+ private:
+  IntakePlant plant_;
+
+  PositionSensorSimulator pot_encoder_;
+
+  const ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
+      zeroing_constants_;
+};
+
+class SuperstructureSimulation {
+ public:
+  SuperstructureSimulation()
+      : left_intake_(::y2018::control_loops::superstructure::intake::
+                         MakeDelayedIntakePlant(),
+                     constants::GetValues().left_intake.zeroing),
+        right_intake_(::y2018::control_loops::superstructure::intake::
+                          MakeDelayedIntakePlant(),
+                      constants::GetValues().right_intake.zeroing),
+        superstructure_queue_(".y2018.control_loops.superstructure", 0xdeadbeef,
+                              ".y2018.control_loops.superstructure.goal",
+                              ".y2018.control_loops.superstructure.position",
+                              ".y2018.control_loops.superstructure.output",
+                              ".y2018.control_loops.superstructure.status") {
+    // Start the intake out in the middle by default.
+    InitializeIntakePosition((constants::Values::kIntakeRange().lower +
+                              constants::Values::kIntakeRange().upper) /
+                             2.0);
+  }
+
+  void InitializeIntakePosition(double start_pos) {
+    left_intake_.InitializePosition(start_pos);
+    right_intake_.InitializePosition(start_pos);
+  }
+
+  void SendPositionMessage() {
+    ::aos::ScopedMessagePtr<SuperstructureQueue::Position> position =
+        superstructure_queue_.position.MakeMessage();
+
+    left_intake_.GetSensorValues(&position->intake.left);
+    right_intake_.GetSensorValues(&position->intake.right);
+    LOG_STRUCT(INFO, "sim position", *position);
+    position.Send();
+  }
+
+  // Sets the difference between the commanded and applied powers.
+  // This lets us test that the integrators work.
+  void set_left_intake_voltage_offset(double voltage_offset) {
+    left_intake_.set_voltage_offset(voltage_offset);
+  }
+  void set_right_intake_voltage_offset(double voltage_offset) {
+    right_intake_.set_voltage_offset(voltage_offset);
+  }
+
+  const IntakeSideSimulation &left_intake() const { return left_intake_; }
+  const IntakeSideSimulation &right_intake() const { return right_intake_; }
+
+  // Simulates the intake for a single timestep.
+  void Simulate() {
+    EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
+    EXPECT_TRUE(superstructure_queue_.status.FetchLatest());
+
+    left_intake_.Simulate(superstructure_queue_.output->intake.left);
+    right_intake_.Simulate(superstructure_queue_.output->intake.right);
+  }
+
+ private:
+  IntakeSideSimulation left_intake_;
+  IntakeSideSimulation right_intake_;
+
+  SuperstructureQueue superstructure_queue_;
+};
+
+class SuperstructureTest : public ::aos::testing::ControlLoopTest {
+ protected:
+  SuperstructureTest()
+      : superstructure_queue_(".y2018.control_loops.superstructure", 0xdeadbeef,
+                              ".y2018.control_loops.superstructure.goal",
+                              ".y2018.control_loops.superstructure.position",
+                              ".y2018.control_loops.superstructure.output",
+                              ".y2018.control_loops.superstructure.status"),
+        superstructure_(&superstructure_queue_) {
+    set_team_id(::frc971::control_loops::testing::kTeamNumber);
+  }
+
+  void VerifyNearGoal() {
+    superstructure_queue_.goal.FetchLatest();
+    superstructure_queue_.status.FetchLatest();
+
+    ASSERT_TRUE(superstructure_queue_.goal.get() != nullptr);
+    ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
+    // Left side test.
+    EXPECT_NEAR(superstructure_queue_.goal->intake.left_intake_angle,
+                 superstructure_queue_.status->left_intake.spring_position,
+                0.001);
+    EXPECT_NEAR(superstructure_queue_.goal->intake.left_intake_angle,
+                superstructure_plant_.left_intake().spring_position(), 0.001);
+
+    // Right side test.
+    EXPECT_NEAR(superstructure_queue_.goal->intake.right_intake_angle,
+                 superstructure_queue_.status->right_intake.spring_position,
+                0.001);
+    EXPECT_NEAR(superstructure_queue_.goal->intake.right_intake_angle,
+                superstructure_plant_.right_intake().spring_position(), 0.001);
+  }
+
+  // Runs one iteration of the whole simulation.
+  void RunIteration(bool enabled = true) {
+    SendMessages(enabled);
+
+    superstructure_plant_.SendPositionMessage();
+    superstructure_.Iterate();
+    superstructure_plant_.Simulate();
+
+    TickTime(::std::chrono::microseconds(5050));
+  }
+
+  // Runs iterations until the specified amount of simulated time has elapsed.
+  void RunForTime(const monotonic_clock::duration run_for,
+                  bool enabled = true) {
+    const auto end_time = monotonic_clock::now() + run_for;
+    while (monotonic_clock::now() < end_time) {
+      RunIteration(enabled);
+    }
+  }
+
+  // Create a new instance of the test queue so that it invalidates the queue
+  // that it points to.  Otherwise, we will have a pointer to shared memory
+  // that is no longer valid.
+  SuperstructureQueue superstructure_queue_;
+
+  // Create a control loop and simulation.
+  Superstructure superstructure_;
+  SuperstructureSimulation superstructure_plant_;
+};
+
+// Tests that the loop zeroes when run for a while without a goal.
+TEST_F(SuperstructureTest, ZeroNoGoalAndDoesNothing) {
+  RunForTime(chrono::seconds(2));
+  superstructure_queue_.output.FetchLatest();
+
+  EXPECT_EQ(intake::IntakeSide::State::RUNNING,
+            superstructure_.intake_left().state());
+  EXPECT_EQ(intake::IntakeSide::State::RUNNING,
+            superstructure_.intake_right().state());
+  EXPECT_EQ(superstructure_queue_.output->intake.left.voltage_elastic, 0.0);
+  EXPECT_EQ(superstructure_queue_.output->intake.right.voltage_elastic, 0.0);
+}
+
+// Tests that the intake loop can reach a goal.
+TEST_F(SuperstructureTest, ReachesGoal) {
+  // Set a reasonable goal.
+  {
+    auto goal = superstructure_queue_.goal.MakeMessage();
+
+    goal->intake.left_intake_angle = 0.1;
+    goal->intake.right_intake_angle = 0.2;
+
+    ASSERT_TRUE(goal.Send());
+  }
+
+  // Give it a lot of time to get there.
+  RunForTime(chrono::seconds(8));
+
+  VerifyNearGoal();
+}
+
+// Tests that the intake loop can reach a goal after starting at a non-zero
+// position.
+TEST_F(SuperstructureTest, OffsetStartReachesGoal) {
+  superstructure_plant_.InitializeIntakePosition(0.5);
+
+  // Set a reasonable goal.
+  {
+    auto goal = superstructure_queue_.goal.MakeMessage();
+
+    goal->intake.left_intake_angle = 0.1;
+    goal->intake.right_intake_angle = 0.2;
+
+    ASSERT_TRUE(goal.Send());
+  }
+
+  // Give it a lot of time to get there.
+  RunForTime(chrono::seconds(8));
+
+  VerifyNearGoal();
+}
+
+// Tests that the intake loops doesn't try and go beyond the
+// physical range of the mechanisms.
+TEST_F(SuperstructureTest, RespectsRange) {
+  // Set some ridiculous goals to test upper limits.
+  {
+    auto goal = superstructure_queue_.goal.MakeMessage();
+
+    goal->intake.left_intake_angle = 5.0 * M_PI;
+    goal->intake.right_intake_angle = 5.0 * M_PI;
+
+    ASSERT_TRUE(goal.Send());
+  }
+  RunForTime(chrono::seconds(10));
+
+  // Check that we are near our soft limit.
+  superstructure_queue_.status.FetchLatest();
+
+  EXPECT_NEAR(constants::Values::kIntakeRange().upper,
+              superstructure_queue_.status->left_intake.spring_position,
+              0.001);
+
+  EXPECT_NEAR(constants::Values::kIntakeRange().upper,
+              superstructure_queue_.status->right_intake.spring_position,
+              0.001);
+
+  // Set some ridiculous goals to test lower limits.
+  {
+    auto goal = superstructure_queue_.goal.MakeMessage();
+
+    goal->intake.left_intake_angle = -5.0 * M_PI;
+    goal->intake.right_intake_angle = -5.0 * M_PI;
+
+    ASSERT_TRUE(goal.Send());
+  }
+
+  RunForTime(chrono::seconds(10));
+
+  // Check that we are near our soft limit.
+  superstructure_queue_.status.FetchLatest();
+
+  EXPECT_NEAR(constants::Values::kIntakeRange().lower,
+              superstructure_queue_.status->left_intake.spring_position,
+              0.001);
+
+  EXPECT_NEAR(constants::Values::kIntakeRange().lower,
+              superstructure_queue_.status->right_intake.spring_position,
+              0.001);}
+
+TEST_F(SuperstructureTest, DISABLED_LowerHardstopStartup) {
+  superstructure_plant_.InitializeIntakePosition(
+      constants::Values::kIntakeRange().lower_hard);
+  {
+    auto goal = superstructure_queue_.goal.MakeMessage();
+    goal->intake.left_intake_angle = constants::Values::kIntakeRange().lower;
+    goal->intake.right_intake_angle = constants::Values::kIntakeRange().lower;
+    ASSERT_TRUE(goal.Send());
+  }
+  RunForTime(chrono::seconds(10));
+  {
+    auto goal = superstructure_queue_.goal.MakeMessage();
+    goal->intake.left_intake_angle = 1.0;
+    goal->intake.right_intake_angle = 1.0;
+    ASSERT_TRUE(goal.Send());
+  }
+  RunForTime(chrono::seconds(10));
+  VerifyNearGoal();
+}
+
+// Tests that starting at the upper hardstops doesn't cause an abort.
+TEST_F(SuperstructureTest, DISABLED_UpperHardstopStartup) {
+  superstructure_plant_.InitializeIntakePosition(
+      constants::Values::kIntakeRange().upper_hard);
+  {
+    auto goal = superstructure_queue_.goal.MakeMessage();
+    goal->intake.left_intake_angle = constants::Values::kIntakeRange().upper;
+    goal->intake.right_intake_angle = constants::Values::kIntakeRange().upper;
+    ASSERT_TRUE(goal.Send());
+  }
+  RunForTime(chrono::seconds(10));
+
+  VerifyNearGoal();
+}
+
+// Tests that resetting WPILib results in a rezero.
+TEST_F(SuperstructureTest, ResetTest) {
+  superstructure_plant_.InitializeIntakePosition(
+      constants::Values::kIntakeRange().upper);
+  {
+    auto goal = superstructure_queue_.goal.MakeMessage();
+    goal->intake.left_intake_angle =
+        constants::Values::kIntakeRange().upper - 0.1;
+    goal->intake.right_intake_angle =
+        constants::Values::kIntakeRange().upper - 0.1;
+    ASSERT_TRUE(goal.Send());
+  }
+  RunForTime(chrono::seconds(10));
+
+  EXPECT_EQ(intake::IntakeSide::State::RUNNING,
+            superstructure_.intake_left().state());
+  EXPECT_EQ(intake::IntakeSide::State::RUNNING,
+            superstructure_.intake_right().state());
+
+  VerifyNearGoal();
+  SimulateSensorReset();
+  RunForTime(chrono::milliseconds(100));
+
+  EXPECT_EQ(intake::IntakeSide::State::ZEROING,
+            superstructure_.intake_left().state());
+  EXPECT_EQ(intake::IntakeSide::State::ZEROING,
+            superstructure_.intake_right().state());
+
+  RunForTime(chrono::seconds(10));
+
+  EXPECT_EQ(intake::IntakeSide::State::RUNNING,
+            superstructure_.intake_left().state());
+  EXPECT_EQ(intake::IntakeSide::State::RUNNING,
+            superstructure_.intake_right().state());
+
+  VerifyNearGoal();
+}
+
+}  // namespace testing
+}  // namespace superstructure
+}  // namespace control_loops
+}  // namespace y2018
diff --git a/y2018/control_loops/superstructure/superstructure_main.cc b/y2018/control_loops/superstructure/superstructure_main.cc
new file mode 100644
index 0000000..04d245e
--- /dev/null
+++ b/y2018/control_loops/superstructure/superstructure_main.cc
@@ -0,0 +1,11 @@
+#include "y2018/control_loops/superstructure/superstructure.h"
+
+#include "aos/linux_code/init.h"
+
+int main() {
+  ::aos::Init();
+  ::y2018::control_loops::superstructure::Superstructure superstructure;
+  superstructure.Run();
+  ::aos::Cleanup();
+  return 0;
+}
diff --git a/y2018/wpilib_interface.cc b/y2018/wpilib_interface.cc
index 0aa4203..292fef9 100644
--- a/y2018/wpilib_interface.cc
+++ b/y2018/wpilib_interface.cc
@@ -430,24 +430,24 @@
       CopyPosition(proximal_encoder_, &superstructure_message->arm.proximal,
                    Values::kProximalEncoderCountsPerRevolution(),
                    Values::kProximalEncoderRatio(), proximal_pot_translate,
-                   false, values.proximal.pot_offset);
+                   false, values.proximal.potentiometer_offset);
 
       CopyPosition(distal_encoder_, &superstructure_message->arm.distal,
                    Values::kDistalEncoderCountsPerRevolution(),
                    Values::kDistalEncoderRatio(), distal_pot_translate, false,
-                   values.distal.pot_offset);
+                   values.distal.potentiometer_offset);
 
       CopyPosition(left_intake_encoder_,
                    &superstructure_message->intake.left.motor_position,
                    Values::kIntakeMotorEncoderCountsPerRevolution(),
                    Values::kIntakeMotorEncoderRatio(), intake_pot_translate,
-                   false, values.intake.left_pot_offset);
+                   false, values.left_intake.potentiometer_offset);
 
       CopyPosition(right_intake_encoder_,
                    &superstructure_message->intake.right.motor_position,
                    Values::kIntakeMotorEncoderCountsPerRevolution(),
                    Values::kIntakeMotorEncoderRatio(), intake_pot_translate,
-                   false, values.intake.right_pot_offset);
+                   false, values.right_intake.potentiometer_offset);
 
       superstructure_message->intake.left.spring_angle =
           intake_spring_translate(left_intake_spring_angle_->GetVoltage());