diff --git a/bot3/control_loops/control_loops.gyp b/bot3/control_loops/control_loops.gyp
new file mode 100644
index 0000000..b63816e
--- /dev/null
+++ b/bot3/control_loops/control_loops.gyp
@@ -0,0 +1,15 @@
+{
+  'targets': [
+    {
+      'target_name': 'position_sensor_sim',
+      'type': 'static_library',
+      'sources': ['position_sensor_sim.cc'],
+      'variables': {
+        'header_path': 'bot3/control_loops',
+      },
+      'dependencies': [
+        '<(DEPTH)/bot3/control_loops/elevator/elevator.gyp:elevator_queue',
+      ],
+    },
+  ],
+}
diff --git a/bot3/control_loops/elevator/elevator.cc b/bot3/control_loops/elevator/elevator.cc
new file mode 100644
index 0000000..49433a7
--- /dev/null
+++ b/bot3/control_loops/elevator/elevator.cc
@@ -0,0 +1,280 @@
+#include "bot3/control_loops/elevator/elevator.h"
+
+#include <cmath>
+
+#include "aos/common/controls/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+
+#include "bot3/control_loops/elevator/integral_elevator_motor_plant.h"
+
+namespace bot3 {
+namespace control_loops {
+
+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_);
+}
+
+double SimpleCappedStateFeedbackLoop::UnsaturateOutputGoalChange() {
+  // Compute K matrix to compensate for position errors.
+  double Kp = K(0, 0);
+
+  // Compute how much we need to change R in order to achieve the change in U
+  // that was observed.
+  return -(1.0 / Kp) * (U_uncapped() - U())(0, 0);
+}
+
+Elevator::Elevator(control_loops::ElevatorQueue *elevator)
+    : aos::controls::ControlLoop<control_loops::ElevatorQueue>(elevator),
+      loop_(new SimpleCappedStateFeedbackLoop(MakeIntegralElevatorLoop())),
+      profile_(::aos::controls::kLoopFrequency) {}
+
+bool Elevator::CheckZeroed() {
+  return state_ == RUNNING;
+}
+
+void Elevator::Correct() {
+  Eigen::Matrix<double, 1, 1> Y;
+  Y << current_position();
+  loop_->Correct(Y);
+}
+
+double Elevator::current_position() {
+  return current_position_.encoder + offset_;
+}
+
+double Elevator::GetZeroingVelocity() {
+  return zeroing_velocity_;
+}
+
+// If the hall_effect is true that means we need to move up until is false.
+// Then we should move down.
+double Elevator::FindZeroingVelocity() {
+  if (glitch_filter_.filtered_value()) {
+    zeroing_velocity_ = kZeroingSlowVelocity;
+  } else {
+    zeroing_velocity_ = -kZeroingVelocity;
+  }
+
+  return zeroing_velocity_;
+}
+
+double Elevator::UseUnlessZero(double target_value, double default_value) {
+  if (target_value != 0.0) {
+    return target_value;
+  } else {
+    return default_value;
+  }
+}
+
+void Elevator::SetOffset(double offset) {
+  LOG(INFO, "Changing Elevator offset from %f to %f\n", offset_, offset);
+  double doffset = offset - offset_;
+
+  loop_->mutable_X_hat(0, 0) += doffset;
+
+  // Modify the zeroing goal.
+  goal_ += doffset;
+
+  // Update the cached offset values to the actual values.
+  offset_ = offset;
+}
+
+void Elevator::RunIteration(
+    const control_loops::ElevatorQueue::Goal *unsafe_goal,
+    const control_loops::ElevatorQueue::Position *position,
+    control_loops::ElevatorQueue::Output *output,
+    control_loops::ElevatorQueue::Status *status) {
+  if (WasReset()) {
+    LOG(ERROR, "WPILib reset, restarting\n");
+    state_ = UNINITIALIZED;
+  }
+  glitch_filter_.Update(position->bottom_hall_effect, position->encoder);
+
+  // Bool to track if we should turn the motors on or not.
+  bool disable = output == nullptr;
+
+  // Save the current position so it can be used easily in the class.
+  current_position_ = *position;
+
+  if (state_ != UNINITIALIZED) {
+    Correct();
+  }
+
+  switch (state_) {
+    case UNINITIALIZED:
+      LOG(DEBUG, "Uninitialized\n");
+      // Startup.  Assume that we are at the origin everywhere.
+      offset_ = -position->encoder;
+      loop_->mutable_X_hat().setZero();
+      LOG(INFO, "Initializing elevator offset to %f\n", offset_);
+      Correct();
+      state_ = ZEROING;
+      disable = true;
+      glitch_filter_.Reset(position->bottom_hall_effect);
+      break;
+
+    case ZEROING:
+      LOG(DEBUG, "Zeroing elevator\n");
+
+      if (glitch_filter_.negedge()) {
+        state_ = RUNNING;
+        SetOffset(-glitch_filter_.negedge_value() + kHallEffectPosition);
+      }
+
+      // We need to check FindZeroingVelocity() every time
+      // in order for zeroing to work.
+      {
+        double zeroing_velocity_temp = FindZeroingVelocity();
+        if (state_ != RUNNING && !disable) {
+          // Move the elevator either up or down based on where the zeroing hall
+          // effect is located.
+
+          goal_velocity_ = zeroing_velocity_temp; 
+          goal_ += goal_velocity_ * ::aos::controls::kLoopFrequency.ToSeconds();
+        }
+      }
+
+      // Bypass motion profiles while we are zeroing.
+      // This is also an important step right after the elevator is zeroed and
+      // we reach into the elevator's state matrix and change it based on the
+      // newly-obtained offset.
+      {
+        Eigen::Matrix<double, 2, 1> current;
+        current.setZero();
+        current << goal_, goal_velocity_;
+        profile_.MoveCurrentState(current);
+      }
+      break;
+
+    case RUNNING:
+      if (unsafe_goal) {
+        profile_.set_maximum_velocity(
+            UseUnlessZero(unsafe_goal->max_velocity, 0.50));
+        profile_.set_maximum_acceleration(
+            UseUnlessZero(unsafe_goal->max_acceleration, 2.0));
+
+        // Use the profiles to limit the elevator's movements.
+        const double unfiltered_goal = ::std::max(
+            ::std::min(unsafe_goal->height, kElevUpperLimit), kElevLowerLimit);
+        ::Eigen::Matrix<double, 2, 1> goal_state =
+            profile_.Update(unfiltered_goal, unsafe_goal->velocity);
+        goal_ = goal_state(0, 0);
+        goal_velocity_ = goal_state(1, 0);
+      }
+
+      if (state_ != RUNNING && state_ != ESTOP) {
+        state_ = UNINITIALIZED;
+      }
+      break;
+
+    case ESTOP:
+      LOG(ERROR, "Estop\n");
+      disable = true;
+      break;
+  }
+
+  // Limit the goals so we can't exceed the hardware limits if we are RUNNING.
+  if (state_ == RUNNING) {
+    // Limit the elevator goal to min/max allowable heights.
+    if (goal_ >= kElevUpperLimit) {
+      LOG(WARNING, "Elevator goal above limit, %f > %f\n", goal_,
+          kElevUpperLimit);
+      goal_ = kElevUpperLimit;
+    }
+
+    if (goal_ <= kElevLowerLimit) {
+      LOG(WARNING, "Elevator goal below limit, %f < %f\n", goal_,
+          kElevLowerLimit);
+      goal_ = kElevLowerLimit;
+    }
+  }
+
+  // Check the hard limits.
+  if (state_ == RUNNING) {
+    if (current_position() >= kElevUpperHardLimit) {
+      LOG(ERROR, "Elevator at %f out of bounds [%f, %f], ESTOPing\n",
+          current_position(), kElevLowerHardLimit, kElevUpperHardLimit);
+      if (output) {
+        state_ = ESTOP;
+      }
+    }
+
+    if (current_position() <= kElevLowerHardLimit) {
+      LOG(ERROR, "Elevator at %f out of bounds [%f, %f], ESTOPing\n",
+          current_position(), kElevLowerHardLimit, kElevUpperHardLimit);
+      if (output) {
+        state_ = ESTOP;
+      }
+    }
+  }
+
+  // Set the goals.
+  loop_->mutable_R() << goal_, goal_velocity_, 0.0;
+
+  const double max_voltage = state_ == RUNNING ? 12.0 : kZeroingVoltage;
+  loop_->set_max_voltage(max_voltage);
+
+  if (state_ == ESTOP) {
+    disable = true;
+  }
+  loop_->Update(disable);
+
+  if (state_ == ZEROING || state_ == RUNNING) {
+    if (loop_->U() != loop_->U_uncapped()) {
+      double deltaR = loop_->UnsaturateOutputGoalChange();
+
+      // Move the elevator goal by the amount observed.
+      LOG(WARNING, "Moving elevator goal by %f to handle saturation\n", deltaR);
+      goal_ += deltaR;
+
+      Eigen::Matrix<double, 2, 1> current;
+      current.setZero();
+      current << goal_, goal_velocity_;
+      profile_.MoveCurrentState(current);
+    }
+  }
+
+  if (output) {
+    output->elevator = loop_->U(0, 0);
+  }
+
+  status->zeroed = state_ == RUNNING;
+
+  status->height = loop_->X_hat(0, 0);
+  status->velocity = loop_->X_hat(1, 0);
+
+  status->goal_height = goal_;
+  status->goal_velocity = goal_velocity_;
+
+  status->estopped = (state_ == ESTOP);
+  status->state = state_;
+}
+
+void GlitchFilter::Update(bool hall_effect, double encoder) {
+  posedge_ = false;
+  negedge_ = false;
+  if (hall_effect != accepted_value_) {
+    if (count_ == 0) {
+      first_encoder_ = encoder;
+    }
+    ++count_;
+  } else {
+    last_encoder_ = encoder;
+    count_ = 0;
+  }
+  if (count_ >= 2) {
+    if (hall_effect) {
+      posedge_ = true;
+      posedge_value_ = (first_encoder_ + last_encoder_) / 2.0;
+    } else {
+      negedge_ = true;
+      negedge_value_ = (first_encoder_ + last_encoder_) / 2.0;
+    }
+    accepted_value_ = hall_effect;
+    count_ = 0;
+  }
+}
+
+}  // namespace control_loops
+}  // namespace bot3
diff --git a/bot3/control_loops/elevator/elevator.gyp b/bot3/control_loops/elevator/elevator.gyp
new file mode 100644
index 0000000..c184429
--- /dev/null
+++ b/bot3/control_loops/elevator/elevator.gyp
@@ -0,0 +1,71 @@
+{
+  'targets': [
+    {
+      'target_name': 'elevator_queue',
+      'type': 'static_library',
+      'sources': ['elevator.q'],
+      'variables': {
+        'header_path': 'bot3/control_loops/elevator',
+      },
+      'dependencies': [
+        '<(AOS)/common/controls/controls.gyp:control_loop_queues',
+        '<(DEPTH)/frc971/control_loops/control_loops.gyp:queues',
+      ],
+      'export_dependent_settings': [
+        '<(AOS)/common/controls/controls.gyp:control_loop_queues',
+        '<(DEPTH)/frc971/control_loops/control_loops.gyp:queues',
+      ],
+      'includes': ['../../../aos/build/queues.gypi'],
+    },
+    {
+      'target_name': 'elevator_lib',
+      'type': 'static_library',
+      'sources': [
+        'elevator.cc',
+        'elevator_motor_plant.cc',
+        'integral_elevator_motor_plant.cc',
+      ],
+      'dependencies': [
+        'elevator_queue',
+        '<(AOS)/build/aos.gyp:logging',
+        '<(AOS)/common/controls/controls.gyp:control_loop_queues',
+        '<(AOS)/common/controls/controls.gyp:control_loop',
+        '<(AOS)/common/util/util.gyp:trapezoid_profile',
+        '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+        '<(DEPTH)/frc971/control_loops/voltage_cap/voltage_cap.gyp:voltage_cap',
+      ],
+      'export_dependent_settings': [
+        'elevator_queue',
+        '<(AOS)/common/controls/controls.gyp:control_loop',
+        '<(AOS)/common/util/util.gyp:trapezoid_profile',
+        '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+      ],
+    },
+    {
+      'target_name': 'elevator_lib_test',
+      'type': 'executable',
+      'sources': [
+        'elevator_lib_test.cc',
+      ],
+      'dependencies': [
+        '<(EXTERNALS):gtest',
+        'elevator_lib',
+        '<(DEPTH)/bot3/control_loops/control_loops.gyp:position_sensor_sim',
+        '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+        '<(AOS)/common/controls/controls.gyp:control_loop_test',
+        '<(AOS)/common/common.gyp:time',
+      ],
+    },
+    {
+      'target_name': 'elevator',
+      'type': 'executable',
+      'sources': [
+        'elevator_main.cc',
+      ],
+      'dependencies': [
+        '<(AOS)/linux_code/linux_code.gyp:init',
+        'elevator_lib',
+      ],
+    },
+  ],
+}
diff --git a/bot3/control_loops/elevator/elevator.h b/bot3/control_loops/elevator/elevator.h
new file mode 100644
index 0000000..3acd1c8
--- /dev/null
+++ b/bot3/control_loops/elevator/elevator.h
@@ -0,0 +1,193 @@
+#ifndef BOT3_CONTROL_LOOPS_ELEVATOR_H_
+#define BOT3_CONTROL_LOOPS_ELEVATOR_H_
+
+#include <memory>
+
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/util/trapezoid_profile.h"
+
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "bot3/control_loops/elevator/elevator.q.h"
+
+namespace bot3 {
+namespace control_loops {
+namespace testing {
+class ElevatorTest_DisabledGoalTest_Test;
+class ElevatorTest_ElevatorGoalPositiveWindupTest_Test;
+class ElevatorTest_ElevatorGoalNegativeWindupTest_Test;
+class ElevatorTest_PositiveRunawayProfileTest_Test;
+class ElevatorTest_NegativeRunawayProfileTest_Test;
+}  // namespace testing
+
+// Constants
+constexpr double kZeroingVoltage = 4.0;
+// TODO(austin): Slow down the zeroing velocity.
+constexpr double kZeroingVelocity = 0.05;
+constexpr double kZeroingSlowVelocity = 0.01;
+
+// Game pieces
+constexpr double kToteHeight = 0.3;
+
+// TODO(comran): Get actual constants for the ones below.
+// Gearing
+
+constexpr double kElevEncoderRatio = 14.0 / 84.0;
+const int kElevGearboxOutputPulleyTeeth = 32;      // 32 teeth
+constexpr double kElevGearboxOutputPitch = 0.005;  // 5 mm/tooth
+
+constexpr double kElevGearboxOutputRadianDistance =
+    kElevGearboxOutputPulleyTeeth * kElevGearboxOutputPitch / (2.0 * M_PI);
+
+// Limits
+constexpr double kElevLowerHardLimit = -0.005;
+constexpr double kElevUpperHardLimit = 0.720000;
+constexpr double kElevLowerLimit = 0.010000;
+constexpr double kElevUpperLimit = 0.680000;
+
+// Zeroing
+namespace {
+// TODO(Adam): Find the actual value of the hall effect position.
+constexpr double kHallEffectPosition = 0.01;
+}  // namespace
+// End constants
+
+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;
+
+  // Returns the amount to change the position goal in order to no longer
+  // saturate the controller.
+  double UnsaturateOutputGoalChange();
+
+ private:
+  double max_voltage_;
+};
+
+// The GlitchFilter filters out single cycle changes in order to filter out
+// sensor noise.  It also captures the sensor value at the time that the
+// original transition happens (not the de-bounced transition) to remove delay
+// caused by this filter.
+class GlitchFilter {
+ public:
+  void Reset(bool hall_effect) {
+    accepted_value_ = hall_effect;
+    posedge_ = false;
+    negedge_ = false;
+    count_ = 0;
+  }
+
+  // Updates the filter state with new observations.
+  void Update(bool hall_effect, double encoder);
+
+  // Returns a debounced hall effect value.
+  bool filtered_value() const { return accepted_value_; }
+
+  // Returns true if last cycle had a posedge.
+  bool posedge() const { return posedge_; }
+  // Returns the encoder value across the last posedge.
+  double posedge_value() const { return posedge_value_; }
+
+  // Returns true if last cycle had a negedge.
+  bool negedge() const { return negedge_; }
+  // Returns the encoder value across the last negedge.
+  double negedge_value() const { return negedge_value_; }
+
+ private:
+  int count_ = 0;
+  bool accepted_value_ = false;
+  bool posedge_ = false;
+  bool negedge_ = false;
+  double posedge_value_ = 0;
+  double negedge_value_ = 0;
+
+  double first_encoder_ = 0;
+  double last_encoder_ = 0;
+};
+
+class Elevator
+    : public aos::controls::ControlLoop<control_loops::ElevatorQueue> {
+ public:
+  explicit Elevator(control_loops::ElevatorQueue *elevator_queue =
+                        &control_loops::elevator_queue);
+
+  bool CheckZeroed();
+
+  // Getter for the current elevator positions with zeroed offset.
+  double current_position();
+
+  double GetZeroingVelocity();
+
+  enum State {
+    // Waiting to receive data before doing anything.
+    UNINITIALIZED = 0,
+    // Moving the elevator to find an index pulse.
+    ZEROING = 2,
+    // All good!
+    RUNNING = 3,
+    // Internal error caused the elevator to abort.
+    ESTOP = 4,
+  };
+
+  State state() const { return state_; }
+
+ protected:
+  void RunIteration(const control_loops::ElevatorQueue::Goal *goal,
+                    const control_loops::ElevatorQueue::Position *position,
+                    control_loops::ElevatorQueue::Output *output,
+                    control_loops::ElevatorQueue::Status *status) override;
+
+ private:
+  friend class testing::ElevatorTest_DisabledGoalTest_Test;
+  friend class testing::ElevatorTest_ElevatorGoalPositiveWindupTest_Test;
+  friend class testing::ElevatorTest_ElevatorGoalNegativeWindupTest_Test;
+  friend class testing::ElevatorTest_PositiveRunawayProfileTest_Test;
+  friend class testing::ElevatorTest_NegativeRunawayProfileTest_Test;
+
+  void SetOffset(double offset);
+
+  // Returns the current elevator zeroing velocity.
+  double FindZeroingVelocity();
+
+  // Corrects the Observer with the current position.
+  void Correct();
+
+  double UseUnlessZero(double target_value, double default_value);
+
+  // The state feedback control loop or loops to talk to.
+  ::std::unique_ptr<SimpleCappedStateFeedbackLoop> loop_;
+
+  // Offsets from the encoder position to the absolute position.  Add these to
+  // the encoder position to get the absolute position.
+  double offset_ = 0.0;
+
+  bool zeroed_ = false;
+  State state_ = State::UNINITIALIZED;
+
+  // Variables for detecting the zeroing hall effect edge.
+  bool zeroing_hall_effect_edge_detector_initialized_ = false;
+  bool zeroing_hall_effect_previous_reading_ = false;
+
+  // Current velocity to move at while zeroing.
+  double zeroing_velocity_ = 0.0;
+
+  // The goals for the elevator.
+  double goal_ = 0.0;
+  double goal_velocity_ = 0.0;
+
+  control_loops::ElevatorQueue::Position current_position_;
+
+  aos::util::TrapezoidProfile profile_;
+  GlitchFilter glitch_filter_;
+};
+
+}  // namespace control_loops
+}  // namespace bot3
+
+#endif  // BOT3_CONTROL_LOOPS_ELEVATOR_H_
diff --git a/bot3/control_loops/elevator/elevator.q b/bot3/control_loops/elevator/elevator.q
new file mode 100644
index 0000000..663bb59
--- /dev/null
+++ b/bot3/control_loops/elevator/elevator.q
@@ -0,0 +1,63 @@
+package bot3.control_loops;
+
+import "aos/common/controls/control_loops.q";
+import "frc971/control_loops/control_loops.q";
+
+queue_group ElevatorQueue {
+  implements aos.control_loops.ControlLoop;
+
+  // Elevator heights are the vertical distance (in meters) from a defined zero.
+
+  message Goal {
+    // Height of the elevator.
+    double height;
+
+    // Linear velocity of the elevator.
+    float velocity;
+
+    // Maximum elevator profile velocity or 0 for the default.
+    float max_velocity;
+
+    // Maximum elevator profile acceleration or 0 for the default.
+    float max_acceleration;
+  };
+
+  message Position {
+    // bottom hall effect sensor.
+    bool bottom_hall_effect;
+    // encoders used for zeroing.
+    double encoder;
+  };
+
+  message Status {
+    // Is the elevator zeroed?
+    bool zeroed;
+
+    // Estimated height of the elevator.
+    double height;
+
+    // Estimated velocity of the elevator.
+    float velocity;
+
+    // Goal height and velocity of the elevator.
+    double goal_height;
+    float goal_velocity;
+
+    // If true, we have aborted.
+    bool estopped;
+
+    // The internal state of the state machine.
+    int32_t state;
+  };
+
+  message Output {
+    double elevator;
+  };
+
+  queue Goal goal;
+  queue Position position;
+  queue Output output;
+  queue Status status;
+};
+
+queue_group ElevatorQueue elevator_queue;
diff --git a/bot3/control_loops/elevator/elevator_lib_test.cc b/bot3/control_loops/elevator/elevator_lib_test.cc
new file mode 100644
index 0000000..d174567
--- /dev/null
+++ b/bot3/control_loops/elevator/elevator_lib_test.cc
@@ -0,0 +1,545 @@
+#include "bot3/control_loops/elevator/elevator.h"
+
+#include <math.h>
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/time.h"
+#include "aos/common/commonmath.h"
+#include "aos/common/controls/control_loop_test.h"
+#include "bot3/control_loops/position_sensor_sim.h"
+#include "bot3/control_loops/elevator/elevator_motor_plant.h"
+#include "bot3/control_loops/elevator/elevator.q.h"
+
+using ::aos::time::Time;
+using ::bot3::control_loops::PositionSensorSimulator;
+
+namespace bot3 {
+namespace control_loops {
+namespace testing {
+
+// TODO(comran+adam): Check these over with Austin, Ben, Brian, and others to
+// make sure we didn't forget anything -- especially the zeroing tests!!!
+
+class ElevatorSimulator {
+ public:
+  ElevatorSimulator()
+      : plant_(new StateFeedbackPlant<2, 1, 1>(MakeElevatorPlant())),
+        position_sim_(),
+        queue_(".bot3.control_loops.elevator_queue", 0xca8daa3b,
+               ".bot3.control_loops.elevator_queue.goal",
+               ".bot3.control_loops.elevator_queue.position",
+               ".bot3.control_loops.elevator_queue.output",
+               ".bot3.control_loops.elevator_queue.status") {
+    // Initialize the elevator.
+    InitializePosition(kElevLowerLimit);
+  }
+
+  void InitializePosition(double start_pos) {
+    InitializePosition(start_pos, kHallEffectPosition);
+  }
+
+  void InitializePosition(double start_pos, double hall_effect_position) {
+    plant_->mutable_X(0, 0) = start_pos;
+    plant_->mutable_X(1, 0) = 0.0;
+    position_sim_.Initialize(start_pos, hall_effect_position);
+  }
+
+  void SendPositionMessage() {
+    ::aos::ScopedMessagePtr<control_loops::ElevatorQueue::Position> position =
+        queue_.position.MakeMessage();
+    position_sim_.GetSensorValues(position.get());
+    position.Send();
+  }
+
+  void set_acceleration_offset(double acceleration_offset) {
+    acceleration_offset_ = acceleration_offset;
+  }
+
+  double height() const { return plant_->Y(0, 0); }
+
+  void Simulate() {
+    EXPECT_TRUE(queue_.output.FetchLatest());
+
+    plant_->mutable_U() << queue_.output->elevator;
+
+    plant_->Update();
+    plant_->mutable_X()(1, 0) += acceleration_offset_ * 0.005;
+
+    const double height = plant_->Y(0, 0);
+
+    position_sim_.MoveTo(height);
+
+    EXPECT_GE(height, kElevLowerHardLimit);
+    EXPECT_LE(height, kElevUpperHardLimit);
+  }
+
+  void MoveTo(double position) { position_sim_.MoveTo(position); }
+
+ private:
+  ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> plant_;
+
+  PositionSensorSimulator position_sim_;
+
+  ElevatorQueue queue_;
+  double acceleration_offset_ = 0.0;
+};
+
+class ElevatorTest : public ::aos::testing::ControlLoopTest {
+ protected:
+  ElevatorTest()
+      : queue_(".bot3.control_loops.elevator_queue", 0xca8daa3b,
+               ".bot3.control_loops.elevator_queue.goal",
+               ".bot3.control_loops.elevator_queue.position",
+               ".bot3.control_loops.elevator_queue.output",
+               ".bot3.control_loops.elevator_queue.status"),
+        elevator_(&queue_),
+        plant_() {
+  }
+
+  // Checks to see if fetching position/status from queues does not return null
+  // pointers.
+  void VerifyNearGoal() {
+    queue_.goal.FetchLatest();
+    queue_.status.FetchLatest();
+    EXPECT_TRUE(queue_.goal.get() != nullptr);
+    EXPECT_TRUE(queue_.status.get() != nullptr);
+    EXPECT_NEAR(queue_.goal->height, queue_.status->height, 0.001);
+    EXPECT_NEAR(queue_.goal->height, plant_.height(), 0.001);
+  }
+
+  // Runs one iteration of the whole simulation.
+  void RunIteration(bool enabled = true) {
+    SendMessages(enabled);
+
+    plant_.SendPositionMessage();
+    elevator_.Iterate();
+    plant_.Simulate();
+
+    TickTime();
+  }
+
+  // Runs iterations until the specified amount of simulated time has elapsed.
+  void RunForTime(const Time &run_for, bool enabled = true) {
+    const auto start_time = Time::Now();
+    while (Time::Now() < start_time + run_for) {
+      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 pointed to shared memory that
+  // is no longer valid.
+  ElevatorQueue queue_;
+
+  // Create a control loop and simulation.
+  Elevator elevator_;
+  ElevatorSimulator plant_;
+};
+
+// Tests that the loop does nothing when the goal is zero.
+TEST_F(ElevatorTest, DoesNothing) {
+  // Set the goals to the starting values. This should theoretically guarantee
+  // that the controller does nothing.
+  ASSERT_TRUE(queue_.goal.MakeWithBuilder()
+                  .height(kElevLowerLimit)
+                  .max_velocity(20)
+                  .max_acceleration(20)
+                  .Send());
+
+  // Run a few iterations.
+  RunForTime(Time::InSeconds(5));
+
+  VerifyNearGoal();
+}
+
+// Tests that the loop can reach a goal.
+TEST_F(ElevatorTest, ReachesGoal) {
+  // Set a reasonable goal.
+  ASSERT_TRUE(queue_.goal.MakeWithBuilder()
+                  .height(kElevLowerLimit + 0.5)
+                  .max_velocity(20)
+                  .max_acceleration(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(ElevatorTest, RespectsRange) {
+  // We're going to send the elevator to -5, which should be significantly too
+  // low.
+  ASSERT_TRUE(queue_.goal.MakeWithBuilder()
+                  .height(-5.0)
+                  .max_velocity(20)
+                  .max_acceleration(20)
+                  .Send());
+
+  RunForTime(Time::InSeconds(10));
+
+  // Check that we are near our soft limit.
+  queue_.status.FetchLatest();
+  EXPECT_NEAR(kElevLowerLimit, queue_.status->height, 0.001);
+
+  // We're going to give the elevator some ridiculously high goal.
+  ASSERT_TRUE(queue_.goal.MakeWithBuilder()
+                  .height(50.0)
+                  .max_velocity(20)
+                  .max_acceleration(20)
+                  .Send());
+
+  RunForTime(Time::InSeconds(10));
+
+  // Check that we are near our soft limit.
+  queue_.status.FetchLatest();
+  EXPECT_NEAR(kElevUpperLimit, queue_.status->height, 0.001);
+}
+
+// Tests that the loop zeroes when run for a while.
+TEST_F(ElevatorTest, ZeroTest) {
+  queue_.goal.MakeWithBuilder()
+      .height(0.5)
+      .max_velocity(20)
+      .max_acceleration(20)
+      .Send();
+  RunForTime(Time::InSeconds(5));
+
+  VerifyNearGoal();
+}
+
+// Tests that starting at the lower hardstops doesn't cause an abort.
+TEST_F(ElevatorTest, LowerHardstopStartup) {
+  plant_.InitializePosition(kElevLowerHardLimit);
+  queue_.goal.MakeWithBuilder().height(0.4).Send();
+  RunForTime(Time::InMS(5000));
+
+  VerifyNearGoal();
+}
+
+// Tests that starting at the upper hardstops doesn't cause an abort.
+TEST_F(ElevatorTest, UpperHardstopStartup) {
+  plant_.InitializePosition(kElevUpperHardLimit);
+  queue_.goal.MakeWithBuilder().height(0.4).Send();
+  RunForTime(Time::InMS(20000));
+
+  VerifyNearGoal();
+}
+
+// Tests that resetting WPILib results in a rezero.
+TEST_F(ElevatorTest, ResetTest) {
+  plant_.InitializePosition(kElevLowerLimit);
+  queue_.goal.MakeWithBuilder().height(0.05).Send();
+  RunForTime(Time::InMS(5000));
+
+  EXPECT_EQ(Elevator::RUNNING, elevator_.state());
+  VerifyNearGoal();
+  SimulateSensorReset();
+  RunForTime(Time::InMS(100));
+  EXPECT_NE(Elevator::RUNNING, elevator_.state());
+  RunForTime(Time::InMS(10000));
+  EXPECT_EQ(Elevator::RUNNING, elevator_.state());
+  VerifyNearGoal();
+}
+
+// Tests that the internal goals don't change while disabled.
+TEST_F(ElevatorTest, DisabledGoalTest) {
+  queue_.goal.MakeWithBuilder().height(0.45).Send();
+
+  RunForTime(Time::InMS(100), false);
+  EXPECT_EQ(0.0, elevator_.goal_);
+
+  // Now make sure they move correctly
+  RunForTime(Time::InMS(4000), true);
+  EXPECT_NE(0.0, elevator_.goal_);
+}
+
+// Tests that the elevator zeroing goals don't wind up too far.
+TEST_F(ElevatorTest, ElevatorGoalPositiveWindupTest) {
+  plant_.InitializePosition(0.0, kHallEffectPosition);
+  ASSERT_TRUE(queue_.goal.MakeWithBuilder().height(0.45).Send());
+
+  while (elevator_.state() != Elevator::ZEROING) {
+    RunIteration();
+  }
+
+  // Kick it.
+  RunForTime(Time::InMS(50));
+  double orig_goal = elevator_.goal_;
+  elevator_.goal_ += 100.0;
+
+  RunIteration();
+  EXPECT_NEAR(orig_goal, elevator_.goal_, 0.05);
+
+  RunIteration();
+  EXPECT_EQ(elevator_.loop_->U(), elevator_.loop_->U_uncapped());
+
+  EXPECT_EQ(elevator_.state(), Elevator::ZEROING);
+}
+
+// Tests that the elevator zeroing goals don't wind up too far.
+TEST_F(ElevatorTest, ElevatorGoalNegativeWindupTest) {
+  plant_.InitializePosition(0.0, kHallEffectPosition);
+  ASSERT_TRUE(queue_.goal.MakeWithBuilder().height(0.45).Send());
+
+  while (elevator_.state() != Elevator::ZEROING) {
+    RunIteration();
+  }
+
+  // Kick it.
+  RunForTime(Time::InMS(50));
+  double orig_goal = elevator_.goal_;
+  elevator_.goal_ -= 100.0;
+
+  RunIteration();
+  EXPECT_NEAR(orig_goal, elevator_.goal_, 0.05);
+
+  RunIteration();
+  EXPECT_EQ(elevator_.loop_->U(), elevator_.loop_->U_uncapped());
+
+  EXPECT_EQ(elevator_.state(), Elevator::ZEROING);
+}
+
+// Tests that the loop zeroes when run for a while.
+TEST_F(ElevatorTest, ZeroNoGoal) {
+  RunForTime(Time::InSeconds(5));
+
+  EXPECT_EQ(Elevator::RUNNING, elevator_.state());
+}
+
+// Zeroing tests
+// TODO(comran+adam): The following tests don't look like they are complete, so
+// we will need to get these tests checked & passed before running the elevator
+// control loop.
+
+// Tests that starting in the middle zeros correctly.
+TEST_F(ElevatorTest, CasualStart) {
+  const double start_position = (kElevUpperLimit + kElevLowerLimit) / 2.0;
+  plant_.InitializePosition(start_position, kHallEffectPosition);
+
+  while (elevator_.state() != Elevator::RUNNING) {
+    RunIteration();
+  }
+
+  EXPECT_TRUE(elevator_.CheckZeroed());
+  queue_.goal.MakeWithBuilder().height(0.5).Send();
+
+  // Find a reasonable value for the time.
+  RunForTime(Time::InSeconds(5));
+  VerifyNearGoal();
+}
+
+// Tests that a start position above the upper soft limit zeros correctly.
+TEST_F(ElevatorTest, AboveSoftLimitStart) {
+  const double start_position = (kElevUpperHardLimit + kElevUpperLimit) / 2.0;
+  plant_.InitializePosition(start_position, kHallEffectPosition);
+  while (elevator_.state() != Elevator::RUNNING) {
+    RunIteration();
+  }
+
+  EXPECT_TRUE(elevator_.CheckZeroed());
+  queue_.goal.MakeWithBuilder().height(0.5).Send();
+  RunForTime(Time::InSeconds(5));
+  VerifyNearGoal();
+}
+
+// Tests that a start position below the lower soft limit zeros correctly.
+TEST_F(ElevatorTest, BelowSoftLimitStart) {
+  const double start_position = (kElevLowerHardLimit + kElevLowerLimit) / 2;
+  plant_.InitializePosition(start_position, kHallEffectPosition);
+  while (elevator_.state() != Elevator::RUNNING) {
+    RunIteration();
+  }
+
+  EXPECT_TRUE(elevator_.CheckZeroed());
+  queue_.goal.MakeWithBuilder().height(0.4).Send();
+  RunForTime(Time::InSeconds(5));
+  VerifyNearGoal();
+}
+
+// Tests it zeroes when we start above the hall effect sensor.
+TEST_F(ElevatorTest, OverHallEffect) {
+  const double start_position = kHallEffectPosition;
+  plant_.InitializePosition(start_position, kHallEffectPosition);
+  while (elevator_.state() != Elevator::RUNNING) {
+    RunIteration();
+  }
+
+  EXPECT_TRUE(elevator_.CheckZeroed());
+  queue_.goal.MakeWithBuilder().height(0.4).Send();
+  RunForTime(Time::InSeconds(5));
+  VerifyNearGoal();
+}
+
+// Tests that we go down fast, up slow, and then get zeroed.
+TEST_F(ElevatorTest, DownAndUp) {
+  const double start_position = (kElevUpperLimit + kElevLowerLimit) / 2.0;
+  plant_.InitializePosition(start_position, kHallEffectPosition);
+
+  while (elevator_.state() != Elevator::RUNNING) {
+    RunIteration();
+  }
+
+  EXPECT_TRUE(elevator_.CheckZeroed());
+
+  // Make sure we ended off the hall effect.
+  queue_.position.FetchLatest();
+  EXPECT_TRUE(queue_.position.get() != nullptr);
+  EXPECT_FALSE(queue_.position->bottom_hall_effect);
+}
+
+// Verify that we can zero while disabled.
+TEST_F(ElevatorTest, ZeroWhileDisabled) {
+  const double start_position = (kElevUpperLimit + kElevLowerLimit) / 2.0;
+  plant_.InitializePosition(start_position, kHallEffectPosition);
+
+  // Running iteration while disabled
+  RunIteration(false);
+  RunIteration(false);
+
+  // Move elevator below hall effect.
+  plant_.MoveTo(kHallEffectPosition - 0.1);
+  RunIteration(false);
+  plant_.MoveTo(kHallEffectPosition - 0.1);
+  RunIteration(false);
+  // Make sure it only zeroes while going up.
+  EXPECT_TRUE(!elevator_.CheckZeroed());
+  // Move above the hall effect.
+  plant_.MoveTo(kHallEffectPosition + 0.1);
+  RunIteration(false);
+  RunIteration(false);
+  // Make sure we are zeroed.
+  EXPECT_TRUE(elevator_.CheckZeroed());
+}
+
+// Tests that the loop can reach a goal with a constant force.
+TEST_F(ElevatorTest, ReachesGoalWithIntegral) {
+  // Set a reasonable goal.
+  ASSERT_TRUE(queue_.goal.MakeWithBuilder()
+                  .height(kElevLowerLimit + 0.5)
+                  .max_velocity(20)
+                  .max_acceleration(20)
+                  .Send());
+
+  plant_.set_acceleration_offset(0.1);
+  // Give it a lot of time to get there.
+  RunForTime(Time::InSeconds(5));
+
+  VerifyNearGoal();
+}
+
+// Tests that the goal (with motion profile) doesn't run away from the elevator
+TEST_F(ElevatorTest, PositiveRunawayProfileTest) {
+  ASSERT_TRUE(queue_.goal.MakeWithBuilder().height(0.45).Send());
+
+  while (elevator_.state() != Elevator::RUNNING) {
+    RunIteration();
+  }
+
+  // Kick it.
+  RunForTime(Time::InMS(50));
+
+
+  double orig_goal = elevator_.goal_;
+  {
+    Eigen::Matrix<double, 2, 1> current;
+    current.setZero();
+    current << elevator_.goal_ + 100.0, elevator_.goal_velocity_;
+    elevator_.profile_.MoveCurrentState(current);
+  }
+  RunIteration();
+  EXPECT_NEAR(orig_goal, elevator_.goal_, 0.05);
+
+  RunIteration();
+  EXPECT_EQ(elevator_.loop_->U(), elevator_.loop_->U_uncapped());
+
+  EXPECT_EQ(elevator_.state(), Elevator::RUNNING);
+}
+
+// Tests that the goal (with motion profile) doesn't run away from the elevator
+TEST_F(ElevatorTest, NegativeRunawayProfileTest) {
+  ASSERT_TRUE(queue_.goal.MakeWithBuilder().height(0.45).Send());
+
+  while (elevator_.state() != Elevator::RUNNING) {
+    RunIteration();
+  }
+
+  // Kick it.
+  RunForTime(Time::InMS(50));
+
+
+  double orig_goal = elevator_.goal_;
+  {
+    Eigen::Matrix<double, 2, 1> current;
+    current.setZero();
+    current << elevator_.goal_ - 100.0, elevator_.goal_velocity_;
+    elevator_.profile_.MoveCurrentState(current);
+  }
+  RunIteration();
+  EXPECT_NEAR(orig_goal, elevator_.goal_, 0.05);
+
+  RunIteration();
+  EXPECT_EQ(elevator_.loop_->U(), elevator_.loop_->U_uncapped());
+
+  EXPECT_EQ(elevator_.state(), Elevator::RUNNING);
+}
+
+// Tests that the glitch filter handles positive edges.
+TEST(GlitchFilterTest, PosedgeDetect) {
+  GlitchFilter filter;
+  filter.Reset(false);
+  EXPECT_FALSE(filter.filtered_value());
+  EXPECT_FALSE(filter.posedge());
+
+  filter.Update(false, 0.0);
+  EXPECT_FALSE(filter.filtered_value());
+  EXPECT_FALSE(filter.posedge());
+
+  filter.Update(true, 1.0);
+  EXPECT_FALSE(filter.filtered_value());
+  EXPECT_FALSE(filter.posedge());
+
+  filter.Update(true, 2.0);
+  EXPECT_TRUE(filter.filtered_value());
+  EXPECT_TRUE(filter.posedge());
+
+  filter.Update(true, 3.0);
+  EXPECT_TRUE(filter.filtered_value());
+  EXPECT_FALSE(filter.posedge());
+  EXPECT_EQ(0.5, filter.posedge_value());
+}
+
+// Tests that the glitch filter handles negative edges.
+TEST(GlitchFilterTest, NegedgeDetect) {
+  GlitchFilter filter;
+  filter.Reset(true);
+  EXPECT_TRUE(filter.filtered_value());
+  EXPECT_FALSE(filter.negedge());
+
+  filter.Update(true, 0.0);
+  EXPECT_TRUE(filter.filtered_value());
+  EXPECT_FALSE(filter.negedge());
+
+  filter.Update(false, 1.0);
+  EXPECT_TRUE(filter.filtered_value());
+  EXPECT_FALSE(filter.negedge());
+
+  filter.Update(false, 2.0);
+  EXPECT_FALSE(filter.filtered_value());
+  EXPECT_TRUE(filter.negedge());
+
+  filter.Update(false, 3.0);
+  EXPECT_FALSE(filter.filtered_value());
+  EXPECT_FALSE(filter.negedge());
+  EXPECT_EQ(0.5, filter.negedge_value());
+}
+
+}  // namespace testing
+}  // namespace control_loops
+}  // namespace bot3
diff --git a/bot3/control_loops/elevator/elevator_main.cc b/bot3/control_loops/elevator/elevator_main.cc
new file mode 100644
index 0000000..f7a5eef
--- /dev/null
+++ b/bot3/control_loops/elevator/elevator_main.cc
@@ -0,0 +1,11 @@
+#include "bot3/control_loops/elevator/elevator.h"
+
+#include "aos/linux_code/init.h"
+
+int main() {
+  ::aos::Init();
+  ::bot3::control_loops::Elevator elevator;
+  elevator.Run();
+  ::aos::Cleanup();
+  return 0;
+}
diff --git a/bot3/control_loops/elevator/elevator_motor_plant.cc b/bot3/control_loops/elevator/elevator_motor_plant.cc
new file mode 100644
index 0000000..d3985b6
--- /dev/null
+++ b/bot3/control_loops/elevator/elevator_motor_plant.cc
@@ -0,0 +1,49 @@
+#include "bot3/control_loops/elevator/elevator_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace bot3 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeElevatorPlantCoefficients() {
+  Eigen::Matrix<double, 2, 2> A;
+  A << 1.0, 0.00329835431624, 0.0, 0.407009515002;
+  Eigen::Matrix<double, 2, 1> B;
+  B << 0.000238685884904, 0.0831773970353;
+  Eigen::Matrix<double, 1, 2> C;
+  C << 1, 0;
+  Eigen::Matrix<double, 1, 1> D;
+  D << 0;
+  Eigen::Matrix<double, 1, 1> U_max;
+  U_max << 12.0;
+  Eigen::Matrix<double, 1, 1> U_min;
+  U_min << -12.0;
+  return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<2, 1, 1> MakeElevatorController() {
+  Eigen::Matrix<double, 2, 1> L;
+  L << 0.843942422954, 1.30830010079;
+  Eigen::Matrix<double, 1, 2> K;
+  K << 327.58799433, 4.77067036522;
+  Eigen::Matrix<double, 2, 2> A_inv;
+  A_inv << 1.0, -0.00810387520358, 0.0, 2.4569450176;
+  return StateFeedbackController<2, 1, 1>(L, K, A_inv, MakeElevatorPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 1, 1> MakeElevatorPlant() {
+  ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 1, 1>>> plants(1);
+  plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 1, 1>>(new StateFeedbackPlantCoefficients<2, 1, 1>(MakeElevatorPlantCoefficients()));
+  return StateFeedbackPlant<2, 1, 1>(&plants);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeElevatorLoop() {
+  ::std::vector< ::std::unique_ptr<StateFeedbackController<2, 1, 1>>> controllers(1);
+  controllers[0] = ::std::unique_ptr<StateFeedbackController<2, 1, 1>>(new StateFeedbackController<2, 1, 1>(MakeElevatorController()));
+  return StateFeedbackLoop<2, 1, 1>(&controllers);
+}
+
+}  // namespace control_loops
+}  // namespace bot3
diff --git a/bot3/control_loops/elevator/elevator_motor_plant.h b/bot3/control_loops/elevator/elevator_motor_plant.h
new file mode 100644
index 0000000..d87c79e
--- /dev/null
+++ b/bot3/control_loops/elevator/elevator_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef BOT3_CONTROL_LOOPS_ELEVATOR_ELEVATOR_MOTOR_PLANT_H_
+#define BOT3_CONTROL_LOOPS_ELEVATOR_ELEVATOR_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace bot3 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeElevatorPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeElevatorController();
+
+StateFeedbackPlant<2, 1, 1> MakeElevatorPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeElevatorLoop();
+
+}  // namespace control_loops
+}  // namespace bot3
+
+#endif  // BOT3_CONTROL_LOOPS_ELEVATOR_ELEVATOR_MOTOR_PLANT_H_
diff --git a/bot3/control_loops/elevator/integral_elevator_motor_plant.cc b/bot3/control_loops/elevator/integral_elevator_motor_plant.cc
new file mode 100644
index 0000000..fba91c1
--- /dev/null
+++ b/bot3/control_loops/elevator/integral_elevator_motor_plant.cc
@@ -0,0 +1,49 @@
+#include "bot3/control_loops/elevator/integral_elevator_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace bot3 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<3, 1, 1> MakeIntegralElevatorPlantCoefficients() {
+  Eigen::Matrix<double, 3, 3> A;
+  A << 1.0, 0.00329835431624, 0.000238685884904, 0.0, 0.407009515002, 0.0831773970353, 0.0, 0.0, 1.0;
+  Eigen::Matrix<double, 3, 1> B;
+  B << 0.000238685884904, 0.0831773970353, 0.0;
+  Eigen::Matrix<double, 1, 3> C;
+  C << 1.0, 0.0, 0.0;
+  Eigen::Matrix<double, 1, 1> D;
+  D << 0;
+  Eigen::Matrix<double, 1, 1> U_max;
+  U_max << 12.0;
+  Eigen::Matrix<double, 1, 1> U_min;
+  U_min << -12.0;
+  return StateFeedbackPlantCoefficients<3, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<3, 1, 1> MakeIntegralElevatorController() {
+  Eigen::Matrix<double, 3, 1> L;
+  L << 0.819604251485, 7.84671425417, 56.221891957;
+  Eigen::Matrix<double, 1, 3> K;
+  K << 327.58799433, 4.77067036522, 1.0;
+  Eigen::Matrix<double, 3, 3> A_inv;
+  A_inv << 1.0, -0.00810387520358, 0.000435373360429, 0.0, 2.4569450176, -0.204362291223, 0.0, 0.0, 1.0;
+  return StateFeedbackController<3, 1, 1>(L, K, A_inv, MakeIntegralElevatorPlantCoefficients());
+}
+
+StateFeedbackPlant<3, 1, 1> MakeIntegralElevatorPlant() {
+  ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<3, 1, 1>>> plants(1);
+  plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<3, 1, 1>>(new StateFeedbackPlantCoefficients<3, 1, 1>(MakeIntegralElevatorPlantCoefficients()));
+  return StateFeedbackPlant<3, 1, 1>(&plants);
+}
+
+StateFeedbackLoop<3, 1, 1> MakeIntegralElevatorLoop() {
+  ::std::vector< ::std::unique_ptr<StateFeedbackController<3, 1, 1>>> controllers(1);
+  controllers[0] = ::std::unique_ptr<StateFeedbackController<3, 1, 1>>(new StateFeedbackController<3, 1, 1>(MakeIntegralElevatorController()));
+  return StateFeedbackLoop<3, 1, 1>(&controllers);
+}
+
+}  // namespace control_loops
+}  // namespace bot3
diff --git a/bot3/control_loops/elevator/integral_elevator_motor_plant.h b/bot3/control_loops/elevator/integral_elevator_motor_plant.h
new file mode 100644
index 0000000..64a4539
--- /dev/null
+++ b/bot3/control_loops/elevator/integral_elevator_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef BOT3_CONTROL_LOOPS_ELEVATOR_INTEGRAL_ELEVATOR_MOTOR_PLANT_H_
+#define BOT3_CONTROL_LOOPS_ELEVATOR_INTEGRAL_ELEVATOR_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace bot3 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<3, 1, 1> MakeIntegralElevatorPlantCoefficients();
+
+StateFeedbackController<3, 1, 1> MakeIntegralElevatorController();
+
+StateFeedbackPlant<3, 1, 1> MakeIntegralElevatorPlant();
+
+StateFeedbackLoop<3, 1, 1> MakeIntegralElevatorLoop();
+
+}  // namespace control_loops
+}  // namespace bot3
+
+#endif  // BOT3_CONTROL_LOOPS_ELEVATOR_INTEGRAL_ELEVATOR_MOTOR_PLANT_H_
diff --git a/bot3/control_loops/position_sensor_sim.cc b/bot3/control_loops/position_sensor_sim.cc
new file mode 100644
index 0000000..480cd98
--- /dev/null
+++ b/bot3/control_loops/position_sensor_sim.cc
@@ -0,0 +1,28 @@
+#include "bot3/control_loops/position_sensor_sim.h"
+
+#include <cmath>
+
+namespace bot3 {
+namespace control_loops {
+
+void PositionSensorSimulator::Initialize(double start_position,
+                                         double hall_effect_position) {
+  hall_effect_position_ = hall_effect_position;
+  start_position_ = start_position;
+  cur_pos_ = start_position;
+}
+
+void PositionSensorSimulator::MoveTo(double new_pos) { cur_pos_ = new_pos; }
+
+void PositionSensorSimulator::GetSensorValues(
+    control_loops::ElevatorQueue::Position* position) {
+  position->encoder = cur_pos_ - start_position_;
+  if (cur_pos_ <= hall_effect_position_) {
+    position->bottom_hall_effect = true;
+  } else {
+    position->bottom_hall_effect = false;
+  }
+}
+
+}  // namespace control_loops
+}  // namespace bot3
diff --git a/bot3/control_loops/position_sensor_sim.h b/bot3/control_loops/position_sensor_sim.h
new file mode 100644
index 0000000..f025010
--- /dev/null
+++ b/bot3/control_loops/position_sensor_sim.h
@@ -0,0 +1,47 @@
+#ifndef BOT3_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_
+#define BOT3_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_
+
+#include "bot3/control_loops/elevator/elevator.q.h"
+
+namespace bot3 {
+namespace control_loops {
+
+// NOTE: All encoder values in this class are assumed to be in
+// translated SI units.
+
+class PositionSensorSimulator {
+ public:
+
+  // Set new parameters for the sensors. This is useful for unit tests to change
+  // the simulated sensors' behavior on the fly.
+  // start_position: The position relative to absolute zero where the simulated
+  //                 structure starts. For example, to simulate the elevator
+  //                 starting at 40cm above absolute zero, set this to 0.4.
+  void Initialize(double start_position, double hall_effect_position);
+
+  // Simulate the structure moving to a new position. The new value is measured
+  // relative to absolute zero. This will update the simulated sensors with new
+  // readings.
+  // new_position: The new position relative to absolute zero.
+  void MoveTo(double new_position);
+
+  // Get the current values of the simulated sensors.
+  // values: The target structure will be populated with simulated sensor
+  //         readings. The readings will be in SI units. For example the units
+  //         can be given in radians, meters, etc.
+  void GetSensorValues(control_loops::ElevatorQueue::Position* position);
+
+ private:
+  // the position of the bottom hall effect sensor.
+  double hall_effect_position_;
+  // Current position of the mechanism relative to absolute zero.
+  double cur_pos_;
+  // Starting position of the mechanism relative to absolute zero. See the
+  // `starting_position` parameter in the constructor for more info.
+  double start_position_;
+};
+
+}  // namespace control_loops
+}  // namespace bot3
+
+#endif /* BOT3_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_ */
diff --git a/bot3/control_loops/python/control_loop.py b/bot3/control_loops/python/control_loop.py
deleted file mode 100644
index 881880f..0000000
--- a/bot3/control_loops/python/control_loop.py
+++ /dev/null
@@ -1,338 +0,0 @@
-import controls
-import numpy
-
-class Constant(object):
-  def __init__ (self, name, formatt, value):
-    self.name = name
-    self.formatt = formatt
-    self.value = value
-    self.formatToType = {}
-    self.formatToType['%f'] = "double";
-    self.formatToType['%d'] = "int";
-  def __str__ (self):
-    return str("\nstatic constexpr %s %s = "+ self.formatt +";\n") % \
-        (self.formatToType[self.formatt], self.name, self.value)
-
-
-class ControlLoopWriter(object):
-  def __init__(self, gain_schedule_name, loops, namespaces=None, write_constants=False):
-    """Constructs a control loop writer.
-
-    Args:
-      gain_schedule_name: string, Name of the overall controller.
-      loops: array[ControlLoop], a list of control loops to gain schedule
-        in order.
-      namespaces: array[string], a list of names of namespaces to nest in
-        order.  If None, the default will be used.
-    """
-    self._gain_schedule_name = gain_schedule_name
-    self._loops = loops
-    if namespaces:
-      self._namespaces = namespaces
-    else:
-      self._namespaces = ['frc971', 'control_loops']
-
-    self._namespace_start = '\n'.join(
-        ['namespace %s {' % name for name in self._namespaces])
-
-    self._namespace_end = '\n'.join(
-        ['}  // namespace %s' % name for name in reversed(self._namespaces)])
-    
-    self._constant_list = []
-
-  def AddConstant(self, constant):
-    """Adds a constant to write.
-
-    Args:
-      constant: Constant, the constant to add to the header.
-    """
-    self._constant_list.append(constant)
-
-  def _TopDirectory(self):
-    return self._namespaces[0]
-
-  def _HeaderGuard(self, header_file):
-    return (self._TopDirectory().upper() + '_CONTROL_LOOPS_' +
-            header_file.upper().replace('.', '_').replace('/', '_') +
-            '_')
-
-  def Write(self, header_file, cc_file):
-    """Writes the loops to the specified files."""
-    self.WriteHeader(header_file)
-    self.WriteCC(header_file, cc_file)
-
-  def _GenericType(self, typename):
-    """Returns a loop template using typename for the type."""
-    num_states = self._loops[0].A.shape[0]
-    num_inputs = self._loops[0].B.shape[1]
-    num_outputs = self._loops[0].C.shape[0]
-    return '%s<%d, %d, %d>' % (
-        typename, num_states, num_inputs, num_outputs)
-
-  def _ControllerType(self):
-    """Returns a template name for StateFeedbackController."""
-    return self._GenericType('StateFeedbackController')
-
-  def _LoopType(self):
-    """Returns a template name for StateFeedbackLoop."""
-    return self._GenericType('StateFeedbackLoop')
-
-  def _PlantType(self):
-    """Returns a template name for StateFeedbackPlant."""
-    return self._GenericType('StateFeedbackPlant')
-
-  def _CoeffType(self):
-    """Returns a template name for StateFeedbackPlantCoefficients."""
-    return self._GenericType('StateFeedbackPlantCoefficients')
-
-  def WriteHeader(self, header_file, double_appendage=False, MoI_ratio=0.0):
-    """Writes the header file to the file named header_file.
-       Set double_appendage to true in order to include a ratio of
-       moments of inertia constant. Currently, only used for 2014 claw."""
-    with open(header_file, 'w') as fd:
-      header_guard = self._HeaderGuard(header_file)
-      fd.write('#ifndef %s\n'
-               '#define %s\n\n' % (header_guard, header_guard))
-      fd.write('#include \"frc971/control_loops/state_feedback_loop.h\"\n')
-      fd.write('\n')
-
-      fd.write(self._namespace_start)
-
-      for const in self._constant_list:
-          fd.write(str(const))
-
-      fd.write('\n\n')
-      for loop in self._loops:
-        fd.write(loop.DumpPlantHeader())
-        fd.write('\n')
-        fd.write(loop.DumpControllerHeader())
-        fd.write('\n')
-
-      fd.write('%s Make%sPlant();\n\n' %
-               (self._PlantType(), self._gain_schedule_name))
-
-      fd.write('%s Make%sLoop();\n\n' %
-               (self._LoopType(), self._gain_schedule_name))
-
-      fd.write(self._namespace_end)
-      fd.write('\n\n')
-      fd.write("#endif  // %s\n" % header_guard)
-
-  def WriteCC(self, header_file_name, cc_file):
-    """Writes the cc file to the file named cc_file."""
-    with open(cc_file, 'w') as fd:
-      fd.write('#include \"%s/control_loops/%s\"\n' %
-               (self._TopDirectory(), header_file_name))
-      fd.write('\n')
-      fd.write('#include <vector>\n')
-      fd.write('\n')
-      fd.write('#include \"frc971/control_loops/state_feedback_loop.h\"\n')
-      fd.write('\n')
-      fd.write(self._namespace_start)
-      fd.write('\n\n')
-      for loop in self._loops:
-        fd.write(loop.DumpPlant())
-        fd.write('\n')
-
-      for loop in self._loops:
-        fd.write(loop.DumpController())
-        fd.write('\n')
-
-      fd.write('%s Make%sPlant() {\n' %
-               (self._PlantType(), self._gain_schedule_name))
-      fd.write('  ::std::vector< ::std::unique_ptr<%s>> plants(%d);\n' % (
-          self._CoeffType(), len(self._loops)))
-      for index, loop in enumerate(self._loops):
-        fd.write('  plants[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
-                 (index, self._CoeffType(), self._CoeffType(),
-                  loop.PlantFunction()))
-      fd.write('  return %s(&plants);\n' % self._PlantType())
-      fd.write('}\n\n')
-
-      fd.write('%s Make%sLoop() {\n' %
-               (self._LoopType(), self._gain_schedule_name))
-      fd.write('  ::std::vector< ::std::unique_ptr<%s>> controllers(%d);\n' % (
-          self._ControllerType(), len(self._loops)))
-      for index, loop in enumerate(self._loops):
-        fd.write('  controllers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
-                 (index, self._ControllerType(), self._ControllerType(),
-                  loop.ControllerFunction()))
-      fd.write('  return %s(&controllers);\n' % self._LoopType())
-      fd.write('}\n\n')
-
-      fd.write(self._namespace_end)
-      fd.write('\n')
-
-
-class ControlLoop(object):
-  def __init__(self, name):
-    """Constructs a control loop object.
-
-    Args:
-      name: string, The name of the loop to use when writing the C++ files.
-    """
-    self._name = name
-
-  def ContinuousToDiscrete(self, A_continuous, B_continuous, dt):
-    """Calculates the discrete time values for A and B.
-
-      Args:
-        A_continuous: numpy.matrix, The continuous time A matrix
-        B_continuous: numpy.matrix, The continuous time B matrix
-        dt: float, The time step of the control loop
-
-      Returns:
-        (A, B), numpy.matrix, the control matricies.
-    """
-    return controls.c2d(A_continuous, B_continuous, dt)
-
-  def InitializeState(self):
-    """Sets X, Y, and X_hat to zero defaults."""
-    self.X = numpy.zeros((self.A.shape[0], 1))
-    self.Y = self.C * self.X
-    self.X_hat = numpy.zeros((self.A.shape[0], 1))
-
-  def PlaceControllerPoles(self, poles):
-    """Places the controller poles.
-
-    Args:
-      poles: array, An array of poles.  Must be complex conjegates if they have
-        any imaginary portions.
-    """
-    self.K = controls.dplace(self.A, self.B, poles)
-
-  def PlaceObserverPoles(self, poles):
-    """Places the observer poles.
-
-    Args:
-      poles: array, An array of poles.  Must be complex conjegates if they have
-        any imaginary portions.
-    """
-    self.L = controls.dplace(self.A.T, self.C.T, poles).T
-
-  def Update(self, U):
-    """Simulates one time step with the provided U."""
-    #U = numpy.clip(U, self.U_min, self.U_max)
-    self.X = self.A * self.X + self.B * U
-    self.Y = self.C * self.X + self.D * U
-
-  def PredictObserver(self, U):
-    """Runs the predict step of the observer update."""
-    self.X_hat = (self.A * self.X_hat + self.B * U)
-
-  def CorrectObserver(self, U):
-    """Runs the correct step of the observer update."""
-    self.X_hat += numpy.linalg.inv(self.A) * self.L * (
-        self.Y - self.C * self.X_hat - self.D * U)
-
-  def UpdateObserver(self, U):
-    """Updates the observer given the provided U."""
-    self.X_hat = (self.A * self.X_hat + self.B * U +
-                  self.L * (self.Y - self.C * self.X_hat - self.D * U))
-
-  def _DumpMatrix(self, matrix_name, matrix):
-    """Dumps the provided matrix into a variable called matrix_name.
-
-    Args:
-      matrix_name: string, The variable name to save the matrix to.
-      matrix: The matrix to dump.
-
-    Returns:
-      string, The C++ commands required to populate a variable named matrix_name
-        with the contents of matrix.
-    """
-    ans = ['  Eigen::Matrix<double, %d, %d> %s;\n' % (
-        matrix.shape[0], matrix.shape[1], matrix_name)]
-    first = True
-    for x in xrange(matrix.shape[0]):
-      for y in xrange(matrix.shape[1]):
-        element = matrix[x, y]
-        if first:
-          ans.append('  %s << ' % matrix_name)
-          first = False
-        else:
-          ans.append(', ')
-        ans.append(str(element))
-
-    ans.append(';\n')
-    return ''.join(ans)
-
-  def DumpPlantHeader(self):
-    """Writes out a c++ header declaration which will create a Plant object.
-
-    Returns:
-      string, The header declaration for the function.
-    """
-    num_states = self.A.shape[0]
-    num_inputs = self.B.shape[1]
-    num_outputs = self.C.shape[0]
-    return 'StateFeedbackPlantCoefficients<%d, %d, %d> Make%sPlantCoefficients();\n' % (
-        num_states, num_inputs, num_outputs, self._name)
-
-  def DumpPlant(self):
-    """Writes out a c++ function which will create a PlantCoefficients object.
-
-    Returns:
-      string, The function which will create the object.
-    """
-    num_states = self.A.shape[0]
-    num_inputs = self.B.shape[1]
-    num_outputs = self.C.shape[0]
-    ans = ['StateFeedbackPlantCoefficients<%d, %d, %d>'
-           ' Make%sPlantCoefficients() {\n' % (
-        num_states, num_inputs, num_outputs, self._name)]
-
-    ans.append(self._DumpMatrix('A', self.A))
-    ans.append(self._DumpMatrix('B', self.B))
-    ans.append(self._DumpMatrix('C', self.C))
-    ans.append(self._DumpMatrix('D', self.D))
-    ans.append(self._DumpMatrix('U_max', self.U_max))
-    ans.append(self._DumpMatrix('U_min', self.U_min))
-
-    ans.append('  return StateFeedbackPlantCoefficients<%d, %d, %d>'
-               '(A, B, C, D, U_max, U_min);\n' % (num_states, num_inputs,
-                                                  num_outputs))
-    ans.append('}\n')
-    return ''.join(ans)
-
-  def PlantFunction(self):
-    """Returns the name of the plant coefficient function."""
-    return 'Make%sPlantCoefficients()' % self._name
-
-  def ControllerFunction(self):
-    """Returns the name of the controller function."""
-    return 'Make%sController()' % self._name
-
-  def DumpControllerHeader(self):
-    """Writes out a c++ header declaration which will create a Controller object.
-
-    Returns:
-      string, The header declaration for the function.
-    """
-    num_states = self.A.shape[0]
-    num_inputs = self.B.shape[1]
-    num_outputs = self.C.shape[0]
-    return 'StateFeedbackController<%d, %d, %d> %s;\n' % (
-        num_states, num_inputs, num_outputs, self.ControllerFunction())
-
-  def DumpController(self):
-    """Returns a c++ function which will create a Controller object.
-
-    Returns:
-      string, The function which will create the object.
-    """
-    num_states = self.A.shape[0]
-    num_inputs = self.B.shape[1]
-    num_outputs = self.C.shape[0]
-    ans = ['StateFeedbackController<%d, %d, %d> %s {\n' % (
-        num_states, num_inputs, num_outputs, self.ControllerFunction())]
-
-    ans.append(self._DumpMatrix('L', self.L))
-    ans.append(self._DumpMatrix('K', self.K))
-    ans.append(self._DumpMatrix('A_inv', numpy.linalg.inv(self.A)))
-
-    ans.append('  return StateFeedbackController<%d, %d, %d>'
-               '(L, K, A_inv, Make%sPlantCoefficients());\n' % (
-                   num_states, num_inputs, num_outputs, self._name))
-    ans.append('}\n')
-    return ''.join(ans)
diff --git a/bot3/control_loops/python/drivetrain.py b/bot3/control_loops/python/drivetrain.py
index 8ed62d6..43b597f 100644
--- a/bot3/control_loops/python/drivetrain.py
+++ b/bot3/control_loops/python/drivetrain.py
@@ -232,7 +232,8 @@
   else:
     dog_loop_writer = control_loop.ControlLoopWriter(
         "Drivetrain", [drivetrain_low_low, drivetrain_low_high,
-                       drivetrain_high_low, drivetrain_high_high])
+                       drivetrain_high_low, drivetrain_high_high],
+        namespaces=['bot3', 'control_loops'])
     if argv[1][-3:] == '.cc':
       dog_loop_writer.Write(argv[2], argv[1])
     else:
diff --git a/bot3/control_loops/python/elevator.py b/bot3/control_loops/python/elevator.py
deleted file mode 100644
index 2d72ff2..0000000
--- a/bot3/control_loops/python/elevator.py
+++ /dev/null
@@ -1,247 +0,0 @@
-#!/usr/bin/python
-
-import control_loop
-import controls
-import polytope
-import polydrivetrain
-import numpy
-import sys
-import matplotlib
-from matplotlib import pylab
-
-class Elevator(control_loop.ControlLoop):
-  def __init__(self, name="Elevator", mass=None):
-    super(Elevator, self).__init__(name)
-    # Stall Torque in N m
-    self.stall_torque = 2.42
-    # Stall Current in Amps
-    self.stall_current = 133
-    # Free Speed in RPM
-    self.free_speed = 4650.0
-    # Free Current in Amps
-    self.free_current = 2.7
-    # Mass of the elevator
-    # TODO(comran): Get actual value.
-    self.mass = 13.0
-    # Resistance of the motor
-    self.R = 12.0 / self.stall_current
-    # Motor velocity constant
-    self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
-               (12.0 - self.R * self.free_current))
-    # Torque constant
-    self.Kt = self.stall_torque / self.stall_current
-    # Gear ratio
-    # TODO(comran): Get actual value.
-    self.G = (56.0 / 12.0) * (84.0 / 14.0)
-    # Pulley diameter
-    # TODO(comran): Get actual value.
-    self.r = 32 * 0.005 / numpy.pi / 2.0
-    # Control loop time step
-    self.dt = 0.005
-
-    # Elevator spring constant (N/m)
-    # TODO(comran): Get actual value.
-    self.spring = 800.0
-
-    # State is [average position, average velocity]
-    # Input is [V]
-
-    # TODO(comran): Change everything below.
-
-    C1 = self.spring / (self.mass * 0.5)
-    C2 = self.Kt * self.G / (self.mass * 0.5 * self.r * self.R)
-    C3 = self.G * self.G * self.Kt / (
-        self.R  * self.r * self.r * self.mass * 0.5 * self.Kv)
-
-    self.A_continuous = numpy.matrix(
-        [[0, 1, 0, 0],
-         [0, -C3, 0, 0],
-         [0, 0, 0, 1],
-         [0, 0, -C1 * 2.0, -C3]])
-
-    print "Full speed is", C2 / C3 * 12.0
-
-    # Start with the unmodified input
-    self.B_continuous = numpy.matrix(
-        [[0, 0],
-         [C2 / 2.0, C2 / 2.0],
-         [0, 0],
-         [C2 / 2.0, -C2 / 2.0]])
-
-    self.C = numpy.matrix([[1, 0, 1, 0],
-                           [1, 0, -1, 0]])
-    self.D = numpy.matrix([[0, 0],
-                           [0, 0]])
-
-    self.A, self.B = self.ContinuousToDiscrete(
-        self.A_continuous, self.B_continuous, self.dt)
-
-    print self.A
-
-    controlability = controls.ctrb(self.A, self.B);
-    print "Rank of augmented controlability matrix.", numpy.linalg.matrix_rank(
-        controlability)
-
-    q_pos = 0.02
-    q_vel = 0.400
-    q_pos_diff = 0.01
-    q_vel_diff = 0.45
-    self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
-                           [0.0, (1.0 / (q_vel ** 2.0)), 0.0, 0.0],
-                           [0.0, 0.0, (1.0 / (q_pos_diff ** 2.0)), 0.0],
-                           [0.0, 0.0, 0.0, (1.0 / (q_vel_diff ** 2.0))]])
-
-    self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
-                           [0.0, 1.0 / (12.0 ** 2.0)]])
-    self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
-    print self.K
-
-    print numpy.linalg.eig(self.A - self.B * self.K)[0]
-
-    self.rpl = 0.20
-    self.ipl = 0.05
-    self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
-                             self.rpl + 1j * self.ipl,
-                             self.rpl - 1j * self.ipl,
-                             self.rpl - 1j * self.ipl])
-
-    # The box formed by U_min and U_max must encompass all possible values,
-    # or else Austin's code gets angry.
-    self.U_max = numpy.matrix([[12.0], [12.0]])
-    self.U_min = numpy.matrix([[-12.0], [-12.0]])
-
-    self.InitializeState()
-
-
-def CapU(U):
-  if U[0, 0] - U[1, 0] > 24:
-    return numpy.matrix([[12], [-12]])
-  elif U[0, 0] - U[1, 0] < -24:
-    return numpy.matrix([[-12], [12]])
-  else:
-    max_u = max(U[0, 0], U[1, 0])
-    min_u = min(U[0, 0], U[1, 0])
-    if max_u > 12:
-      return U - (max_u - 12)
-    if min_u < -12:
-      return U - (min_u + 12)
-    return U
-
-
-def run_test(elevator, initial_X, goal, max_separation_error=0.01,
-             show_graph=True, iterations=200, controller_elevator=None,
-             observer_elevator=None):
-  """Runs the elevator plant with an initial condition and goal.
-
-    The tests themselves are not terribly sophisticated; I just test for
-    whether the goal has been reached and whether the separation goes
-    outside of the initial and goal values by more than max_separation_error.
-    Prints out something for a failure of either condition and returns
-    False if tests fail.
-    Args:
-      elevator: elevator object to use.
-      initial_X: starting state.
-      goal: goal state.
-      show_graph: Whether or not to display a graph showing the changing
-           states and voltages.
-      iterations: Number of timesteps to run the model for.
-      controller_elevator: elevator object to get K from, or None if we should
-          use elevator.
-      observer_elevator: elevator object to use for the observer, or None if we
-          should use the actual state.
-  """
-
-  elevator.X = initial_X
-
-  if controller_elevator is None:
-    controller_elevator = elevator
-
-  if observer_elevator is not None:
-    observer_elevator.X_hat = initial_X + 0.01
-    observer_elevator.X_hat = initial_X
-
-  # Various lists for graphing things.
-  t = []
-  x_avg = []
-  x_sep = []
-  x_hat_avg = []
-  x_hat_sep = []
-  v_avg = []
-  v_sep = []
-  u_left = []
-  u_right = []
-
-  sep_plot_gain = 100.0
-
-  for i in xrange(iterations):
-    X_hat = elevator.X
-    if observer_elevator is not None:
-      X_hat = observer_elevator.X_hat
-      x_hat_avg.append(observer_elevator.X_hat[0, 0])
-      x_hat_sep.append(observer_elevator.X_hat[2, 0] * sep_plot_gain)
-    U = controller_elevator.K * (goal - X_hat)
-    U = CapU(U)
-    x_avg.append(elevator.X[0, 0])
-    v_avg.append(elevator.X[1, 0])
-    x_sep.append(elevator.X[2, 0] * sep_plot_gain)
-    v_sep.append(elevator.X[3, 0])
-    if observer_elevator is not None:
-      observer_elevator.PredictObserver(U)
-    elevator.Update(U)
-    if observer_elevator is not None:
-      observer_elevator.Y = elevator.Y
-      observer_elevator.CorrectObserver(U)
-
-    t.append(i * elevator.dt)
-    u_left.append(U[0, 0])
-    u_right.append(U[1, 0])
-
-  print numpy.linalg.inv(elevator.A)
-  print "delta time is ", elevator.dt
-  print "Velocity at t=0 is ", x_avg[0], v_avg[0], x_sep[0], v_sep[0]
-  print "Velocity at t=1+dt is ", x_avg[1], v_avg[1], x_sep[1], v_sep[1]
-
-  if show_graph:
-    pylab.subplot(2, 1, 1)
-    pylab.plot(t, x_avg, label='x avg')
-    pylab.plot(t, x_sep, label='x sep')
-    if observer_elevator is not None:
-      pylab.plot(t, x_hat_avg, label='x_hat avg')
-      pylab.plot(t, x_hat_sep, label='x_hat sep')
-    pylab.legend()
-
-    pylab.subplot(2, 1, 2)
-    pylab.plot(t, u_left, label='u left')
-    pylab.plot(t, u_right, label='u right')
-    pylab.legend()
-    pylab.show()
-
-
-def main(argv):
-  loaded_mass = 25
-  #loaded_mass = 0
-  elevator = Elevator(mass=13 + loaded_mass)
-  elevator_controller = Elevator(mass=13 + 15)
-  observer_elevator = Elevator(mass=13 + 15)
-  #observer_elevator = None
-
-  # Test moving the elevator with constant separation.
-  initial_X = numpy.matrix([[0.0], [0.0], [0.01], [0.0]])
-  #initial_X = numpy.matrix([[0.0], [0.0], [0.00], [0.0]])
-  R = numpy.matrix([[1.0], [0.0], [0.0], [0.0]])
-  run_test(elevator, initial_X, R, controller_elevator=elevator_controller,
-           observer_elevator=observer_elevator)
-
-  # Write the generated constants out to a file.
-  if len(argv) != 3:
-    print "Expected .h file name and .cc file name for the elevator."
-  else:
-    elevator = Elevator("Elevator")
-    loop_writer = control_loop.ControlLoopWriter("Elevator", [elevator])
-    if argv[1][-3:] == '.cc':
-      loop_writer.Write(argv[2], argv[1])
-    else:
-      loop_writer.Write(argv[1], argv[2])
-
-if __name__ == '__main__':
-  sys.exit(main(sys.argv))
diff --git a/bot3/control_loops/python/elevator3.py b/bot3/control_loops/python/elevator3.py
index 120e8cc..04edd53 100755
--- a/bot3/control_loops/python/elevator3.py
+++ b/bot3/control_loops/python/elevator3.py
@@ -259,15 +259,24 @@
   scenario_plotter.Plot()
 
   # Write the generated constants out to a file.
-  if len(argv) != 3:
-    print "Expected .h file name and .cc file name for the Elevator."
+  if len(argv) != 5:
+    print "Expected .h file name and .cc file name for the Elevator and integral elevator."
   else:
     elevator = Elevator("Elevator")
-    loop_writer = control_loop.ControlLoopWriter("Elevator", [elevator])
+    loop_writer = control_loop.ControlLoopWriter("Elevator", [elevator],
+                                                 namespaces=['bot3', 'control_loops'])
     if argv[1][-3:] == '.cc':
       loop_writer.Write(argv[2], argv[1])
     else:
       loop_writer.Write(argv[1], argv[2])
 
+    integral_elevator = IntegralElevator("IntegralElevator")
+    integral_loop_writer = control_loop.ControlLoopWriter("IntegralElevator", [integral_elevator],
+                                                          namespaces=['bot3', 'control_loops'])
+    if argv[3][-3:] == '.cc':
+      integral_loop_writer.Write(argv[4], argv[3])
+    else:
+      integral_loop_writer.Write(argv[3], argv[4])
+
 if __name__ == '__main__':
   sys.exit(main(sys.argv))
diff --git a/bot3/control_loops/python/polydrivetrain.py b/bot3/control_loops/python/polydrivetrain.py
index a62c8c8..1090c8f 100644
--- a/bot3/control_loops/python/polydrivetrain.py
+++ b/bot3/control_loops/python/polydrivetrain.py
@@ -405,7 +405,8 @@
         "VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
                        vdrivetrain.drivetrain_low_high,
                        vdrivetrain.drivetrain_high_low,
-                       vdrivetrain.drivetrain_high_high])
+                       vdrivetrain.drivetrain_high_high],
+        namespaces=['bot3', 'control_loops'])
 
     if argv[1][-3:] == '.cc':
       dog_loop_writer.Write(argv[2], argv[1])
@@ -413,7 +414,8 @@
       dog_loop_writer.Write(argv[1], argv[2])
 
     cim_writer = control_loop.ControlLoopWriter(
-        "CIM", [drivetrain.CIM()])
+        "CIM", [drivetrain.CIM()],
+        namespaces=['bot3', 'control_loops'])
 
     if argv[5][-3:] == '.cc':
       cim_writer.Write(argv[6], argv[5])
diff --git a/bot3/control_loops/update_elevator.sh b/bot3/control_loops/update_elevator.sh
new file mode 100644
index 0000000..8bfb0ba
--- /dev/null
+++ b/bot3/control_loops/update_elevator.sh
@@ -0,0 +1,8 @@
+#!/bin/bash
+#
+# Updates the elevator controllers.
+
+cd $(dirname $0)
+
+./python/elevator3.py elevator/elevator_motor_plant.h \
+    elevator/elevator_motor_plant.cc