diff --git a/y2016/control_loops/python/shooter.py b/y2016/control_loops/python/shooter.py
index c81d597..3bb22a9 100755
--- a/y2016/control_loops/python/shooter.py
+++ b/y2016/control_loops/python/shooter.py
@@ -1,13 +1,21 @@
 #!/usr/bin/python
 
 from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
 import numpy
 import sys
 from matplotlib import pylab
 
-class Shooter(control_loop.ControlLoop):
-  def __init__(self):
-    super(Shooter, self).__init__("Shooter")
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
+class VelocityShooter(control_loop.ControlLoop):
+  def __init__(self, name='VelocityShooter'):
+    super(VelocityShooter, self).__init__(name)
     # Stall Torque in N m
     self.stall_torque = 0.71
     # Stall Current in Amps
@@ -31,43 +39,243 @@
     self.dt = 0.005
 
     # State feedback matrices
-    # [position, angular velocity]
+    # [angular velocity]
     self.A_continuous = numpy.matrix(
-        [[0, 1],
-         [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+        [[-self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
     self.B_continuous = numpy.matrix(
-        [[0],
-         [self.Kt / (self.J * self.G * self.R)]])
+        [[self.Kt / (self.J * self.G * self.R)]])
+    self.C = numpy.matrix([[1]])
+    self.D = numpy.matrix([[0]])
+
+    self.A, self.B = self.ContinuousToDiscrete(
+        self.A_continuous, self.B_continuous, self.dt)
+
+    self.PlaceControllerPoles([.87])
+
+    self.PlaceObserverPoles([0.3])
+
+    self.U_max = numpy.matrix([[12.0]])
+    self.U_min = numpy.matrix([[-12.0]])
+
+    qff_vel = 8.0
+    self.Qff = numpy.matrix([[1.0 / (qff_vel ** 2.0)]])
+
+    self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+
+
+class Shooter(VelocityShooter):
+  def __init__(self, name='Shooter'):
+    super(Shooter, self).__init__(name)
+
+    self.A_continuous_unaugmented = self.A_continuous
+    self.B_continuous_unaugmented = self.B_continuous
+
+    self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
+    self.A_continuous[1:2, 1:2] = self.A_continuous_unaugmented
+    self.A_continuous[0, 1] = 1
+
+    self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
+    self.B_continuous[1:2, 0] = self.B_continuous_unaugmented
+
+    # State feedback matrices
+    # [position, angular velocity]
     self.C = numpy.matrix([[1, 0]])
     self.D = numpy.matrix([[0]])
 
     self.A, self.B = self.ContinuousToDiscrete(
         self.A_continuous, self.B_continuous, self.dt)
 
-    self.PlaceControllerPoles([.6, .981])
-
     self.rpl = .45
     self.ipl = 0.07
     self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
                              self.rpl - 1j * self.ipl])
 
-    self.U_max = numpy.matrix([[12.0]])
-    self.U_min = numpy.matrix([[-12.0]])
+    self.K_unaugmented = self.K
+    self.K = numpy.matrix(numpy.zeros((1, 2)))
+    self.K[0, 1:2] = self.K_unaugmented
+    self.Kff_unaugmented = self.Kff
+    self.Kff = numpy.matrix(numpy.zeros((1, 2)))
+    self.Kff[0, 1:2] = self.Kff_unaugmented
+
+    self.InitializeState()
+
+
+class IntegralShooter(Shooter):
+  def __init__(self, name="IntegralShooter"):
+    super(IntegralShooter, self).__init__(name=name)
+
+    self.A_continuous_unaugmented = self.A_continuous
+    self.B_continuous_unaugmented = self.B_continuous
+
+    self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+    self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
+    self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
+
+    self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+    self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
+
+    self.C_unaugmented = self.C
+    self.C = numpy.matrix(numpy.zeros((1, 3)))
+    self.C[0:1, 0:2] = self.C_unaugmented
+
+    self.A, self.B = self.ContinuousToDiscrete(
+        self.A_continuous, self.B_continuous, self.dt)
+
+    q_pos = 0.08
+    q_vel = 4.00
+    q_voltage = 0.2
+    self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0],
+                           [0.0, (q_vel ** 2.0), 0.0],
+                           [0.0, 0.0, (q_voltage ** 2.0)]])
+
+    r_pos = 0.05
+    self.R = numpy.matrix([[(r_pos ** 2.0)]])
+
+    self.KalmanGain, self.Q_steady = controls.kalman(
+        A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+    self.L = self.A * self.KalmanGain
+
+    self.K_unaugmented = self.K
+    self.K = numpy.matrix(numpy.zeros((1, 3)))
+    self.K[0, 0:2] = self.K_unaugmented
+    self.K[0, 2] = 1
+    self.Kff_unaugmented = self.Kff
+    self.Kff = numpy.matrix(numpy.zeros((1, 3)))
+    self.Kff[0, 0:2] = self.Kff_unaugmented
+
+    self.InitializeState()
+
+
+class ScenarioPlotter(object):
+  def __init__(self):
+    # Various lists for graphing things.
+    self.t = []
+    self.x = []
+    self.v = []
+    self.a = []
+    self.x_hat = []
+    self.u = []
+    self.offset = []
+
+  def run_test(self, shooter, goal, iterations=200, controller_shooter=None,
+             observer_shooter=None):
+    """Runs the shooter plant with an initial condition and goal.
+
+      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:
+        shooter: shooter object to use.
+        goal: goal state.
+        iterations: Number of timesteps to run the model for.
+        controller_shooter: Wrist object to get K from, or None if we should
+            use shooter.
+        observer_shooter: Wrist object to use for the observer, or None if we should
+            use the actual state.
+    """
+
+    if controller_shooter is None:
+      controller_shooter = shooter
+
+    vbat = 12.0
+
+    if self.t:
+      initial_t = self.t[-1] + shooter.dt
+    else:
+      initial_t = 0
+
+    for i in xrange(iterations):
+      X_hat = shooter.X
+
+      if observer_shooter is not None:
+        X_hat = observer_shooter.X_hat
+        self.x_hat.append(observer_shooter.X_hat[1, 0])
+
+      ff_U = controller_shooter.Kff * (goal - observer_shooter.A * goal)
+
+      U = controller_shooter.K * (goal - X_hat) + ff_U
+      U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+      self.x.append(shooter.X[0, 0])
+
+
+      if self.v:
+        last_v = self.v[-1]
+      else:
+        last_v = 0
+
+      self.v.append(shooter.X[1, 0])
+      self.a.append((self.v[-1] - last_v) / shooter.dt)
+
+      if observer_shooter is not None:
+        observer_shooter.Y = shooter.Y
+        observer_shooter.CorrectObserver(U)
+        self.offset.append(observer_shooter.X_hat[2, 0])
+
+      applied_U = U.copy()
+      if i > 30:
+        applied_U += 2
+      shooter.Update(applied_U)
+
+      if observer_shooter is not None:
+        observer_shooter.PredictObserver(U)
+
+      self.t.append(initial_t + i * shooter.dt)
+      self.u.append(U[0, 0])
+
+      glog.debug('Time: %f', self.t[-1])
+
+  def Plot(self):
+    pylab.subplot(3, 1, 1)
+    pylab.plot(self.t, self.v, label='x')
+    pylab.plot(self.t, self.x_hat, label='x_hat')
+    pylab.legend()
+
+    pylab.subplot(3, 1, 2)
+    pylab.plot(self.t, self.u, label='u')
+    pylab.plot(self.t, self.offset, label='voltage_offset')
+    pylab.legend()
+
+    pylab.subplot(3, 1, 3)
+    pylab.plot(self.t, self.a, label='a')
+    pylab.legend()
+
+    pylab.show()
 
 
 def main(argv):
-  if len(argv) != 3:
-    print "Expected .h file name and .cc file name"
+  scenario_plotter = ScenarioPlotter()
+
+  shooter = Shooter()
+  shooter_controller = IntegralShooter()
+  observer_shooter = IntegralShooter()
+
+  # Test moving the shooter with constant separation.
+  initial_X = numpy.matrix([[0.0], [0.0]])
+  R = numpy.matrix([[0.0], [100.0], [0.0]])
+  scenario_plotter.run_test(shooter, goal=R, controller_shooter=shooter_controller,
+                            observer_shooter=observer_shooter, iterations=200)
+
+  if FLAGS.plot:
+    scenario_plotter.Plot()
+
+  if len(argv) != 5:
+    glog.fatal('Expected .h file name and .cc file name')
   else:
     namespaces = ['y2016', 'control_loops', 'shooter']
-    shooter = Shooter()
-    loop_writer = control_loop.ControlLoopWriter("Shooter", [shooter],
+    shooter = Shooter('Shooter')
+    loop_writer = control_loop.ControlLoopWriter('Shooter', [shooter],
                                                  namespaces=namespaces)
-    if argv[1][-3:] == '.cc':
-      loop_writer.Write(argv[2], argv[1])
-    else:
-      loop_writer.Write(argv[1], argv[2])
+    loop_writer.Write(argv[1], argv[2])
+
+    integral_shooter = IntegralShooter('IntegralShooter')
+    integral_loop_writer = control_loop.ControlLoopWriter(
+        'IntegralShooter', [integral_shooter], namespaces=namespaces)
+    integral_loop_writer.Write(argv[3], argv[4])
 
 
 if __name__ == '__main__':
-  sys.exit(main(sys.argv))
+  argv = FLAGS(sys.argv)
+  sys.exit(main(argv))
diff --git a/y2016/control_loops/python/wrist.py b/y2016/control_loops/python/wrist.py
index 2ad1d07..4d4f5f1 100755
--- a/y2016/control_loops/python/wrist.py
+++ b/y2016/control_loops/python/wrist.py
@@ -238,8 +238,6 @@
 
 
 def main(argv):
-  argv = FLAGS(argv)
-
   scenario_plotter = ScenarioPlotter()
 
   wrist = Wrist()
@@ -260,7 +258,7 @@
     glog.fatal('Expected .h file name and .cc file name for the wrist and integral wrist.')
   else:
     namespaces = ['y2016', 'control_loops', 'superstructure']
-    wrist = Wrist("Wrist")
+    wrist = Wrist('Wrist')
     loop_writer = control_loop.ControlLoopWriter(
         'Wrist', [wrist], namespaces=namespaces)
     loop_writer.Write(argv[1], argv[2])
@@ -271,4 +269,5 @@
     integral_loop_writer.Write(argv[3], argv[4])
 
 if __name__ == '__main__':
-  sys.exit(main(sys.argv))
+  argv = FLAGS(sys.argv)
+  sys.exit(main(argv))
diff --git a/y2016/control_loops/shooter/BUILD b/y2016/control_loops/shooter/BUILD
index cb3c0bc..c641a7d 100644
--- a/y2016/control_loops/shooter/BUILD
+++ b/y2016/control_loops/shooter/BUILD
@@ -23,6 +23,8 @@
   outs = [
     'shooter_plant.h',
     'shooter_plant.cc',
+    'shooter_integral_plant.h',
+    'shooter_integral_plant.cc',
   ],
 )
 
@@ -30,9 +32,11 @@
   name = 'shooter_plants',
   srcs = [
     'shooter_plant.cc',
+    'shooter_integral_plant.cc',
   ],
   hdrs = [
     'shooter_plant.h',
+    'shooter_integral_plant.h',
   ],
   deps = [
     '//frc971/control_loops:state_feedback_loop',
diff --git a/y2016/control_loops/shooter/shooter.cc b/y2016/control_loops/shooter/shooter.cc
index 9a46906..6550fc2 100644
--- a/y2016/control_loops/shooter/shooter.cc
+++ b/y2016/control_loops/shooter/shooter.cc
@@ -9,129 +9,92 @@
 
 namespace y2016 {
 namespace control_loops {
+namespace shooter {
+
+// TODO(austin): Pseudo current limit?
 
 ShooterSide::ShooterSide()
-    : loop_(new StateFeedbackLoop<2, 1, 1>(
-          ::y2016::control_loops::shooter::MakeShooterLoop())) {
-  memset(history_, 0, sizeof(history_));
+    : loop_(new StateFeedbackLoop<3, 1, 1>(MakeIntegralShooterLoop())) {
+  history_.fill(0);
+  Y_.setZero();
 }
 
-void ShooterSide::SetGoal(double angular_velocity_goal_uncapped) {
-  angular_velocity_goal_ = std::min(angular_velocity_goal_uncapped,
-                                    kMaxSpeed);
+void ShooterSide::set_goal(double angular_velocity_goal) {
+  loop_->mutable_next_R() << 0.0, angular_velocity_goal, 0.0;
 }
 
-void ShooterSide::EstimatePositionTimestep() {
-  // NULL position, so look at the loop.
-  SetPosition(loop_->X_hat(0, 0));
-}
-
-void ShooterSide::SetPosition(double current_position) {
-  current_position_ = current_position;
-
-  // Track the current position if the velocity goal is small.
-  if (angular_velocity_goal_ <= 1.0) position_goal_ = current_position_;
-
+void ShooterSide::set_position(double current_position) {
   // Update position in the model.
-  Eigen::Matrix<double, 1, 1> Y;
-  Y << current_position_;
-  loop_->Correct(Y);
-
-  // Prevents integral windup by limiting the position error such that the
-  // error can't produce much more than full power.
-  const double max_reference =
-      (loop_->U_max(0, 0) -
-       kAngularVelocityWeightScalar *
-           (angular_velocity_goal_ - loop_->X_hat(1, 0)) * loop_->K(0, 1)) /
-          loop_->K(0, 0) +
-      loop_->X_hat(0, 0);
-  const double min_reference =
-      (loop_->U_min(0, 0) -
-       kAngularVelocityWeightScalar *
-           (angular_velocity_goal_ - loop_->X_hat(1, 0)) * loop_->K(0, 1)) /
-          loop_->K(0, 0) +
-      loop_->X_hat(0, 0);
-  position_goal_ =
-      ::std::max(::std::min(position_goal_, max_reference), min_reference);
-
-  loop_->mutable_R() << position_goal_, angular_velocity_goal_;
-  position_goal_ +=
-      angular_velocity_goal_ * ::aos::controls::kLoopFrequency.ToSeconds();
+  Y_ << current_position;
 
   // Add the position to the history.
-  history_[history_position_] = current_position_;
+  history_[history_position_] = current_position;
   history_position_ = (history_position_ + 1) % kHistoryLength;
 }
 
-const ShooterStatus ShooterSide::GetStatus() {
-  // Calculate average over dt * kHistoryLength.
-  int old_history_position =
-      ((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
-  double avg_angular_velocity =
-      (history_[old_history_position] - history_[history_position_]) /
-      ::aos::controls::kLoopFrequency.ToSeconds() /
-      static_cast<double>(kHistoryLength - 1);
-
-  // Ready if average angular velocity is close to the goal.
-  bool ready = (std::abs(angular_velocity_goal_ - avg_angular_velocity) <
-                    kTolerance &&
-                angular_velocity_goal_ != 0.0);
-
-  return {avg_angular_velocity, ready};
-}
-
-double ShooterSide::GetOutput() {
-  if (angular_velocity_goal_ < 1.0) {
-    // Kill power at low angular velocities.
-    return 0.0;
-  }
-
+double ShooterSide::voltage() const {
   return loop_->U(0, 0);
 }
 
-void ShooterSide::UpdateLoop(bool output_is_null) {
-  loop_->Update(output_is_null);
+void ShooterSide::Update(bool disabled) {
+  loop_->mutable_R() = loop_->next_R();
+  if (loop_->R(1, 0) < 1.0) {
+    // Kill power at low angular velocities.
+    disabled = true;
+  }
+
+  loop_->Correct(Y_);
+  loop_->Update(disabled);
 }
 
-Shooter::Shooter(control_loops::ShooterQueue *my_shooter)
-    : aos::controls::ControlLoop<control_loops::ShooterQueue>(my_shooter) {}
+void ShooterSide::SetStatus(ShooterSideStatus *status) {
+  // Compute the oldest point in the history.
+  const int oldest_history_position =
+      ((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
 
-void Shooter::RunIteration(
-    const control_loops::ShooterQueue::Goal *goal,
-    const control_loops::ShooterQueue::Position *position,
-    control_loops::ShooterQueue::Output *output,
-    control_loops::ShooterQueue::Status *status) {
+  // Compute the distance moved over that time period.
+  status->avg_angular_velocity =
+      (history_[oldest_history_position] - history_[history_position_]) /
+      (::aos::controls::kLoopFrequency.ToSeconds() *
+       static_cast<double>(kHistoryLength - 1));
+
+  status->angular_velocity = loop_->X_hat(1, 0);
+
+  // Ready if average angular velocity is close to the goal.
+  status->ready = (std::abs(loop_->next_R(1, 0) -
+                            status->avg_angular_velocity) < kTolerance &&
+                   loop_->next_R(1, 0) > 1.0);
+}
+
+Shooter::Shooter(ShooterQueue *my_shooter)
+    : aos::controls::ControlLoop<ShooterQueue>(my_shooter) {}
+
+void Shooter::RunIteration(const ShooterQueue::Goal *goal,
+                           const ShooterQueue::Position *position,
+                           ShooterQueue::Output *output,
+                           ShooterQueue::Status *status) {
   if (goal) {
     // Update position/goal for our two shooter sides.
-    left_.SetGoal(goal->angular_velocity_left);
-    right_.SetGoal(goal->angular_velocity_right);
-
-    if (position == nullptr) {
-      left_.EstimatePositionTimestep();
-      right_.EstimatePositionTimestep();
-    } else {
-      left_.SetPosition(position->theta_left);
-      right_.SetPosition(position->theta_right);
-    }
+    left_.set_goal(goal->angular_velocity);
+    right_.set_goal(goal->angular_velocity);
   }
 
-  ShooterStatus status_left = left_.GetStatus();
-  ShooterStatus status_right = right_.GetStatus();
-  status->avg_angular_velocity_left = status_left.avg_angular_velocity;
-  status->avg_angular_velocity_right = status_right.avg_angular_velocity;
+  left_.set_position(position->theta_left);
+  right_.set_position(position->theta_right);
 
-  status->ready_left = status_left.ready;
-  status->ready_right = status_right.ready;
-  status->ready_both = (status_left.ready && status_right.ready);
+  left_.Update(output == nullptr);
+  right_.Update(output == nullptr);
 
-  left_.UpdateLoop(output == nullptr);
-  right_.UpdateLoop(output == nullptr);
+  left_.SetStatus(&status->left);
+  right_.SetStatus(&status->right);
+  status->ready = (status->left.ready && status->right.ready);
 
   if (output) {
-    output->voltage_left = left_.GetOutput();
-    output->voltage_right = right_.GetOutput();
+    output->voltage_left = left_.voltage();
+    output->voltage_right = right_.voltage();
   }
 }
 
+}  // namespace shooter
 }  // namespace control_loops
 }  // namespace y2016
diff --git a/y2016/control_loops/shooter/shooter.h b/y2016/control_loops/shooter/shooter.h
index 6a3fa4d..0354bb5 100644
--- a/y2016/control_loops/shooter/shooter.h
+++ b/y2016/control_loops/shooter/shooter.h
@@ -6,61 +6,59 @@
 #include "aos/common/controls/control_loop.h"
 #include "frc971/control_loops/state_feedback_loop.h"
 
-#include "y2016/control_loops/shooter/shooter_plant.h"
+#include "y2016/control_loops/shooter/shooter_integral_plant.h"
 #include "y2016/control_loops/shooter/shooter.q.h"
 
 namespace y2016 {
 namespace control_loops {
+namespace shooter {
 
 namespace {
-// TODO(constants): Update.
-const double kTolerance = 10.0;
-const double kMaxSpeed = 10000.0 * (2.0 * M_PI) / 60.0 * 15.0 / 34.0;
-const double kAngularVelocityWeightScalar = 0.35;
+constexpr double kTolerance = 10.0;
 }  // namespace
 
-struct ShooterStatus {
-  double avg_angular_velocity;
-  bool ready;
-};
-
 class ShooterSide {
  public:
   ShooterSide();
 
-  void SetGoal(double angular_velocity_goal);
-  void EstimatePositionTimestep();
-  void SetPosition(double current_position);
-  const ShooterStatus GetStatus();
-  double GetOutput();
-  void UpdateLoop(bool output_is_null);
+  // Sets the velocity goal in radians/sec
+  void set_goal(double angular_velocity_goal);
+  // Sets the current encoder position in radians
+  void set_position(double current_position);
+
+  // Populates the status structure.
+  void SetStatus(ShooterSideStatus *status);
+
+  // Returns the control loop calculated voltage.
+  double voltage() const;
+
+  // Executes the control loop for a cycle.
+  void Update(bool disabled);
 
  private:
-  ::std::unique_ptr<StateFeedbackLoop<2, 1, 1>> loop_;
-
-  double current_position_ = 0.0;
-  double position_goal_ = 0.0;
-  double angular_velocity_goal_ = 0.0;
+  // The current sensor measurement.
+  Eigen::Matrix<double, 1, 1> Y_;
+  // The control loop.
+  ::std::unique_ptr<StateFeedbackLoop<3, 1, 1>> loop_;
 
   // History array for calculating a filtered angular velocity.
-  static const int kHistoryLength = 5;
-  double history_[kHistoryLength];
+  static constexpr int kHistoryLength = 10;
+  ::std::array<double, kHistoryLength> history_;
   ptrdiff_t history_position_;
 
   DISALLOW_COPY_AND_ASSIGN(ShooterSide);
 };
 
-class Shooter
-    : public ::aos::controls::ControlLoop<control_loops::ShooterQueue> {
+class Shooter : public ::aos::controls::ControlLoop<ShooterQueue> {
  public:
-  explicit Shooter(control_loops::ShooterQueue *shooter_queue =
-                       &control_loops::shooter_queue);
+  explicit Shooter(
+      ShooterQueue *shooter_queue = &control_loops::shooter::shooter_queue);
 
  protected:
-  void RunIteration(const control_loops::ShooterQueue::Goal *goal,
-                    const control_loops::ShooterQueue::Position *position,
-                    control_loops::ShooterQueue::Output *output,
-                    control_loops::ShooterQueue::Status *status) override;
+  void RunIteration(const ShooterQueue::Goal *goal,
+                    const ShooterQueue::Position *position,
+                    ShooterQueue::Output *output,
+                    ShooterQueue::Status *status) override;
 
  private:
   ShooterSide left_, right_;
@@ -68,6 +66,7 @@
   DISALLOW_COPY_AND_ASSIGN(Shooter);
 };
 
+}  // namespace shooter
 }  // namespace control_loops
 }  // namespace y2016
 
diff --git a/y2016/control_loops/shooter/shooter.q b/y2016/control_loops/shooter/shooter.q
index 49575fb..af3f064 100644
--- a/y2016/control_loops/shooter/shooter.q
+++ b/y2016/control_loops/shooter/shooter.q
@@ -1,38 +1,47 @@
-package y2016.control_loops;
+package y2016.control_loops.shooter;
 
 import "aos/common/controls/control_loops.q";
 import "frc971/control_loops/control_loops.q";
 
+struct ShooterSideStatus {
+  // True if the shooter side is up to speed and stable.
+  bool ready;
+  // The current average velocity in radians/second.
+  double avg_angular_velocity;
+  // The current instantaneous filtered velocity in radians/second.
+  double angular_velocity;
+};
+
 queue_group ShooterQueue {
   implements aos.control_loops.ControlLoop;
 
-  // All angles are in radians, and angular velocities are in rad/sec. For all
-  // angular velocities, positive is shooting the ball out of the robot.
+  // All angles are in radians, and angular velocities are in radians/second.
+  // For all angular velocities, positive is shooting the ball out of the robot.
 
   message Goal {
-    // Angular velocity goals in rad/sec.
-    double angular_velocity_left;
-    double angular_velocity_right;
+    // Angular velocity goals in radians/second.
+    double angular_velocity;
   };
 
   message Position {
-    // Wheel angle in rad/sec.
+    // Wheel angle in radians/second.
     double theta_left;
     double theta_right;
   };
 
   message Status {
-    bool ready_left;
-    bool ready_right;
-    // Is the shooter ready to shoot?
-    bool ready_both;
+    // Left side status.
+    ShooterSideStatus left;
+    // Right side status.
+    ShooterSideStatus right;
 
-    // Average angular velocities over dt * kHistoryLength.
-    double avg_angular_velocity_left;
-    double avg_angular_velocity_right;
+    // True if the shooter is ready.  It is better to compare the velocities
+    // directly so there isn't confusion on if the goal is up to date.
+    bool ready;
   };
 
   message Output {
+    // Voltage in volts of the left and right shooter motors.
     double voltage_left;
     double voltage_right;
   };
diff --git a/y2016/control_loops/shooter/shooter_lib_test.cc b/y2016/control_loops/shooter/shooter_lib_test.cc
index 9ec0334..8d0ae39 100644
--- a/y2016/control_loops/shooter/shooter_lib_test.cc
+++ b/y2016/control_loops/shooter/shooter_lib_test.cc
@@ -10,23 +10,45 @@
 #include "frc971/control_loops/team_number_test_environment.h"
 #include "y2016/control_loops/shooter/shooter.q.h"
 #include "y2016/control_loops/shooter/shooter.h"
+#include "y2016/control_loops/shooter/shooter_plant.h"
 
 using ::aos::time::Time;
 using ::frc971::control_loops::testing::kTeamNumber;
 
 namespace y2016 {
 namespace control_loops {
+namespace shooter {
 namespace testing {
 
+class ShooterPlant : public StateFeedbackPlant<2, 1, 1> {
+ public:
+  explicit ShooterPlant(StateFeedbackPlant<2, 1, 1> &&other)
+      : StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
+
+  void CheckU() override {
+    assert(U(0, 0) <= U_max(0, 0) + 0.00001 + voltage_offset_);
+    assert(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 which simulates the shooter and sends out queue messages with the
 // position.
 class ShooterSimulation {
  public:
   // Constructs a shooter simulation.
   ShooterSimulation()
-      : shooter_plant_left_(new StateFeedbackPlant<2, 1, 1>(
+      : shooter_plant_left_(new ShooterPlant(
             ::y2016::control_loops::shooter::MakeShooterPlant())),
-        shooter_plant_right_(new StateFeedbackPlant<2, 1, 1>(
+        shooter_plant_right_(new ShooterPlant(
             ::y2016::control_loops::shooter::MakeShooterPlant())),
         shooter_queue_(".y2016.control_loops.shooter", 0x78d8e372,
                        ".y2016.control_loops.shooter.goal",
@@ -36,7 +58,7 @@
 
   // Sends a queue message with the position of the shooter.
   void SendPositionMessage() {
-    ::aos::ScopedMessagePtr<control_loops::ShooterQueue::Position> position =
+    ::aos::ScopedMessagePtr<ShooterQueue::Position> position =
         shooter_queue_.position.MakeMessage();
 
     position->theta_left = shooter_plant_left_->Y(0, 0);
@@ -44,20 +66,30 @@
     position.Send();
   }
 
+  void set_left_voltage_offset(double voltage_offset) {
+    shooter_plant_left_->set_voltage_offset(voltage_offset);
+  }
+
+  void set_right_voltage_offset(double voltage_offset) {
+    shooter_plant_right_->set_voltage_offset(voltage_offset);
+  }
+
   // Simulates shooter for a single timestep.
   void Simulate() {
     EXPECT_TRUE(shooter_queue_.output.FetchLatest());
 
-    shooter_plant_left_->mutable_U(0, 0) = shooter_queue_.output->voltage_left;
+    shooter_plant_left_->mutable_U(0, 0) =
+        shooter_queue_.output->voltage_left +
+        shooter_plant_left_->voltage_offset();
     shooter_plant_right_->mutable_U(0, 0) =
-        shooter_queue_.output->voltage_right;
+        shooter_queue_.output->voltage_right +
+        shooter_plant_right_->voltage_offset();
 
     shooter_plant_left_->Update();
     shooter_plant_right_->Update();
   }
 
-  ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> shooter_plant_left_,
-      shooter_plant_right_;
+  ::std::unique_ptr<ShooterPlant> shooter_plant_left_, shooter_plant_right_;
 
  private:
   ShooterQueue shooter_queue_;
@@ -83,10 +115,15 @@
     EXPECT_TRUE(shooter_queue_.goal.get() != nullptr);
     EXPECT_TRUE(shooter_queue_.status.get() != nullptr);
 
-    EXPECT_NEAR(shooter_queue_.goal->angular_velocity_left,
-                shooter_queue_.status->avg_angular_velocity_left, 10.0);
-    EXPECT_NEAR(shooter_queue_.goal->angular_velocity_right,
-                shooter_queue_.status->avg_angular_velocity_right, 10.0);
+    EXPECT_NEAR(shooter_queue_.goal->angular_velocity,
+                shooter_queue_.status->left.angular_velocity, 10.0);
+    EXPECT_NEAR(shooter_queue_.goal->angular_velocity,
+                shooter_queue_.status->right.angular_velocity, 10.0);
+
+    EXPECT_NEAR(shooter_queue_.goal->angular_velocity,
+                shooter_queue_.status->left.avg_angular_velocity, 10.0);
+    EXPECT_NEAR(shooter_queue_.goal->angular_velocity,
+                shooter_queue_.status->right.avg_angular_velocity, 10.0);
   }
 
   // Runs one iteration of the whole simulation.
@@ -121,8 +158,7 @@
 // Tests that the shooter does nothing when the goal is zero.
 TEST_F(ShooterTest, DoesNothing) {
   ASSERT_TRUE(shooter_queue_.goal.MakeWithBuilder()
-                  .angular_velocity_left(0.0)
-                  .angular_velocity_right(0.0)
+                  .angular_velocity(0.0)
                   .Send());
   RunIteration();
 
@@ -137,17 +173,13 @@
 // without applying any power.
 TEST_F(ShooterTest, SpinUpAndDown) {
   // Spin up.
-  EXPECT_TRUE(shooter_queue_.goal.MakeWithBuilder()
-                  .angular_velocity_left(450.0)
-                  .angular_velocity_right(450.0)
-                  .Send());
-  RunForTime(Time::InSeconds(10));
+  EXPECT_TRUE(
+      shooter_queue_.goal.MakeWithBuilder().angular_velocity(450.0).Send());
+  RunForTime(Time::InSeconds(1));
   VerifyNearGoal();
-  EXPECT_TRUE(shooter_queue_.status->ready_both);
-  EXPECT_TRUE(shooter_queue_.goal.MakeWithBuilder()
-                  .angular_velocity_left(0.0)
-                  .angular_velocity_right(0.0)
-                  .Send());
+  EXPECT_TRUE(shooter_queue_.status->ready);
+  EXPECT_TRUE(
+      shooter_queue_.goal.MakeWithBuilder().angular_velocity(0.0).Send());
 
   // Make sure we don't apply voltage on spin-down.
   RunIteration();
@@ -167,10 +199,10 @@
 // mistakenly say that we are ready. This test should look at both extremes.
 TEST_F(ShooterTest, SideLagTest) {
   // Spin up.
-  EXPECT_TRUE(shooter_queue_.goal.MakeWithBuilder()
-                  .angular_velocity_left(20.0)
-                  .angular_velocity_right(450.0)
-                  .Send());
+  EXPECT_TRUE(
+      shooter_queue_.goal.MakeWithBuilder().angular_velocity(20.0).Send());
+  // Cause problems by adding a voltage error on one side.
+  shooter_plant_.set_right_voltage_offset(-4.0);
   RunForTime(Time::InSeconds(0.1));
 
   shooter_queue_.goal.FetchLatest();
@@ -180,9 +212,9 @@
   EXPECT_TRUE(shooter_queue_.status.get() != nullptr);
 
   // Left should be up to speed, right shouldn't.
-  EXPECT_TRUE(shooter_queue_.status->ready_left);
-  EXPECT_FALSE(shooter_queue_.status->ready_right);
-  EXPECT_FALSE(shooter_queue_.status->ready_both);
+  EXPECT_TRUE(shooter_queue_.status->left.ready);
+  EXPECT_FALSE(shooter_queue_.status->right.ready);
+  EXPECT_FALSE(shooter_queue_.status->ready);
 
   RunForTime(Time::InSeconds(5));
 
@@ -193,71 +225,25 @@
   EXPECT_TRUE(shooter_queue_.status.get() != nullptr);
 
   // Both should be up to speed.
-  EXPECT_TRUE(shooter_queue_.status->ready_left);
-  EXPECT_TRUE(shooter_queue_.status->ready_right);
-  EXPECT_TRUE(shooter_queue_.status->ready_both);
+  EXPECT_TRUE(shooter_queue_.status->left.ready);
+  EXPECT_TRUE(shooter_queue_.status->right.ready);
+  EXPECT_TRUE(shooter_queue_.status->ready);
 }
 
-// Test to make sure that it does not exceed the encoder's rated speed.
-TEST_F(ShooterTest, RateLimitTest) {
-  ASSERT_TRUE(shooter_queue_.goal.MakeWithBuilder()
-                  .angular_velocity_left(1000.0)
-                  .angular_velocity_right(1000.0)
-                  .Send());
-
-  RunForTime(Time::InSeconds(10));
-  EXPECT_TRUE(shooter_queue_.status.FetchLatest());
-  EXPECT_TRUE(shooter_queue_.status->ready_both);
-
-  shooter_queue_.goal.FetchLatest();
-  shooter_queue_.status.FetchLatest();
-
-  EXPECT_TRUE(shooter_queue_.goal.get() != nullptr);
-  EXPECT_TRUE(shooter_queue_.status.get() != nullptr);
-
-  EXPECT_GT(::y2016::control_loops::kMaxSpeed,
-            shooter_queue_.status->avg_angular_velocity_left);
-
-  EXPECT_GT(::y2016::control_loops::kMaxSpeed,
-            shooter_queue_.status->avg_angular_velocity_right);
-}
-
-// Tests that the shooter can spin up nicely while missing position packets.
-TEST_F(ShooterTest, MissingPositionMessages) {
-  ASSERT_TRUE(shooter_queue_.goal.MakeWithBuilder()
-                  .angular_velocity_left(200.0)
-                  .angular_velocity_right(200.0)
-                  .Send());
-  for (int i = 0; i < 100; ++i) {
-    if (i % 7) {
-      shooter_plant_.SendPositionMessage();
-    }
-
-    RunIteration();
-  }
-
-  VerifyNearGoal();
-}
-
-// Tests that the shooter can spin up nicely while disabled for a while.
-// TODO(comran): Update this test, since it's the same as
-// MissingPositionMessages.
+// Tests that the shooter can spin up nicely after being disabled for a while.
 TEST_F(ShooterTest, Disabled) {
   ASSERT_TRUE(shooter_queue_.goal.MakeWithBuilder()
-                  .angular_velocity_left(200.0)
-                  .angular_velocity_right(200.0)
+                  .angular_velocity(200.0)
                   .Send());
-  for (int i = 0; i < 100; ++i) {
-    if (i % 7) {
-      shooter_plant_.SendPositionMessage();
-    }
+  RunForTime(Time::InSeconds(5), false);
+  EXPECT_EQ(nullptr, shooter_queue_.output.get());
 
-    RunIteration();
-  }
+  RunForTime(Time::InSeconds(5));
 
   VerifyNearGoal();
 }
 
 }  // namespace testing
+}  // namespace shooter
 }  // namespace control_loops
 }  // namespace y2016
diff --git a/y2016/control_loops/shooter/shooter_main.cc b/y2016/control_loops/shooter/shooter_main.cc
index 911ab29..4036c5f 100644
--- a/y2016/control_loops/shooter/shooter_main.cc
+++ b/y2016/control_loops/shooter/shooter_main.cc
@@ -4,7 +4,7 @@
 
 int main() {
   ::aos::Init();
-  ::y2016::control_loops::Shooter shooter;
+  ::y2016::control_loops::shooter::Shooter shooter;
   shooter.Run();
   ::aos::Cleanup();
   return 0;