diff --git a/y2016/control_loops/python/BUILD b/y2016/control_loops/python/BUILD
index 0134379..d3245a9 100644
--- a/y2016/control_loops/python/BUILD
+++ b/y2016/control_loops/python/BUILD
@@ -39,11 +39,38 @@
 )
 
 py_binary(
-  name = 'shooter',
+  name = 'intake',
   srcs = [
-    'shooter.py',
+    'intake.py',
   ],
   deps = [
+    ':polydrivetrain_lib',
+    '//external:python-gflags',
+    '//external:python-glog',
+    '//frc971/control_loops/python:controls',
+  ]
+)
+
+py_binary(
+  name = 'shoulder',
+  srcs = [
+    'shoulder.py',
+  ],
+  deps = [
+    ':polydrivetrain_lib',
+    '//external:python-gflags',
+    '//external:python-glog',
+    '//frc971/control_loops/python:controls',
+  ]
+)
+
+py_binary(
+  name = 'wrist',
+  srcs = [
+    'wrist.py',
+  ],
+  deps = [
+    ':polydrivetrain_lib',
     '//external:python-gflags',
     '//external:python-glog',
     '//frc971/control_loops/python:controls',
diff --git a/y2016/control_loops/python/intake.py b/y2016/control_loops/python/intake.py
new file mode 100755
index 0000000..b5e61c1
--- /dev/null
+++ b/y2016/control_loops/python/intake.py
@@ -0,0 +1,287 @@
+#!/usr/bin/python
+
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
+from frc971.control_loops.python import polytope
+from y2016.control_loops.python import polydrivetrain
+import numpy
+import sys
+import matplotlib
+from matplotlib import pylab
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+try:
+  gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+  pass
+
+class Intake(control_loop.ControlLoop):
+  def __init__(self, name="Intake", mass=None):
+    super(Intake, self).__init__(name)
+    # TODO(constants): Update all of these & retune poles.
+    # Stall Torque in N m
+    self.stall_torque = 0.476
+    # Stall Current in Amps
+    self.stall_current = 80.730
+    # Free Speed in RPM
+    self.free_speed = 13906.0
+    # Free Current in Amps
+    self.free_current = 5.820
+    # Mass of the intake
+    if mass is None:
+      self.mass = 5.0
+    else:
+      self.mass = mass
+
+    # 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
+    self.G = (56.0 / 12.0) * (54.0 / 14.0) * (64.0 / 14.0) * (72.0 / 18.0)
+    # Intake length
+    self.r = 18 * 0.0254
+
+    self.J = self.r * self.mass
+
+    # Control loop time step
+    self.dt = 0.005
+
+    # State is [position, velocity]
+    # Input is [Voltage]
+
+    C1 = self.G * self.G * self.Kt / (self.R  * self.J * self.Kv)
+    C2 = self.Kt * self.G / (self.J * self.R)
+
+    self.A_continuous = numpy.matrix(
+        [[0, 1],
+         [0, -C1]])
+
+    # Start with the unmodified input
+    self.B_continuous = numpy.matrix(
+        [[0],
+         [C2]])
+
+    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)
+
+    controllability = controls.ctrb(self.A, self.B)
+
+    print "Free speed is", self.free_speed * numpy.pi * 2.0 / 60.0 / self.G
+
+    q_pos = 0.15
+    q_vel = 2.5
+    self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0],
+                           [0.0, (1.0 / (q_vel ** 2.0))]])
+
+    self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0))]])
+    self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+
+    print 'K', self.K
+    print 'Poles are', numpy.linalg.eig(self.A - self.B * self.K)[0]
+
+    self.rpl = 0.30
+    self.ipl = 0.10
+    self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+                             self.rpl - 1j * self.ipl])
+
+    print 'L is', self.L
+
+    q_pos = 0.05
+    q_vel = 2.65
+    self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0],
+                           [0.0, (q_vel ** 2.0)]])
+
+    r_volts = 0.025
+    self.R = numpy.matrix([[(r_volts ** 2.0)]])
+
+    self.KalmanGain, self.Q_steady = controls.kalman(
+        A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+
+    print 'Kal', self.KalmanGain
+    self.L = self.A * self.KalmanGain
+    print 'KalL is', self.L
+
+    # 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]])
+    self.U_min = numpy.matrix([[-12.0]])
+
+    self.InitializeState()
+
+class IntegralIntake(Intake):
+  def __init__(self, name="IntegralIntake", mass=None):
+    super(IntegralIntake, self).__init__(name=name, mass=mass)
+
+    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 = 6.0
+    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.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 = []
+
+  def run_test(self, intake, goal, iterations=200, controller_intake=None,
+             observer_intake=None):
+    """Runs the intake 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:
+        intake: intake object to use.
+        goal: goal state.
+        iterations: Number of timesteps to run the model for.
+        controller_intake: Intake object to get K from, or None if we should
+            use intake.
+        observer_intake: Intake object to use for the observer, or None if we should
+            use the actual state.
+    """
+
+    if controller_intake is None:
+      controller_intake = intake
+
+    vbat = 12.0
+
+    if self.t:
+      initial_t = self.t[-1] + intake.dt
+    else:
+      initial_t = 0
+
+    for i in xrange(iterations):
+      X_hat = intake.X
+
+      if observer_intake is not None:
+        X_hat = observer_intake.X_hat
+        self.x_hat.append(observer_intake.X_hat[0, 0])
+
+      U = controller_intake.K * (goal - X_hat)
+      U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+      self.x.append(intake.X[0, 0])
+
+      if self.v:
+        last_v = self.v[-1]
+      else:
+        last_v = 0
+
+      self.v.append(intake.X[1, 0])
+      self.a.append((self.v[-1] - last_v) / intake.dt)
+
+      if observer_intake is not None:
+        observer_intake.Y = intake.Y
+        observer_intake.CorrectObserver(U)
+
+      intake.Update(U)
+
+      if observer_intake is not None:
+        observer_intake.PredictObserver(U)
+
+      self.t.append(initial_t + i * intake.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.x, 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.subplot(3, 1, 3)
+    pylab.plot(self.t, self.a, label='a')
+
+    pylab.legend()
+    pylab.show()
+
+
+def main(argv):
+  argv = FLAGS(argv)
+
+  base_mass = 4
+  load_mass = 0
+
+  scenario_plotter = ScenarioPlotter()
+
+  intake = Intake(mass=base_mass + load_mass)
+  intake_controller = IntegralIntake(mass=base_mass + load_mass)
+  observer_intake = IntegralIntake(mass=base_mass + load_mass)
+
+  # Test moving the intake with constant separation.
+  initial_X = numpy.matrix([[0.0], [0.0]])
+  R = numpy.matrix([[1.0], [0.0], [0.0]])
+  scenario_plotter.run_test(intake, goal=R, controller_intake=intake_controller,
+                            observer_intake=observer_intake, iterations=200)
+
+  if FLAGS.plot:
+    scenario_plotter.Plot()
+
+  # Write the generated constants out to a file.
+  if len(argv) != 5:
+    glog.fatal('Expected .h file name and .cc file name for the intake and integral intake.')
+  else:
+    namespaces = ['y2016', 'control_loops', 'superstructure']
+    intake = Intake("Intake")
+    loop_writer = control_loop.ControlLoopWriter('Intake', [intake],
+                                                 namespaces=namespaces)
+    loop_writer.Write(argv[1], argv[2])
+
+    integral_intake = IntegralIntake("IntegralIntake", mass=base_mass + load_mass)
+    integral_loop_writer = control_loop.ControlLoopWriter("IntegralIntake", [integral_intake],
+                                                          namespaces=['y2016', 'control_loops', 'superstructure'])
+    integral_loop_writer.Write(argv[3], argv[4])
+
+if __name__ == '__main__':
+  sys.exit(main(sys.argv))
diff --git a/y2016/control_loops/python/shoulder.py b/y2016/control_loops/python/shoulder.py
new file mode 100755
index 0000000..05ab1b6
--- /dev/null
+++ b/y2016/control_loops/python/shoulder.py
@@ -0,0 +1,287 @@
+#!/usr/bin/python
+
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
+from frc971.control_loops.python import polytope
+from y2016.control_loops.python import polydrivetrain
+import numpy
+import sys
+import matplotlib
+from matplotlib import pylab
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+try:
+  gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+  pass
+
+class Shoulder(control_loop.ControlLoop):
+  def __init__(self, name="Shoulder", mass=None):
+    super(Shoulder, self).__init__(name)
+    # TODO(constants): Update all of these & retune poles.
+    # Stall Torque in N m
+    self.stall_torque = 0.476
+    # Stall Current in Amps
+    self.stall_current = 80.730
+    # Free Speed in RPM
+    self.free_speed = 13906.0
+    # Free Current in Amps
+    self.free_current = 5.820
+    # Mass of the shoulder
+    if mass is None:
+      self.mass = 5.0
+    else:
+      self.mass = mass
+
+    # 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
+    self.G = (56.0 / 12.0) * (54.0 / 14.0) * (64.0 / 14.0) * (72.0 / 18.0)
+    # Shoulder length
+    self.r = 18 * 0.0254
+
+    self.J = self.r * self.mass
+
+    # Control loop time step
+    self.dt = 0.005
+
+    # State is [position, velocity]
+    # Input is [Voltage]
+
+    C1 = self.G * self.G * self.Kt / (self.R  * self.J * self.Kv)
+    C2 = self.Kt * self.G / (self.J * self.R)
+
+    self.A_continuous = numpy.matrix(
+        [[0, 1],
+         [0, -C1]])
+
+    # Start with the unmodified input
+    self.B_continuous = numpy.matrix(
+        [[0],
+         [C2]])
+
+    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)
+
+    controllability = controls.ctrb(self.A, self.B)
+
+    print "Free speed is", self.free_speed * numpy.pi * 2.0 / 60.0 / self.G
+
+    q_pos = 0.15
+    q_vel = 2.5
+    self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0],
+                           [0.0, (1.0 / (q_vel ** 2.0))]])
+
+    self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0))]])
+    self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+
+    print 'K', self.K
+    print 'Poles are', numpy.linalg.eig(self.A - self.B * self.K)[0]
+
+    self.rpl = 0.30
+    self.ipl = 0.10
+    self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+                             self.rpl - 1j * self.ipl])
+
+    print 'L is', self.L
+
+    q_pos = 0.05
+    q_vel = 2.65
+    self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0],
+                           [0.0, (q_vel ** 2.0)]])
+
+    r_volts = 0.025
+    self.R = numpy.matrix([[(r_volts ** 2.0)]])
+
+    self.KalmanGain, self.Q_steady = controls.kalman(
+        A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+
+    print 'Kal', self.KalmanGain
+    self.L = self.A * self.KalmanGain
+    print 'KalL is', self.L
+
+    # 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]])
+    self.U_min = numpy.matrix([[-12.0]])
+
+    self.InitializeState()
+
+class IntegralShoulder(Shoulder):
+  def __init__(self, name="IntegralShoulder", mass=None):
+    super(IntegralShoulder, self).__init__(name=name, mass=mass)
+
+    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 = 6.0
+    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.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 = []
+
+  def run_test(self, shoulder, goal, iterations=200, controller_shoulder=None,
+             observer_shoulder=None):
+    """Runs the shoulder 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:
+        shoulder: shoulder object to use.
+        goal: goal state.
+        iterations: Number of timesteps to run the model for.
+        controller_shoulder: Shoulder object to get K from, or None if we should
+            use shoulder.
+        observer_shoulder: Shoulder object to use for the observer, or None if we should
+            use the actual state.
+    """
+
+    if controller_shoulder is None:
+      controller_shoulder = shoulder
+
+    vbat = 12.0
+
+    if self.t:
+      initial_t = self.t[-1] + shoulder.dt
+    else:
+      initial_t = 0
+
+    for i in xrange(iterations):
+      X_hat = shoulder.X
+
+      if observer_shoulder is not None:
+        X_hat = observer_shoulder.X_hat
+        self.x_hat.append(observer_shoulder.X_hat[0, 0])
+
+      U = controller_shoulder.K * (goal - X_hat)
+      U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+      self.x.append(shoulder.X[0, 0])
+
+      if self.v:
+        last_v = self.v[-1]
+      else:
+        last_v = 0
+
+      self.v.append(shoulder.X[1, 0])
+      self.a.append((self.v[-1] - last_v) / shoulder.dt)
+
+      if observer_shoulder is not None:
+        observer_shoulder.Y = shoulder.Y
+        observer_shoulder.CorrectObserver(U)
+
+      shoulder.Update(U)
+
+      if observer_shoulder is not None:
+        observer_shoulder.PredictObserver(U)
+
+      self.t.append(initial_t + i * shoulder.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.x, 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.subplot(3, 1, 3)
+    pylab.plot(self.t, self.a, label='a')
+
+    pylab.legend()
+    pylab.show()
+
+
+def main(argv):
+  argv = FLAGS(argv)
+
+  base_mass = 4
+  load_mass = 0
+
+  scenario_plotter = ScenarioPlotter()
+
+  shoulder = Shoulder(mass=base_mass + load_mass)
+  shoulder_controller = IntegralShoulder(mass=base_mass + load_mass)
+  observer_shoulder = IntegralShoulder(mass=base_mass + load_mass)
+
+  # Test moving the shoulder with constant separation.
+  initial_X = numpy.matrix([[0.0], [0.0]])
+  R = numpy.matrix([[1.0], [0.0], [0.0]])
+  scenario_plotter.run_test(shoulder, goal=R, controller_shoulder=shoulder_controller,
+                            observer_shoulder=observer_shoulder, iterations=200)
+
+  if FLAGS.plot:
+    scenario_plotter.Plot()
+
+  # Write the generated constants out to a file.
+  if len(argv) != 5:
+    glog.fatal('Expected .h file name and .cc file name for the shoulder and integral shoulder.')
+  else:
+    namespaces = ['y2016', 'control_loops', 'superstructure']
+    shoulder = Shoulder("Shoulder")
+    loop_writer = control_loop.ControlLoopWriter('Shoulder', [shoulder],
+                                                 namespaces=namespaces)
+    loop_writer.Write(argv[1], argv[2])
+
+    integral_shoulder = IntegralShoulder("IntegralShoulder", mass=base_mass + load_mass)
+    integral_loop_writer = control_loop.ControlLoopWriter("IntegralShoulder", [integral_shoulder],
+                                                          namespaces=['y2016', 'control_loops', 'superstructure'])
+    integral_loop_writer.Write(argv[3], argv[4])
+
+if __name__ == '__main__':
+  sys.exit(main(sys.argv))
diff --git a/y2016/control_loops/python/wrist.py b/y2016/control_loops/python/wrist.py
new file mode 100755
index 0000000..7498cc4
--- /dev/null
+++ b/y2016/control_loops/python/wrist.py
@@ -0,0 +1,287 @@
+#!/usr/bin/python
+
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
+from frc971.control_loops.python import polytope
+from y2016.control_loops.python import polydrivetrain
+import numpy
+import sys
+import matplotlib
+from matplotlib import pylab
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+try:
+  gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+  pass
+
+class Wrist(control_loop.ControlLoop):
+  def __init__(self, name="Wrist", mass=None):
+    super(Wrist, self).__init__(name)
+    # TODO(constants): Update all of these & retune poles.
+    # Stall Torque in N m
+    self.stall_torque = 0.476
+    # Stall Current in Amps
+    self.stall_current = 80.730
+    # Free Speed in RPM
+    self.free_speed = 13906.0
+    # Free Current in Amps
+    self.free_current = 5.820
+    # Mass of the wrist
+    if mass is None:
+      self.mass = 5.0
+    else:
+      self.mass = mass
+
+    # 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
+    self.G = (56.0 / 12.0) * (54.0 / 14.0) * (64.0 / 14.0) * (72.0 / 18.0)
+    # Wrist length
+    self.r = 18 * 0.0254
+
+    self.J = self.r * self.mass
+
+    # Control loop time step
+    self.dt = 0.005
+
+    # State is [position, velocity]
+    # Input is [Voltage]
+
+    C1 = self.G * self.G * self.Kt / (self.R  * self.J * self.Kv)
+    C2 = self.Kt * self.G / (self.J * self.R)
+
+    self.A_continuous = numpy.matrix(
+        [[0, 1],
+         [0, -C1]])
+
+    # Start with the unmodified input
+    self.B_continuous = numpy.matrix(
+        [[0],
+         [C2]])
+
+    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)
+
+    controllability = controls.ctrb(self.A, self.B)
+
+    print "Free speed is", self.free_speed * numpy.pi * 2.0 / 60.0 / self.G
+
+    q_pos = 0.15
+    q_vel = 2.5
+    self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0],
+                           [0.0, (1.0 / (q_vel ** 2.0))]])
+
+    self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0))]])
+    self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+
+    print 'K', self.K
+    print 'Poles are', numpy.linalg.eig(self.A - self.B * self.K)[0]
+
+    self.rpl = 0.30
+    self.ipl = 0.10
+    self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+                             self.rpl - 1j * self.ipl])
+
+    print 'L is', self.L
+
+    q_pos = 0.05
+    q_vel = 2.65
+    self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0],
+                           [0.0, (q_vel ** 2.0)]])
+
+    r_volts = 0.025
+    self.R = numpy.matrix([[(r_volts ** 2.0)]])
+
+    self.KalmanGain, self.Q_steady = controls.kalman(
+        A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+
+    print 'Kal', self.KalmanGain
+    self.L = self.A * self.KalmanGain
+    print 'KalL is', self.L
+
+    # 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]])
+    self.U_min = numpy.matrix([[-12.0]])
+
+    self.InitializeState()
+
+class IntegralWrist(Wrist):
+  def __init__(self, name="IntegralWrist", mass=None):
+    super(IntegralWrist, self).__init__(name=name, mass=mass)
+
+    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 = 6.0
+    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.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 = []
+
+  def run_test(self, wrist, goal, iterations=200, controller_wrist=None,
+             observer_wrist=None):
+    """Runs the wrist 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:
+        wrist: wrist object to use.
+        goal: goal state.
+        iterations: Number of timesteps to run the model for.
+        controller_wrist: Wrist object to get K from, or None if we should
+            use wrist.
+        observer_wrist: Wrist object to use for the observer, or None if we should
+            use the actual state.
+    """
+
+    if controller_wrist is None:
+      controller_wrist = wrist
+
+    vbat = 12.0
+
+    if self.t:
+      initial_t = self.t[-1] + wrist.dt
+    else:
+      initial_t = 0
+
+    for i in xrange(iterations):
+      X_hat = wrist.X
+
+      if observer_wrist is not None:
+        X_hat = observer_wrist.X_hat
+        self.x_hat.append(observer_wrist.X_hat[0, 0])
+
+      U = controller_wrist.K * (goal - X_hat)
+      U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+      self.x.append(wrist.X[0, 0])
+
+      if self.v:
+        last_v = self.v[-1]
+      else:
+        last_v = 0
+
+      self.v.append(wrist.X[1, 0])
+      self.a.append((self.v[-1] - last_v) / wrist.dt)
+
+      if observer_wrist is not None:
+        observer_wrist.Y = wrist.Y
+        observer_wrist.CorrectObserver(U)
+
+      wrist.Update(U)
+
+      if observer_wrist is not None:
+        observer_wrist.PredictObserver(U)
+
+      self.t.append(initial_t + i * wrist.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.x, 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.subplot(3, 1, 3)
+    pylab.plot(self.t, self.a, label='a')
+
+    pylab.legend()
+    pylab.show()
+
+
+def main(argv):
+  argv = FLAGS(argv)
+
+  base_mass = 4
+  load_mass = 0
+
+  scenario_plotter = ScenarioPlotter()
+
+  wrist = Wrist(mass=base_mass + load_mass)
+  wrist_controller = IntegralWrist(mass=base_mass + load_mass)
+  observer_wrist = IntegralWrist(mass=base_mass + load_mass)
+
+  # Test moving the wrist with constant separation.
+  initial_X = numpy.matrix([[0.0], [0.0]])
+  R = numpy.matrix([[1.0], [0.0], [0.0]])
+  scenario_plotter.run_test(wrist, goal=R, controller_wrist=wrist_controller,
+                            observer_wrist=observer_wrist, iterations=200)
+
+  if FLAGS.plot:
+    scenario_plotter.Plot()
+
+  # Write the generated constants out to a file.
+  if len(argv) != 5:
+    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")
+    loop_writer = control_loop.ControlLoopWriter('Wrist', [wrist],
+                                                 namespaces=namespaces)
+    loop_writer.Write(argv[1], argv[2])
+
+    integral_wrist = IntegralWrist("IntegralWrist", mass=base_mass + load_mass)
+    integral_loop_writer = control_loop.ControlLoopWriter("IntegralWrist", [integral_wrist],
+                                                          namespaces=['y2016', 'control_loops', 'superstructure'])
+    integral_loop_writer.Write(argv[3], argv[4])
+
+if __name__ == '__main__':
+  sys.exit(main(sys.argv))