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))