diff --git a/y2014/control_loops/python/arm.py b/y2014/control_loops/python/arm.py
new file mode 100755
index 0000000..e17990a
--- /dev/null
+++ b/y2014/control_loops/python/arm.py
@@ -0,0 +1,409 @@
+#!/usr/bin/python
+
+import control_loop
+import controls
+import polytope
+import polydrivetrain
+import numpy
+import math
+import sys
+import matplotlib
+from matplotlib import pylab
+
+
+class Arm(control_loop.ControlLoop):
+  def __init__(self, name="Arm", mass=None):
+    super(Arm, self).__init__(name)
+    # 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 arm
+    if mass is None:
+      self.mass = 13.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 = (44.0 / 12.0) * (54.0 / 14.0) * (54.0 / 14.0) * (44.0 / 20.0) * (72.0 / 16.0)
+    # Fridge arm length
+    self.r = 32 * 0.0254
+    # Control loop time step
+    self.dt = 0.005
+
+    # Arm moment of inertia
+    self.J = self.r * self.mass
+
+    # Arm left/right spring constant (N*m / radian)
+    self.spring = 100.0
+
+    # State is [average position, average velocity,
+    #           position difference/2, velocity difference/2]
+    # Position difference is 1 - 2
+    # Input is [Voltage 1, Voltage 2]
+
+    self.C1 = self.spring / (self.J * 0.5)
+    self.C2 = self.Kt * self.G / (self.J * 0.5 * self.R)
+    self.C3 = self.G * self.G * self.Kt / (self.R  * self.J * 0.5 * self.Kv)
+
+    self.A_continuous = numpy.matrix(
+        [[0, 1, 0, 0],
+         [0, -self.C3, 0, 0],
+         [0, 0, 0, 1],
+         [0, 0, -self.C1 * 2.0, -self.C3]])
+
+    print 'Full speed is', self.C2 / self.C3 * 12.0
+
+    print 'Stall arm difference is', 12.0 * self.C2 / self.C1
+    print 'Stall arm difference first principles is', self.stall_torque * self.G / self.spring
+
+    print '5 degrees of arm error is', self.spring / self.r * (math.pi * 5.0 / 180.0)
+
+    # Start with the unmodified input
+    self.B_continuous = numpy.matrix(
+        [[0, 0],
+         [self.C2 / 2.0, self.C2 / 2.0],
+         [0, 0],
+         [self.C2 / 2.0, -self.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)
+
+    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.300
+    q_pos_diff = 0.005
+    q_vel_diff = 0.13
+    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 'Controller'
+    print self.K
+
+    print 'Controller Poles'
+    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]])
+
+    print 'Observer (Converted to a KF)', numpy.linalg.inv(self.A) * self.L
+
+    self.InitializeState()
+
+
+class IntegralArm(Arm):
+  def __init__(self, name="IntegralArm", mass=None):
+    super(IntegralArm, self).__init__(name=name, mass=mass)
+
+    self.A_continuous_unaugmented = self.A_continuous
+    self.A_continuous = numpy.matrix(numpy.zeros((5, 5)))
+    self.A_continuous[0:4, 0:4] = self.A_continuous_unaugmented
+    self.A_continuous[1, 4] = self.C2
+
+    # Start with the unmodified input
+    self.B_continuous_unaugmented = self.B_continuous
+    self.B_continuous = numpy.matrix(numpy.zeros((5, 2)))
+    self.B_continuous[0:4, 0:2] = self.B_continuous_unaugmented
+
+    self.C_unaugmented = self.C
+    self.C = numpy.matrix(numpy.zeros((2, 5)))
+    self.C[0:2, 0:4] = self.C_unaugmented
+
+    self.A, self.B = self.ContinuousToDiscrete(
+        self.A_continuous, self.B_continuous, self.dt)
+    print 'A cont', self.A_continuous
+    print 'B cont', self.B_continuous
+    print 'A discrete', self.A
+
+    q_pos = 0.08
+    q_vel = 0.40
+
+    q_pos_diff = 0.08
+    q_vel_diff = 0.40
+    q_voltage = 6.0
+    self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0, 0.0, 0.0],
+                           [0.0, (q_vel ** 2.0), 0.0, 0.0, 0.0],
+                           [0.0, 0.0, (q_pos_diff ** 2.0), 0.0, 0.0],
+                           [0.0, 0.0, 0.0, (q_vel_diff ** 2.0), 0.0],
+                           [0.0, 0.0, 0.0, 0.0, (q_voltage ** 2.0)]])
+
+    r_volts = 0.05
+    self.R = numpy.matrix([[(r_volts ** 2.0), 0.0],
+                           [0.0, (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)
+
+    self.U_max = numpy.matrix([[12.0], [12.0]])
+    self.U_min = numpy.matrix([[-12.0], [-12.0]])
+
+    self.K_unaugmented = self.K
+    self.K = numpy.matrix(numpy.zeros((2, 5)));
+    self.K[0:2, 0:4] = self.K_unaugmented
+    self.K[0, 4] = 1;
+    self.K[1, 4] = 1;
+    print 'Kal', self.KalmanGain
+    self.L = self.A * self.KalmanGain
+
+    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(arm, initial_X, goal, max_separation_error=0.01,
+             show_graph=True, iterations=200, controller_arm=None,
+             observer_arm=None):
+  """Runs the arm 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:
+      arm: arm 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_arm: arm object to get K from, or None if we should
+          use arm.
+      observer_arm: arm object to use for the observer, or None if we should
+          use the actual state.
+  """
+
+  arm.X = initial_X
+
+  if controller_arm is None:
+    controller_arm = arm
+
+  if observer_arm is not None:
+    observer_arm.X_hat = initial_X + 0.01
+    observer_arm.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 = arm.X
+    if observer_arm is not None:
+      X_hat = observer_arm.X_hat
+      x_hat_avg.append(observer_arm.X_hat[0, 0])
+      x_hat_sep.append(observer_arm.X_hat[2, 0] * sep_plot_gain)
+    U = controller_arm.K * (goal - X_hat)
+    U = CapU(U)
+    x_avg.append(arm.X[0, 0])
+    v_avg.append(arm.X[1, 0])
+    x_sep.append(arm.X[2, 0] * sep_plot_gain)
+    v_sep.append(arm.X[3, 0])
+    if observer_arm is not None:
+      observer_arm.PredictObserver(U)
+    arm.Update(U)
+    if observer_arm is not None:
+      observer_arm.Y = arm.Y
+      observer_arm.CorrectObserver(U)
+
+    t.append(i * arm.dt)
+    u_left.append(U[0, 0])
+    u_right.append(U[1, 0])
+
+  print numpy.linalg.inv(arm.A)
+  print "delta time is ", arm.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_arm 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 run_integral_test(arm, initial_X, goal, observer_arm, disturbance,
+                      max_separation_error=0.01, show_graph=True,
+                      iterations=400):
+  """Runs the integral control arm 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:
+      arm: arm object to use.
+      initial_X: starting state.
+      goal: goal state.
+      observer_arm: arm object to use for the observer.
+      show_graph: Whether or not to display a graph showing the changing
+           states and voltages.
+      iterations: Number of timesteps to run the model for.
+      disturbance: Voltage missmatch between controller and model.
+  """
+
+  arm.X = 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 = []
+  u_error = []
+
+  sep_plot_gain = 100.0
+
+  unaugmented_goal = goal
+  goal = numpy.matrix(numpy.zeros((5, 1)))
+  goal[0:4, 0] = unaugmented_goal
+
+  for i in xrange(iterations):
+    X_hat = observer_arm.X_hat[0:4]
+
+    x_hat_avg.append(observer_arm.X_hat[0, 0])
+    x_hat_sep.append(observer_arm.X_hat[2, 0] * sep_plot_gain)
+
+    U = observer_arm.K * (goal - observer_arm.X_hat)
+    u_error.append(observer_arm.X_hat[4,0])
+    U = CapU(U)
+    x_avg.append(arm.X[0, 0])
+    v_avg.append(arm.X[1, 0])
+    x_sep.append(arm.X[2, 0] * sep_plot_gain)
+    v_sep.append(arm.X[3, 0])
+
+    observer_arm.PredictObserver(U)
+
+    arm.Update(U + disturbance)
+    observer_arm.Y = arm.Y
+    observer_arm.CorrectObserver(U)
+
+    t.append(i * arm.dt)
+    u_left.append(U[0, 0])
+    u_right.append(U[1, 0])
+
+  print 'End is', observer_arm.X_hat[4, 0]
+
+  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_arm 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.plot(t, u_error, label='u error')
+    pylab.legend()
+    pylab.show()
+
+
+def main(argv):
+  loaded_mass = 25
+  #loaded_mass = 0
+  arm = Arm(mass=13 + loaded_mass)
+  #arm_controller = Arm(mass=13 + 15)
+  #observer_arm = Arm(mass=13 + 15)
+  #observer_arm = None
+
+  integral_arm = IntegralArm(mass=13 + loaded_mass)
+  integral_arm.X_hat[0, 0] += 0.02
+  integral_arm.X_hat[2, 0] += 0.02
+  integral_arm.X_hat[4] = 0
+
+  # Test moving the arm with constant separation.
+  initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+  R = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+  run_integral_test(arm, initial_X, R, integral_arm, disturbance=2)
+
+  # Write the generated constants out to a file.
+  if len(argv) != 5:
+    print "Expected .h file name and .cc file name for the arm and augmented arm."
+  else:
+    arm = Arm("Arm", mass=13)
+    loop_writer = control_loop.ControlLoopWriter("Arm", [arm])
+    if argv[1][-3:] == '.cc':
+      loop_writer.Write(argv[2], argv[1])
+    else:
+      loop_writer.Write(argv[1], argv[2])
+
+    integral_arm = IntegralArm("IntegralArm", mass=13)
+    loop_writer = control_loop.ControlLoopWriter("IntegralArm", [integral_arm])
+    if argv[3][-3:] == '.cc':
+      loop_writer.Write(argv[4], argv[3])
+    else:
+      loop_writer.Write(argv[3], argv[4])
+
+if __name__ == '__main__':
+  sys.exit(main(sys.argv))
diff --git a/y2014/control_loops/python/claw.py b/y2014/control_loops/python/claw.py
new file mode 100755
index 0000000..b5ea6a1
--- /dev/null
+++ b/y2014/control_loops/python/claw.py
@@ -0,0 +1,491 @@
+#!/usr/bin/python
+
+import control_loop
+import controls
+import polytope
+import polydrivetrain
+import numpy
+import sys
+from matplotlib import pylab
+
+class Claw(control_loop.ControlLoop):
+  def __init__(self, name="RawClaw"):
+    super(Claw, 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 = 5500.0
+    # Free Current in Amps
+    self.free_current = 2.7
+    # Moment of inertia of the claw in kg m^2
+    self.J_top = 2.8
+    self.J_bottom = 3.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) /
+               (13.5 - self.R * self.free_current))
+    # Torque constant
+    self.Kt = self.stall_torque / self.stall_current
+    # Gear ratio
+    self.G = 14.0 / 48.0 * 18.0 / 32.0 * 18.0 / 66.0 * 12.0 / 60.0
+    # Control loop time step
+    self.dt = 0.01
+
+    # State is [bottom position, bottom velocity, top - bottom position,
+    #           top - bottom velocity]
+    # Input is [bottom power, top power - bottom power * J_top / J_bottom]
+    # Motor time constants. difference_bottom refers to the constant for how the
+    # bottom velocity affects the difference of the top and bottom velocities.
+    self.common_motor_constant = -self.Kt / self.Kv / (self.G * self.G * self.R)
+    self.bottom_bottom = self.common_motor_constant / self.J_bottom
+    self.difference_bottom = -self.common_motor_constant * (1 / self.J_bottom
+                                                        - 1 / self.J_top)
+    self.difference_difference = self.common_motor_constant / self.J_top
+    # State feedback matrices
+
+    self.A_continuous = numpy.matrix(
+        [[0, 0, 1, 0],
+         [0, 0, 0, 1],
+         [0, 0, self.bottom_bottom, 0],
+         [0, 0, self.difference_bottom, self.difference_difference]])
+
+    self.A_bottom_cont = numpy.matrix(
+        [[0, 1],
+         [0, self.bottom_bottom]])
+
+    self.A_diff_cont = numpy.matrix(
+        [[0, 1],
+         [0, self.difference_difference]])
+
+    self.motor_feedforward = self.Kt / (self.G * self.R)
+    self.motor_feedforward_bottom = self.motor_feedforward / self.J_bottom
+    self.motor_feedforward_difference = self.motor_feedforward / self.J_top
+    self.motor_feedforward_difference_bottom = (
+        self.motor_feedforward * (1 / self.J_bottom - 1 / self.J_top))
+    self.B_continuous = numpy.matrix(
+        [[0, 0],
+         [0, 0],
+         [self.motor_feedforward_bottom, 0],
+         [-self.motor_feedforward_bottom, self.motor_feedforward_difference]])
+
+    print "Cont X_ss", self.motor_feedforward / -self.common_motor_constant
+
+    self.B_bottom_cont = numpy.matrix(
+        [[0],
+         [self.motor_feedforward_bottom]])
+
+    self.B_diff_cont = numpy.matrix(
+        [[0],
+         [self.motor_feedforward_difference]])
+
+    self.C = numpy.matrix([[1, 0, 0, 0],
+                           [1, 1, 0, 0]])
+    self.D = numpy.matrix([[0, 0],
+                           [0, 0]])
+
+    self.A, self.B = self.ContinuousToDiscrete(
+        self.A_continuous, self.B_continuous, self.dt)
+
+    self.A_bottom, self.B_bottom = controls.c2d(
+        self.A_bottom_cont, self.B_bottom_cont, self.dt)
+    self.A_diff, self.B_diff = controls.c2d(
+        self.A_diff_cont, self.B_diff_cont, self.dt)
+
+    self.K_bottom = controls.dplace(self.A_bottom, self.B_bottom, [.75 + 0.1j, .75 - 0.1j])
+    self.K_diff = controls.dplace(self.A_diff, self.B_diff, [.75 + 0.1j, .75 - 0.1j])
+
+    print "K_diff", self.K_diff
+    print "K_bottom", self.K_bottom
+
+    print "A"
+    print self.A
+    print "B"
+    print self.B
+
+    
+    self.Q = numpy.matrix([[(1.0 / (0.10 ** 2.0)), 0.0, 0.0, 0.0],
+                           [0.0, (1.0 / (0.06 ** 2.0)), 0.0, 0.0],
+                           [0.0, 0.0, 0.10, 0.0],
+                           [0.0, 0.0, 0.0, 0.1]])
+
+    self.R = numpy.matrix([[(1.0 / (40.0 ** 2.0)), 0.0],
+                           [0.0, (1.0 / (5.0 ** 2.0))]])
+    #self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+
+    self.K = numpy.matrix([[self.K_bottom[0, 0], 0.0, self.K_bottom[0, 1], 0.0],
+                           [0.0, self.K_diff[0, 0], 0.0, self.K_diff[0, 1]]])
+
+    # Compute the feed forwards aceleration term.
+    self.K[1, 0] = -self.B[1, 0] * self.K[0, 0] / self.B[1, 1]
+
+    lstsq_A = numpy.identity(2)
+    lstsq_A[0, :] = self.B[1, :]
+    lstsq_A[1, :] = self.B[3, :]
+    print "System of Equations coefficients:"
+    print lstsq_A
+    print "det", numpy.linalg.det(lstsq_A)
+
+    out_x = numpy.linalg.lstsq(
+                         lstsq_A,
+                         numpy.matrix([[self.A[1, 2]], [self.A[3, 2]]]))[0]
+    self.K[1, 2] = -lstsq_A[0, 0] * (self.K[0, 2] - out_x[0]) / lstsq_A[0, 1] + out_x[1]
+
+    print "K unaugmented"
+    print self.K
+    print "B * K unaugmented"
+    print self.B * self.K
+    F = self.A - self.B * self.K
+    print "A - B * K unaugmented"
+    print F
+    print "eigenvalues"
+    print numpy.linalg.eig(F)[0]
+
+    self.rpl = .05
+    self.ipl = 0.010
+    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]])
+
+    # For the tests that check the limits, these are (upper, lower) for both
+    # claws.
+    self.hard_pos_limits = None
+    self.pos_limits = None
+
+    # Compute the steady state velocities for a given applied voltage.
+    # The top and bottom of the claw should spin at the same rate if the
+    # physics is right.
+    X_ss = numpy.matrix([[0], [0], [0.0], [0]])
+    
+    U = numpy.matrix([[1.0],[1.0]])
+    A = self.A
+    B = self.B
+    #X_ss[2, 0] = X_ss[2, 0] * A[2, 2] + B[2, 0] * U[0, 0]
+    X_ss[2, 0] = 1 / (1 - A[2, 2]) * B[2, 0] * U[0, 0]
+    #X_ss[3, 0] = X_ss[3, 0] * A[3, 3] + X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
+    #X_ss[3, 0] * (1 - A[3, 3]) = X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
+    X_ss[3, 0] = 1 / (1 - A[3, 3]) * (X_ss[2, 0] * A[3, 2] + B[3, 1] * U[1, 0] + B[3, 0] * U[0, 0])
+    #X_ss[3, 0] = 1 / (1 - A[3, 3]) / (1 - A[2, 2]) * B[2, 0] * U[0, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
+    X_ss[0, 0] = A[0, 2] * X_ss[2, 0] + B[0, 0] * U[0, 0]
+    X_ss[1, 0] = A[1, 2] * X_ss[2, 0] + A[1, 3] * X_ss[3, 0] + B[1, 0] * U[0, 0] + B[1, 1] * U[1, 0]
+
+    print "X_ss", X_ss
+
+    self.InitializeState()
+
+
+class ClawDeltaU(Claw):
+  def __init__(self, name="Claw"):
+    super(ClawDeltaU, self).__init__(name)
+    A_unaugmented = self.A
+    B_unaugmented = self.B
+    C_unaugmented = self.C
+
+    self.A = numpy.matrix([[0.0, 0.0, 0.0, 0.0, 0.0],
+                           [0.0, 0.0, 0.0, 0.0, 0.0],
+                           [0.0, 0.0, 0.0, 0.0, 0.0],
+                           [0.0, 0.0, 0.0, 0.0, 0.0],
+                           [0.0, 0.0, 0.0, 0.0, 1.0]])
+    self.A[0:4, 0:4] = A_unaugmented
+    self.A[0:4, 4] = B_unaugmented[0:4, 0]
+
+    self.B = numpy.matrix([[0.0, 0.0],
+                           [0.0, 0.0],
+                           [0.0, 0.0],
+                           [0.0, 0.0],
+                           [1.0, 0.0]])
+    self.B[0:4, 1] = B_unaugmented[0:4, 1]
+
+    self.C = numpy.concatenate((C_unaugmented, numpy.matrix([[0.0], [0.0]])),
+                               axis=1)
+    self.D = numpy.matrix([[0.0, 0.0],
+                           [0.0, 0.0]])
+
+    #self.PlaceControllerPoles([0.55, 0.35, 0.55, 0.35, 0.80])
+    self.Q = numpy.matrix([[(1.0 / (0.04 ** 2.0)), 0.0, 0.0, 0.0, 0.0],
+                           [0.0, (1.0 / (0.01 ** 2)), 0.0, 0.0, 0.0],
+                           [0.0, 0.0, 0.01, 0.0, 0.0],
+                           [0.0, 0.0, 0.0, 0.08, 0.0],
+                           [0.0, 0.0, 0.0, 0.0, (1.0 / (10.0 ** 2))]])
+
+    self.R = numpy.matrix([[0.000001, 0.0],
+                           [0.0, 1.0 / (10.0 ** 2.0)]])
+    self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+
+    self.K = numpy.matrix([[50.0, 0.0, 10.0, 0.0, 1.0],
+                           [50.0, 0.0, 10.0, 0.0, 1.0]])
+    #self.K = numpy.matrix([[50.0, -100.0, 0, -10, 0],
+    #                       [50.0,  100.0, 0, 10, 0]])
+
+    controlability = controls.ctrb(self.A, self.B);
+    print "Rank of augmented controlability matrix.", numpy.linalg.matrix_rank(controlability)
+
+    print "K"
+    print self.K
+    print "Placed controller poles are"
+    print numpy.linalg.eig(self.A - self.B * self.K)[0]
+    print [numpy.abs(x) for x in numpy.linalg.eig(self.A - self.B * self.K)[0]]
+
+    self.rpl = .05
+    self.ipl = 0.008
+    self.PlaceObserverPoles([self.rpl + 1j * self.ipl, 0.10, 0.09,
+                             self.rpl - 1j * self.ipl, 0.90])
+    #print "A is"
+    #print self.A
+    #print "L is"
+    #print self.L
+    #print "C is"
+    #print self.C
+    #print "A - LC is"
+    #print self.A - self.L * self.C
+
+    #print "Placed observer poles are"
+    #print numpy.linalg.eig(self.A - self.L * self.C)[0]
+
+    self.U_max = numpy.matrix([[12.0], [12.0]])
+    self.U_min = numpy.matrix([[-12.0], [-12.0]])
+
+    self.InitializeState()
+
+def ScaleU(claw, U, K, error):
+  """Clips U as necessary.
+
+    Args:
+      claw: claw object containing moments of inertia and U limits.
+      U: Input matrix to clip as necessary.
+  """
+
+  bottom_u = U[0, 0]
+  top_u = U[1, 0]
+  position_error = error[0:2, 0]
+  velocity_error = error[2:, 0]
+
+  U_poly = polytope.HPolytope(
+      numpy.matrix([[1, 0],
+                    [-1, 0],
+                    [0, 1],
+                    [0, -1]]),
+      numpy.matrix([[12],
+                    [12],
+                    [12],
+                    [12]]))
+
+  if (bottom_u > claw.U_max[0, 0] or bottom_u < claw.U_min[0, 0] or
+      top_u > claw.U_max[0, 0] or top_u < claw.U_min[0, 0]):
+
+    position_K = K[:, 0:2]
+    velocity_K = K[:, 2:]
+
+    # H * U <= k
+    # U = UPos + UVel
+    # H * (UPos + UVel) <= k
+    # H * UPos <= k - H * UVel
+    #
+    # Now, we can do a coordinate transformation and say the following.
+    #
+    # UPos = position_K * position_error
+    # (H * position_K) * position_error <= k - H * UVel
+    #
+    # Add in the constraint that 0 <= t <= 1
+    # Now, there are 2 ways this can go.  Either we have a region, or we don't
+    # have a region.  If we have a region, then pick the largest t and go for it.
+    # If we don't have a region, we need to pick a good comprimise.
+
+    pos_poly = polytope.HPolytope(
+        U_poly.H * position_K,
+        U_poly.k - U_poly.H * velocity_K * velocity_error)
+
+    # The actual angle for the line we call 45.
+    angle_45 = numpy.matrix([[numpy.sqrt(3), 1]])
+    if claw.pos_limits and claw.hard_pos_limits and claw.X[0, 0] + claw.X[1, 0] > claw.pos_limits[1]:
+      angle_45 = numpy.matrix([[1, 1]])
+
+    P = position_error
+    L45 = numpy.multiply(numpy.matrix([[numpy.sign(P[1, 0]), -numpy.sign(P[0, 0])]]), angle_45)
+    if L45[0, 1] == 0:
+      L45[0, 1] = 1
+    if L45[0, 0] == 0:
+      L45[0, 0] = 1
+    w45 = numpy.matrix([[0]])
+
+    if numpy.abs(P[0, 0]) > numpy.abs(P[1, 0]):
+      LH = numpy.matrix([[0, 1]])
+    else:
+      LH = numpy.matrix([[1, 0]])
+    wh = LH * P
+    standard = numpy.concatenate((L45, LH))
+    W = numpy.concatenate((w45, wh))
+    intersection = numpy.linalg.inv(standard) * W
+    adjusted_pos_error_h, is_inside_h = polydrivetrain.DoCoerceGoal(pos_poly,
+        LH, wh, position_error)
+    adjusted_pos_error_45, is_inside_45 = polydrivetrain.DoCoerceGoal(pos_poly,
+        L45, w45, intersection)
+    if pos_poly.IsInside(intersection):
+      adjusted_pos_error = adjusted_pos_error_h
+    else:
+      if is_inside_h:
+        if numpy.linalg.norm(adjusted_pos_error_h) > numpy.linalg.norm(adjusted_pos_error_45):
+          adjusted_pos_error = adjusted_pos_error_h
+        else:
+          adjusted_pos_error = adjusted_pos_error_45
+      else:
+        adjusted_pos_error = adjusted_pos_error_45
+    #print adjusted_pos_error
+
+    #print "Actual power is ", velocity_K * velocity_error + position_K * adjusted_pos_error
+    return velocity_K * velocity_error + position_K * adjusted_pos_error
+
+    #U = Kpos * poserror + Kvel * velerror
+      
+    #scalar = claw.U_max[0, 0] / max(numpy.abs(top_u), numpy.abs(bottom_u))
+
+    #top_u *= scalar
+    #bottom_u *= scalar
+
+  return numpy.matrix([[bottom_u], [top_u]])
+
+def run_test(claw, initial_X, goal, max_separation_error=0.01, show_graph=False, iterations=200):
+  """Runs the claw plant on a given claw (claw) with an initial condition (initial_X) and goal (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:
+      claw: claw 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."""
+
+  claw.X = initial_X
+
+  # Various lists for graphing things.
+  t = []
+  x_bottom = []
+  x_top = []
+  u_bottom = []
+  u_top = []
+  x_separation = []
+
+  tests_passed = True
+
+  # Bounds which separation should not exceed.
+  lower_bound = (initial_X[1, 0] if initial_X[1, 0] < goal[1, 0]
+                 else goal[1, 0]) - max_separation_error
+  upper_bound = (initial_X[1, 0] if initial_X[1, 0] > goal[1, 0]
+                 else goal[1, 0]) + max_separation_error
+
+  for i in xrange(iterations):
+    U = claw.K * (goal - claw.X)
+    U = ScaleU(claw, U, claw.K, goal - claw.X)
+    claw.Update(U)
+
+    if claw.X[1, 0] > upper_bound or claw.X[1, 0] < lower_bound:
+      tests_passed = False
+      print "Claw separation was", claw.X[1, 0]
+      print "Should have been between", lower_bound, "and", upper_bound
+
+    if claw.hard_pos_limits and \
+      (claw.X[0, 0] > claw.hard_pos_limits[1] or
+          claw.X[0, 0] < claw.hard_pos_limits[0] or
+          claw.X[0, 0] + claw.X[1, 0] > claw.hard_pos_limits[1] or
+          claw.X[0, 0] + claw.X[1, 0] < claw.hard_pos_limits[0]):
+      tests_passed = False
+      print "Claws at %f and %f" % (claw.X[0, 0], claw.X[0, 0] + claw.X[1, 0])
+      print "Both should be in %s, definitely %s" % \
+          (claw.pos_limits, claw.hard_pos_limits)
+
+    t.append(i * claw.dt)
+    x_bottom.append(claw.X[0, 0] * 10.0)
+    x_top.append((claw.X[1, 0] + claw.X[0, 0]) * 10.0)
+    u_bottom.append(U[0, 0])
+    u_top.append(U[1, 0])
+    x_separation.append(claw.X[1, 0] * 10.0)
+
+  if show_graph:
+    pylab.plot(t, x_bottom, label='x bottom * 10')
+    pylab.plot(t, x_top, label='x top * 10')
+    pylab.plot(t, u_bottom, label='u bottom')
+    pylab.plot(t, u_top, label='u top')
+    pylab.plot(t, x_separation, label='separation * 10')
+    pylab.legend()
+    pylab.show()
+
+  # Test to make sure that we are near the goal.
+  if numpy.max(abs(claw.X - goal)) > 1e-4:
+    tests_passed = False
+    print "X was", claw.X, "Expected", goal
+
+  return tests_passed
+
+def main(argv):
+  claw = Claw()
+
+  # Test moving the claw with constant separation.
+  initial_X = numpy.matrix([[-1.0], [0.0], [0.0], [0.0]])
+  R = numpy.matrix([[1.0], [0.0], [0.0], [0.0]])
+  run_test(claw, initial_X, R)
+
+  # Test just changing separation.
+  initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+  R = numpy.matrix([[0.0], [1.0], [0.0], [0.0]])
+  run_test(claw, initial_X, R)
+
+  # Test changing both separation and position at once.
+  initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+  R = numpy.matrix([[1.0], [1.0], [0.0], [0.0]])
+  run_test(claw, initial_X, R)
+
+  # Test a small separation error and a large position one.
+  initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+  R = numpy.matrix([[2.0], [0.05], [0.0], [0.0]])
+  run_test(claw, initial_X, R)
+
+  # Test a small separation error and a large position one.
+  initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+  R = numpy.matrix([[-0.5], [1.0], [0.0], [0.0]])
+  run_test(claw, initial_X, R)
+
+  # Test opening with the top claw at the limit.
+  initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+  R = numpy.matrix([[-1.5], [1.5], [0.0], [0.0]])
+  claw.hard_pos_limits = (-1.6, 0.1)
+  claw.pos_limits = (-1.5, 0.0)
+  run_test(claw, initial_X, R)
+  claw.pos_limits = None
+
+  # Test opening with the bottom claw at the limit.
+  initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+  R = numpy.matrix([[0], [1.5], [0.0], [0.0]])
+  claw.hard_pos_limits = (-0.1, 1.6)
+  claw.pos_limits = (0.0, 1.6)
+  run_test(claw, initial_X, R)
+  claw.pos_limits = None
+
+  # Write the generated constants out to a file.
+  if len(argv) != 3:
+    print "Expected .h file name and .cc file name for the claw."
+  else:
+    claw = Claw("Claw")
+    loop_writer = control_loop.ControlLoopWriter("Claw", [claw])
+    loop_writer.AddConstant(control_loop.Constant("kClawMomentOfInertiaRatio",
+      "%f", claw.J_top / claw.J_bottom))
+    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/y2014/control_loops/python/drivetrain.py b/y2014/control_loops/python/drivetrain.py
new file mode 100755
index 0000000..ac05a57
--- /dev/null
+++ b/y2014/control_loops/python/drivetrain.py
@@ -0,0 +1,239 @@
+#!/usr/bin/python
+
+import control_loop
+import controls
+import numpy
+import sys
+from matplotlib import pylab
+
+
+class CIM(control_loop.ControlLoop):
+  def __init__(self):
+    super(CIM, self).__init__("CIM")
+    # 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
+    # Moment of inertia of the CIM in kg m^2
+    self.J = 0.0001
+    # Resistance of the motor, divided by 2 to account for the 2 motors
+    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
+    # Control loop time step
+    self.dt = 0.005
+
+    # State feedback matrices
+    self.A_continuous = numpy.matrix(
+        [[-self.Kt / self.Kv / (self.J * self.R)]])
+    self.B_continuous = numpy.matrix(
+        [[self.Kt / (self.J * 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([0.01])
+    self.PlaceObserverPoles([0.01])
+
+    self.U_max = numpy.matrix([[12.0]])
+    self.U_min = numpy.matrix([[-12.0]])
+
+    self.InitializeState()
+
+
+class Drivetrain(control_loop.ControlLoop):
+  def __init__(self, name="Drivetrain", left_low=True, right_low=True):
+    super(Drivetrain, self).__init__(name)
+    # Stall Torque in N m
+    self.stall_torque = 2.42
+    # Stall Current in Amps
+    self.stall_current = 133.0
+    # Free Speed in RPM. Used number from last year.
+    self.free_speed = 4650.0
+    # Free Current in Amps
+    self.free_current = 2.7
+    # Moment of inertia of the drivetrain in kg m^2
+    # Just borrowed from last year.
+    self.J = 4.5
+    # Mass of the robot, in kg.
+    self.m = 68
+    # Radius of the robot, in meters (from last year).
+    self.rb = 0.617998644 / 2.0
+    # Radius of the wheels, in meters.
+    self.r = .04445
+    # Resistance of the motor, divided by the number of motors.
+    self.R = 12.0 / self.stall_current / 4
+    # 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 ratios
+    self.G_low = 18.0 / 60.0 * 18.0 / 50.0
+    self.G_high = 28.0 / 50.0 * 18.0 / 50.0
+    if left_low:
+      self.Gl = self.G_low
+    else:
+      self.Gl = self.G_high
+    if right_low:
+      self.Gr = self.G_low
+    else:
+      self.Gr = self.G_high
+
+    # Control loop time step
+    self.dt = 0.005
+
+    # These describe the way that a given side of a robot will be influenced
+    # by the other side. Units of 1 / kg.
+    self.msp = 1.0 / self.m + self.rb * self.rb / self.J
+    self.msn = 1.0 / self.m - self.rb * self.rb / self.J
+    # The calculations which we will need for A and B.
+    self.tcl = -self.Kt / self.Kv / (self.Gl * self.Gl * self.R * self.r * self.r)
+    self.tcr = -self.Kt / self.Kv / (self.Gr * self.Gr * self.R * self.r * self.r)
+    self.mpl = self.Kt / (self.Gl * self.R * self.r)
+    self.mpr = self.Kt / (self.Gr * self.R * self.r)
+
+    # State feedback matrices
+    # X will be of the format
+    # [[positionl], [velocityl], [positionr], velocityr]]
+    self.A_continuous = numpy.matrix(
+        [[0, 1, 0, 0],
+         [0, self.msp * self.tcl, 0, self.msn * self.tcr],
+         [0, 0, 0, 1],
+         [0, self.msn * self.tcl, 0, self.msp * self.tcr]])
+    self.B_continuous = numpy.matrix(
+        [[0, 0],
+         [self.msp * self.mpl, self.msn * self.mpr],
+         [0, 0],
+         [self.msn * self.mpl, self.msp * self.mpr]])
+    self.C = numpy.matrix([[1, 0, 0, 0],
+                           [0, 0, 1, 0]])
+    self.D = numpy.matrix([[0, 0],
+                           [0, 0]])
+
+    #print "THE NUMBER I WANT" + str(numpy.linalg.inv(self.A_continuous) * -self.B_continuous * numpy.matrix([[12.0], [12.0]]))
+    self.A, self.B = self.ContinuousToDiscrete(
+        self.A_continuous, self.B_continuous, self.dt)
+
+    # Poles from last year.
+    self.hp = 0.65
+    self.lp = 0.83
+    self.PlaceControllerPoles([self.hp, self.lp, self.hp, self.lp])
+    print self.K
+    q_pos = 0.07
+    q_vel = 1.0
+    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 ** 2.0)), 0.0],
+                           [0.0, 0.0, 0.0, (1.0 / (q_vel ** 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.A
+    print self.B
+    print self.K
+    print numpy.linalg.eig(self.A - self.B * self.K)[0]
+
+    self.hlp = 0.3
+    self.llp = 0.4
+    self.PlaceObserverPoles([self.hlp, self.hlp, self.llp, self.llp])
+
+    self.U_max = numpy.matrix([[12.0], [12.0]])
+    self.U_min = numpy.matrix([[-12.0], [-12.0]])
+    self.InitializeState()
+
+def main(argv):
+  # Simulate the response of the system to a step input.
+  drivetrain = Drivetrain()
+  simulated_left = []
+  simulated_right = []
+  for _ in xrange(100):
+    drivetrain.Update(numpy.matrix([[12.0], [12.0]]))
+    simulated_left.append(drivetrain.X[0, 0])
+    simulated_right.append(drivetrain.X[2, 0])
+
+  #pylab.plot(range(100), simulated_left)
+  #pylab.plot(range(100), simulated_right)
+  #pylab.show()
+
+  # Simulate forwards motion.
+  drivetrain = Drivetrain()
+  close_loop_left = []
+  close_loop_right = []
+  R = numpy.matrix([[1.0], [0.0], [1.0], [0.0]])
+  for _ in xrange(100):
+    U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
+                   drivetrain.U_min, drivetrain.U_max)
+    drivetrain.UpdateObserver(U)
+    drivetrain.Update(U)
+    close_loop_left.append(drivetrain.X[0, 0])
+    close_loop_right.append(drivetrain.X[2, 0])
+
+  #pylab.plot(range(100), close_loop_left)
+  #pylab.plot(range(100), close_loop_right)
+  #pylab.show()
+
+  # Try turning in place
+  drivetrain = Drivetrain()
+  close_loop_left = []
+  close_loop_right = []
+  R = numpy.matrix([[-1.0], [0.0], [1.0], [0.0]])
+  for _ in xrange(100):
+    U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
+                   drivetrain.U_min, drivetrain.U_max)
+    drivetrain.UpdateObserver(U)
+    drivetrain.Update(U)
+    close_loop_left.append(drivetrain.X[0, 0])
+    close_loop_right.append(drivetrain.X[2, 0])
+
+  #pylab.plot(range(100), close_loop_left)
+  #pylab.plot(range(100), close_loop_right)
+  #pylab.show()
+
+  # Try turning just one side.
+  drivetrain = Drivetrain()
+  close_loop_left = []
+  close_loop_right = []
+  R = numpy.matrix([[0.0], [0.0], [1.0], [0.0]])
+  for _ in xrange(100):
+    U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
+                   drivetrain.U_min, drivetrain.U_max)
+    drivetrain.UpdateObserver(U)
+    drivetrain.Update(U)
+    close_loop_left.append(drivetrain.X[0, 0])
+    close_loop_right.append(drivetrain.X[2, 0])
+
+  #pylab.plot(range(100), close_loop_left)
+  #pylab.plot(range(100), close_loop_right)
+  #pylab.show()
+
+  # Write the generated constants out to a file.
+  print "Output one"
+  drivetrain_low_low = Drivetrain(name="DrivetrainLowLow", left_low=True, right_low=True)
+  drivetrain_low_high = Drivetrain(name="DrivetrainLowHigh", left_low=True, right_low=False)
+  drivetrain_high_low = Drivetrain(name="DrivetrainHighLow", left_low=False, right_low=True)
+  drivetrain_high_high = Drivetrain(name="DrivetrainHighHigh", left_low=False, right_low=False)
+
+  if len(argv) != 5:
+    print "Expected .h file name and .cc file name"
+  else:
+    dog_loop_writer = control_loop.ControlLoopWriter(
+        "Drivetrain", [drivetrain_low_low, drivetrain_low_high,
+                       drivetrain_high_low, drivetrain_high_high])
+    if argv[1][-3:] == '.cc':
+      dog_loop_writer.Write(argv[2], argv[1])
+    else:
+      dog_loop_writer.Write(argv[1], argv[2])
+
+if __name__ == '__main__':
+  sys.exit(main(sys.argv))
diff --git a/y2014/control_loops/python/elevator.py b/y2014/control_loops/python/elevator.py
new file mode 100755
index 0000000..fba72c8
--- /dev/null
+++ b/y2014/control_loops/python/elevator.py
@@ -0,0 +1,246 @@
+#!/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 = 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 elevator
+    if mass is None:
+      self.mass = 13.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) * (84.0 / 14.0)
+    # Pulley diameter
+    self.r = 32 * 0.005 / numpy.pi / 2.0
+    # Control loop time step
+    self.dt = 0.005
+
+    # Elevator left/right spring constant (N/m)
+    self.spring = 800.0
+
+    # State is [average position, average velocity,
+    #           position difference/2, velocity difference/2]
+    # Input is [V_left, V_right]
+
+    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/y2014/control_loops/python/polydrivetrain.py b/y2014/control_loops/python/polydrivetrain.py
new file mode 100755
index 0000000..a2c63fa
--- /dev/null
+++ b/y2014/control_loops/python/polydrivetrain.py
@@ -0,0 +1,503 @@
+#!/usr/bin/python
+
+import numpy
+import sys
+import polytope
+import drivetrain
+import control_loop
+import controls
+from matplotlib import pylab
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+
+def CoerceGoal(region, K, w, R):
+  """Intersects a line with a region, and finds the closest point to R.
+
+  Finds a point that is closest to R inside the region, and on the line
+  defined by K X = w.  If it is not possible to find a point on the line,
+  finds a point that is inside the region and closest to the line.  This
+  function assumes that
+
+  Args:
+    region: HPolytope, the valid goal region.
+    K: numpy.matrix (2 x 1), the matrix for the equation [K1, K2] [x1; x2] = w
+    w: float, the offset in the equation above.
+    R: numpy.matrix (2 x 1), the point to be closest to.
+
+  Returns:
+    numpy.matrix (2 x 1), the point.
+  """
+  return DoCoerceGoal(region, K, w, R)[0]
+
+def DoCoerceGoal(region, K, w, R):
+  if region.IsInside(R):
+    return (R, True)
+
+  perpendicular_vector = K.T / numpy.linalg.norm(K)
+  parallel_vector = numpy.matrix([[perpendicular_vector[1, 0]],
+                                  [-perpendicular_vector[0, 0]]])
+
+  # We want to impose the constraint K * X = w on the polytope H * X <= k.
+  # We do this by breaking X up into parallel and perpendicular components to
+  # the half plane.  This gives us the following equation.
+  #
+  #  parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) = X
+  #
+  # Then, substitute this into the polytope.
+  #
+  #  H * (parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) <= k
+  #
+  # Substitute K * X = w
+  #
+  # H * parallel * (parallel.T \dot X) + H * perpendicular * w <= k
+  #
+  # Move all the knowns to the right side.
+  #
+  # H * parallel * ([parallel1 parallel2] * X) <= k - H * perpendicular * w
+  #
+  # Let t = parallel.T \dot X, the component parallel to the surface.
+  #
+  # H * parallel * t <= k - H * perpendicular * w
+  #
+  # This is a polytope which we can solve, and use to figure out the range of X
+  # that we care about!
+
+  t_poly = polytope.HPolytope(
+      region.H * parallel_vector,
+      region.k - region.H * perpendicular_vector * w)
+
+  vertices = t_poly.Vertices()
+
+  if vertices.shape[0]:
+    # The region exists!
+    # Find the closest vertex
+    min_distance = numpy.infty
+    closest_point = None
+    for vertex in vertices:
+      point = parallel_vector * vertex + perpendicular_vector * w
+      length = numpy.linalg.norm(R - point)
+      if length < min_distance:
+        min_distance = length
+        closest_point = point
+
+    return (closest_point, True)
+  else:
+    # Find the vertex of the space that is closest to the line.
+    region_vertices = region.Vertices()
+    min_distance = numpy.infty
+    closest_point = None
+    for vertex in region_vertices:
+      point = vertex.T
+      length = numpy.abs((perpendicular_vector.T * point)[0, 0])
+      if length < min_distance:
+        min_distance = length
+        closest_point = point
+
+    return (closest_point, False)
+
+
+class VelocityDrivetrainModel(control_loop.ControlLoop):
+  def __init__(self, left_low=True, right_low=True, name="VelocityDrivetrainModel"):
+    super(VelocityDrivetrainModel, self).__init__(name)
+    self._drivetrain = drivetrain.Drivetrain(left_low=left_low,
+                                             right_low=right_low)
+    self.dt = 0.01
+    self.A_continuous = numpy.matrix(
+        [[self._drivetrain.A_continuous[1, 1], self._drivetrain.A_continuous[1, 3]],
+         [self._drivetrain.A_continuous[3, 1], self._drivetrain.A_continuous[3, 3]]])
+
+    self.B_continuous = numpy.matrix(
+        [[self._drivetrain.B_continuous[1, 0], self._drivetrain.B_continuous[1, 1]],
+         [self._drivetrain.B_continuous[3, 0], self._drivetrain.B_continuous[3, 1]]])
+    self.C = numpy.matrix(numpy.eye(2));
+    self.D = numpy.matrix(numpy.zeros((2, 2)));
+
+    self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+                                               self.B_continuous, self.dt)
+
+    # FF * X = U (steady state)
+    self.FF = self.B.I * (numpy.eye(2) - self.A)
+
+    self.PlaceControllerPoles([0.6, 0.6])
+    self.PlaceObserverPoles([0.02, 0.02])
+
+    self.G_high = self._drivetrain.G_high
+    self.G_low = self._drivetrain.G_low
+    self.R = self._drivetrain.R
+    self.r = self._drivetrain.r
+    self.Kv = self._drivetrain.Kv
+    self.Kt = self._drivetrain.Kt
+
+    self.U_max = self._drivetrain.U_max
+    self.U_min = self._drivetrain.U_min
+
+
+class VelocityDrivetrain(object):
+  HIGH = 'high'
+  LOW = 'low'
+  SHIFTING_UP = 'up'
+  SHIFTING_DOWN = 'down'
+
+  def __init__(self):
+    self.drivetrain_low_low = VelocityDrivetrainModel(
+        left_low=True, right_low=True, name='VelocityDrivetrainLowLow')
+    self.drivetrain_low_high = VelocityDrivetrainModel(left_low=True, right_low=False, name='VelocityDrivetrainLowHigh')
+    self.drivetrain_high_low = VelocityDrivetrainModel(left_low=False, right_low=True, name = 'VelocityDrivetrainHighLow')
+    self.drivetrain_high_high = VelocityDrivetrainModel(left_low=False, right_low=False, name = 'VelocityDrivetrainHighHigh')
+
+    # X is [lvel, rvel]
+    self.X = numpy.matrix(
+        [[0.0],
+         [0.0]])
+
+    self.U_poly = polytope.HPolytope(
+        numpy.matrix([[1, 0],
+                      [-1, 0],
+                      [0, 1],
+                      [0, -1]]),
+        numpy.matrix([[12],
+                      [12],
+                      [12],
+                      [12]]))
+
+    self.U_max = numpy.matrix(
+        [[12.0],
+         [12.0]])
+    self.U_min = numpy.matrix(
+        [[-12.0000000000],
+         [-12.0000000000]])
+
+    self.dt = 0.01
+
+    self.R = numpy.matrix(
+        [[0.0],
+         [0.0]])
+
+    # ttrust is the comprimise between having full throttle negative inertia,
+    # and having no throttle negative inertia.  A value of 0 is full throttle
+    # inertia.  A value of 1 is no throttle negative inertia.
+    self.ttrust = 1.0
+
+    self.left_gear = VelocityDrivetrain.LOW
+    self.right_gear = VelocityDrivetrain.LOW
+    self.left_shifter_position = 0.0
+    self.right_shifter_position = 0.0
+    self.left_cim = drivetrain.CIM()
+    self.right_cim = drivetrain.CIM()
+
+  def IsInGear(self, gear):
+    return gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.LOW
+
+  def MotorRPM(self, shifter_position, velocity):
+    if shifter_position > 0.5:
+      return (velocity / self.CurrentDrivetrain().G_high /
+              self.CurrentDrivetrain().r)
+    else:
+      return (velocity / self.CurrentDrivetrain().G_low /
+              self.CurrentDrivetrain().r)
+
+  def CurrentDrivetrain(self):
+    if self.left_shifter_position > 0.5:
+      if self.right_shifter_position > 0.5:
+        return self.drivetrain_high_high
+      else:
+        return self.drivetrain_high_low
+    else:
+      if self.right_shifter_position > 0.5:
+        return self.drivetrain_low_high
+      else:
+        return self.drivetrain_low_low
+
+  def SimShifter(self, gear, shifter_position):
+    if gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.SHIFTING_UP:
+      shifter_position = min(shifter_position + 0.5, 1.0)
+    else:
+      shifter_position = max(shifter_position - 0.5, 0.0)
+
+    if shifter_position == 1.0:
+      gear = VelocityDrivetrain.HIGH
+    elif shifter_position == 0.0:
+      gear = VelocityDrivetrain.LOW
+
+    return gear, shifter_position
+
+  def ComputeGear(self, wheel_velocity, should_print=False, current_gear=False, gear_name=None):
+    high_omega = (wheel_velocity / self.CurrentDrivetrain().G_high /
+                  self.CurrentDrivetrain().r)
+    low_omega = (wheel_velocity / self.CurrentDrivetrain().G_low /
+                 self.CurrentDrivetrain().r)
+    #print gear_name, "Motor Energy Difference.", 0.5 * 0.000140032647 * (low_omega * low_omega - high_omega * high_omega), "joules"
+    high_torque = ((12.0 - high_omega / self.CurrentDrivetrain().Kv) *
+                   self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().R)
+    low_torque = ((12.0 - low_omega / self.CurrentDrivetrain().Kv) *
+                  self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().R)
+    high_power = high_torque * high_omega
+    low_power = low_torque * low_omega
+    #if should_print:
+    #  print gear_name, "High omega", high_omega, "Low omega", low_omega
+    #  print gear_name, "High torque", high_torque, "Low torque", low_torque
+    #  print gear_name, "High power", high_power, "Low power", low_power
+
+    # Shift algorithm improvements.
+    # TODO(aschuh):
+    # It takes time to shift.  Shifting down for 1 cycle doesn't make sense
+    # because you will end up slower than without shifting.  Figure out how
+    # to include that info.
+    # If the driver is still in high gear, but isn't asking for the extra power
+    # from low gear, don't shift until he asks for it.
+    goal_gear_is_high = high_power > low_power
+    #goal_gear_is_high = True
+
+    if not self.IsInGear(current_gear):
+      print gear_name, 'Not in gear.'
+      return current_gear
+    else:
+      is_high = current_gear is VelocityDrivetrain.HIGH
+      if is_high != goal_gear_is_high:
+        if goal_gear_is_high:
+          print gear_name, 'Shifting up.'
+          return VelocityDrivetrain.SHIFTING_UP
+        else:
+          print gear_name, 'Shifting down.'
+          return VelocityDrivetrain.SHIFTING_DOWN
+      else:
+        return current_gear
+
+  def FilterVelocity(self, throttle):
+    # Invert the plant to figure out how the velocity filter would have to work
+    # out in order to filter out the forwards negative inertia.
+    # This math assumes that the left and right power and velocity are equal.
+
+    # The throttle filter should filter such that the motor in the highest gear
+    # should be controlling the time constant.
+    # Do this by finding the index of FF that has the lowest value, and computing
+    # the sums using that index.
+    FF_sum = self.CurrentDrivetrain().FF.sum(axis=1)
+    min_FF_sum_index = numpy.argmin(FF_sum)
+    min_FF_sum = FF_sum[min_FF_sum_index, 0]
+    min_K_sum = self.CurrentDrivetrain().K[min_FF_sum_index, :].sum()
+    # Compute the FF sum for high gear.
+    high_min_FF_sum = self.drivetrain_high_high.FF[0, :].sum()
+
+    # U = self.K[0, :].sum() * (R - x_avg) + self.FF[0, :].sum() * R
+    # throttle * 12.0 = (self.K[0, :].sum() + self.FF[0, :].sum()) * R
+    #                   - self.K[0, :].sum() * x_avg
+
+    # R = (throttle * 12.0 + self.K[0, :].sum() * x_avg) /
+    #     (self.K[0, :].sum() + self.FF[0, :].sum())
+
+    # U = (K + FF) * R - K * X
+    # (K + FF) ^-1 * (U + K * X) = R
+
+    # Scale throttle by min_FF_sum / high_min_FF_sum.  This will make low gear
+    # have the same velocity goal as high gear, and so that the robot will hold
+    # the same speed for the same throttle for all gears.
+    adjusted_ff_voltage = numpy.clip(throttle * 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0)
+    return ((adjusted_ff_voltage + self.ttrust * min_K_sum * (self.X[0, 0] + self.X[1, 0]) / 2.0)
+            / (self.ttrust * min_K_sum + min_FF_sum))
+
+  def Update(self, throttle, steering):
+    # Shift into the gear which sends the most power to the floor.
+    # This is the same as sending the most torque down to the floor at the
+    # wheel.
+
+    self.left_gear = self.right_gear = True
+    if True:
+      self.left_gear = self.ComputeGear(self.X[0, 0], should_print=True,
+                                        current_gear=self.left_gear,
+                                        gear_name="left")
+      self.right_gear = self.ComputeGear(self.X[1, 0], should_print=True,
+                                         current_gear=self.right_gear,
+                                         gear_name="right")
+      if self.IsInGear(self.left_gear):
+        self.left_cim.X[0, 0] = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
+
+      if self.IsInGear(self.right_gear):
+        self.right_cim.X[0, 0] = self.MotorRPM(self.right_shifter_position, self.X[0, 0])
+
+    if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+      # Filter the throttle to provide a nicer response.
+      fvel = self.FilterVelocity(throttle)
+
+      # Constant radius means that angualar_velocity / linear_velocity = constant.
+      # Compute the left and right velocities.
+      steering_velocity = numpy.abs(fvel) * steering
+      left_velocity = fvel - steering_velocity
+      right_velocity = fvel + steering_velocity
+
+      # Write this constraint in the form of K * R = w
+      # angular velocity / linear velocity = constant
+      # (left - right) / (left + right) = constant
+      # left - right = constant * left + constant * right
+
+      # (fvel - steering * numpy.abs(fvel) - fvel - steering * numpy.abs(fvel)) /
+      #  (fvel - steering * numpy.abs(fvel) + fvel + steering * numpy.abs(fvel)) =
+      #       constant
+      # (- 2 * steering * numpy.abs(fvel)) / (2 * fvel) = constant
+      # (-steering * sign(fvel)) = constant
+      # (-steering * sign(fvel)) * (left + right) = left - right
+      # (steering * sign(fvel) + 1) * left + (steering * sign(fvel) - 1) * right = 0
+
+      equality_k = numpy.matrix(
+          [[1 + steering * numpy.sign(fvel), -(1 - steering * numpy.sign(fvel))]])
+      equality_w = 0.0
+
+      self.R[0, 0] = left_velocity
+      self.R[1, 0] = right_velocity
+
+      # Construct a constraint on R by manipulating the constraint on U
+      # Start out with H * U <= k
+      # U = FF * R + K * (R - X)
+      # H * (FF * R + K * R - K * X) <= k
+      # H * (FF + K) * R <= k + H * K * X
+      R_poly = polytope.HPolytope(
+          self.U_poly.H * (self.CurrentDrivetrain().K + self.CurrentDrivetrain().FF),
+          self.U_poly.k + self.U_poly.H * self.CurrentDrivetrain().K * self.X)
+
+      # Limit R back inside the box.
+      self.boxed_R = CoerceGoal(R_poly, equality_k, equality_w, self.R)
+
+      FF_volts = self.CurrentDrivetrain().FF * self.boxed_R
+      self.U_ideal = self.CurrentDrivetrain().K * (self.boxed_R - self.X) + FF_volts
+    else:
+      print 'Not all in gear'
+      if not self.IsInGear(self.left_gear) and not self.IsInGear(self.right_gear):
+        # TODO(austin): Use battery volts here.
+        R_left = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
+        self.U_ideal[0, 0] = numpy.clip(
+            self.left_cim.K * (R_left - self.left_cim.X) + R_left / self.left_cim.Kv,
+            self.left_cim.U_min, self.left_cim.U_max)
+        self.left_cim.Update(self.U_ideal[0, 0])
+
+        R_right = self.MotorRPM(self.right_shifter_position, self.X[1, 0])
+        self.U_ideal[1, 0] = numpy.clip(
+            self.right_cim.K * (R_right - self.right_cim.X) + R_right / self.right_cim.Kv,
+            self.right_cim.U_min, self.right_cim.U_max)
+        self.right_cim.Update(self.U_ideal[1, 0])
+      else:
+        assert False
+
+    self.U = numpy.clip(self.U_ideal, self.U_min, self.U_max)
+
+    # TODO(austin): Model the robot as not accelerating when you shift...
+    # This hack only works when you shift at the same time.
+    if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+      self.X = self.CurrentDrivetrain().A * self.X + self.CurrentDrivetrain().B * self.U
+
+    self.left_gear, self.left_shifter_position = self.SimShifter(
+        self.left_gear, self.left_shifter_position)
+    self.right_gear, self.right_shifter_position = self.SimShifter(
+        self.right_gear, self.right_shifter_position)
+
+    print "U is", self.U[0, 0], self.U[1, 0]
+    print "Left shifter", self.left_gear, self.left_shifter_position, "Right shifter", self.right_gear, self.right_shifter_position
+
+
+def main(argv):
+  vdrivetrain = VelocityDrivetrain()
+
+  if len(argv) != 7:
+    print "Expected .h file name and .cc file name"
+  else:
+    dog_loop_writer = control_loop.ControlLoopWriter(
+        "VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
+                       vdrivetrain.drivetrain_low_high,
+                       vdrivetrain.drivetrain_high_low,
+                       vdrivetrain.drivetrain_high_high])
+
+    if argv[1][-3:] == '.cc':
+      dog_loop_writer.Write(argv[2], argv[1])
+    else:
+      dog_loop_writer.Write(argv[1], argv[2])
+
+    cim_writer = control_loop.ControlLoopWriter(
+        "CIM", [drivetrain.CIM()])
+
+    if argv[5][-3:] == '.cc':
+      cim_writer.Write(argv[6], argv[5])
+    else:
+      cim_writer.Write(argv[5], argv[6])
+    return
+
+  vl_plot = []
+  vr_plot = []
+  ul_plot = []
+  ur_plot = []
+  radius_plot = []
+  t_plot = []
+  left_gear_plot = []
+  right_gear_plot = []
+  vdrivetrain.left_shifter_position = 0.0
+  vdrivetrain.right_shifter_position = 0.0
+  vdrivetrain.left_gear = VelocityDrivetrain.LOW
+  vdrivetrain.right_gear = VelocityDrivetrain.LOW
+
+  print "K is", vdrivetrain.CurrentDrivetrain().K
+
+  if vdrivetrain.left_gear is VelocityDrivetrain.HIGH:
+    print "Left is high"
+  else:
+    print "Left is low"
+  if vdrivetrain.right_gear is VelocityDrivetrain.HIGH:
+    print "Right is high"
+  else:
+    print "Right is low"
+
+  for t in numpy.arange(0, 1.7, vdrivetrain.dt):
+    if t < 0.5:
+      vdrivetrain.Update(throttle=0.00, steering=1.0)
+    elif t < 1.2:
+      vdrivetrain.Update(throttle=0.5, steering=1.0)
+    else:
+      vdrivetrain.Update(throttle=0.00, steering=1.0)
+    t_plot.append(t)
+    vl_plot.append(vdrivetrain.X[0, 0])
+    vr_plot.append(vdrivetrain.X[1, 0])
+    ul_plot.append(vdrivetrain.U[0, 0])
+    ur_plot.append(vdrivetrain.U[1, 0])
+    left_gear_plot.append((vdrivetrain.left_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
+    right_gear_plot.append((vdrivetrain.right_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
+
+    fwd_velocity = (vdrivetrain.X[1, 0] + vdrivetrain.X[0, 0]) / 2
+    turn_velocity = (vdrivetrain.X[1, 0] - vdrivetrain.X[0, 0])
+    if abs(fwd_velocity) < 0.0000001:
+      radius_plot.append(turn_velocity)
+    else:
+      radius_plot.append(turn_velocity / fwd_velocity)
+
+  cim_velocity_plot = []
+  cim_voltage_plot = []
+  cim_time = []
+  cim = drivetrain.CIM()
+  R = numpy.matrix([[300]])
+  for t in numpy.arange(0, 0.5, cim.dt):
+    U = numpy.clip(cim.K * (R - cim.X) + R / cim.Kv, cim.U_min, cim.U_max)
+    cim.Update(U)
+    cim_velocity_plot.append(cim.X[0, 0])
+    cim_voltage_plot.append(U[0, 0] * 10)
+    cim_time.append(t)
+  pylab.plot(cim_time, cim_velocity_plot, label='cim spinup')
+  pylab.plot(cim_time, cim_voltage_plot, label='cim voltage')
+  pylab.legend()
+  pylab.show()
+
+  # TODO(austin):
+  # Shifting compensation.
+
+  # Tighten the turn.
+  # Closed loop drive.
+
+  pylab.plot(t_plot, vl_plot, label='left velocity')
+  pylab.plot(t_plot, vr_plot, label='right velocity')
+  pylab.plot(t_plot, ul_plot, label='left voltage')
+  pylab.plot(t_plot, ur_plot, label='right voltage')
+  pylab.plot(t_plot, radius_plot, label='radius')
+  pylab.plot(t_plot, left_gear_plot, label='left gear high')
+  pylab.plot(t_plot, right_gear_plot, label='right gear high')
+  pylab.legend()
+  pylab.show()
+  return 0
+
+if __name__ == '__main__':
+  sys.exit(main(sys.argv))
diff --git a/y2014/control_loops/python/polydrivetrain_test.py b/y2014/control_loops/python/polydrivetrain_test.py
new file mode 100755
index 0000000..434cdca
--- /dev/null
+++ b/y2014/control_loops/python/polydrivetrain_test.py
@@ -0,0 +1,82 @@
+#!/usr/bin/python
+
+import polydrivetrain
+import numpy
+from numpy.testing import *
+import polytope
+import unittest
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+
+class TestVelocityDrivetrain(unittest.TestCase):
+  def MakeBox(self, x1_min, x1_max, x2_min, x2_max):
+    H = numpy.matrix([[1, 0],
+                      [-1, 0],
+                      [0, 1],
+                      [0, -1]])
+    K = numpy.matrix([[x1_max],
+                      [-x1_min],
+                      [x2_max],
+                      [-x2_min]])
+    return polytope.HPolytope(H, K)
+
+  def test_coerce_inside(self):
+    """Tests coercion when the point is inside the box."""
+    box = self.MakeBox(1, 2, 1, 2)
+
+    # x1 = x2
+    K = numpy.matrix([[1, -1]])
+    w = 0
+
+    assert_array_equal(polydrivetrain.CoerceGoal(box, K, w,
+                                                 numpy.matrix([[1.5], [1.5]])),
+                       numpy.matrix([[1.5], [1.5]]))
+
+  def test_coerce_outside_intersect(self):
+    """Tests coercion when the line intersects the box."""
+    box = self.MakeBox(1, 2, 1, 2)
+
+    # x1 = x2
+    K = numpy.matrix([[1, -1]])
+    w = 0
+
+    assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+                       numpy.matrix([[2.0], [2.0]]))
+
+  def test_coerce_outside_no_intersect(self):
+    """Tests coercion when the line does not intersect the box."""
+    box = self.MakeBox(3, 4, 1, 2)
+
+    # x1 = x2
+    K = numpy.matrix([[1, -1]])
+    w = 0
+
+    assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+                       numpy.matrix([[3.0], [2.0]]))
+
+  def test_coerce_middle_of_edge(self):
+    """Tests coercion when the line intersects the middle of an edge."""
+    box = self.MakeBox(0, 4, 1, 2)
+
+    # x1 = x2
+    K = numpy.matrix([[-1, 1]])
+    w = 0
+
+    assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+                       numpy.matrix([[2.0], [2.0]]))
+
+  def test_coerce_perpendicular_line(self):
+    """Tests coercion when the line does not intersect and is in quadrant 2."""
+    box = self.MakeBox(1, 2, 1, 2)
+
+    # x1 = -x2
+    K = numpy.matrix([[1, 1]])
+    w = 0
+
+    assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+                       numpy.matrix([[1.0], [1.0]]))
+
+
+if __name__ == '__main__':
+  unittest.main()
diff --git a/y2014/control_loops/python/shooter.py b/y2014/control_loops/python/shooter.py
new file mode 100755
index 0000000..69f2599
--- /dev/null
+++ b/y2014/control_loops/python/shooter.py
@@ -0,0 +1,254 @@
+#!/usr/bin/python
+
+import control_loop
+import numpy
+import sys
+from matplotlib import pylab
+
+class SprungShooter(control_loop.ControlLoop):
+  def __init__(self, name="RawSprungShooter"):
+    super(SprungShooter, self).__init__(name)
+    # Stall Torque in N m
+    self.stall_torque = .4982
+    # Stall Current in Amps
+    self.stall_current = 85
+    # Free Speed in RPM
+    self.free_speed = 19300.0
+    # Free Current in Amps
+    self.free_current = 1.2
+    # Effective mass of the shooter in kg.
+    # This rough estimate should about include the effect of the masses
+    # of the gears. If this number is too low, the eigen values of self.A
+    # will start to become extremely small.
+    self.J = 200
+    # Resistance of the motor, divided by the number of motors.
+    self.R = 12.0 / self.stall_current / 2.0
+    # 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
+    # Spring constant for the springs, N/m
+    self.Ks = 2800.0
+    # Maximum extension distance (Distance from the 0 force point on the
+    # spring to the latch position.)
+    self.max_extension = 0.32385
+    # Gear ratio multiplied by radius of final sprocket.
+    self.G = 10.0 / 40.0 * 20.0 / 54.0 * 24.0 / 54.0 * 20.0 / 84.0 * 16.0 * (3.0 / 8.0) / (2.0 * numpy.pi) * 0.0254
+
+    # Control loop time step
+    self.dt = 0.01
+
+    # State feedback matrices
+    self.A_continuous = numpy.matrix(
+        [[0, 1],
+         [-self.Ks / self.J,
+          -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.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([0.45, 0.45])
+
+    self.rpl = .05
+    self.ipl = 0.008
+    self.PlaceObserverPoles([self.rpl,
+                             self.rpl])
+
+    self.U_max = numpy.matrix([[12.0]])
+    self.U_min = numpy.matrix([[-12.0]])
+
+    self.InitializeState()
+
+
+class Shooter(SprungShooter):
+  def __init__(self, name="RawShooter"):
+    super(Shooter, self).__init__(name)
+
+    # State feedback matrices
+    self.A_continuous = numpy.matrix(
+        [[0, 1],
+         [0, -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.A, self.B = self.ContinuousToDiscrete(
+        self.A_continuous, self.B_continuous, self.dt)
+
+    self.PlaceControllerPoles([0.45, 0.45])
+
+    self.rpl = .05
+    self.ipl = 0.008
+    self.PlaceObserverPoles([self.rpl,
+                             self.rpl])
+
+    self.U_max = numpy.matrix([[12.0]])
+    self.U_min = numpy.matrix([[-12.0]])
+
+    self.InitializeState()
+
+
+class SprungShooterDeltaU(SprungShooter):
+  def __init__(self, name="SprungShooter"):
+    super(SprungShooterDeltaU, self).__init__(name)
+    A_unaugmented = self.A
+    B_unaugmented = self.B
+
+    self.A = numpy.matrix([[0.0, 0.0, 0.0],
+                           [0.0, 0.0, 0.0],
+                           [0.0, 0.0, 1.0]])
+    self.A[0:2, 0:2] = A_unaugmented
+    self.A[0:2, 2] = B_unaugmented
+
+    self.B = numpy.matrix([[0.0],
+                           [0.0],
+                           [1.0]])
+
+    self.C = numpy.matrix([[1.0, 0.0, 0.0]])
+    self.D = numpy.matrix([[0.0]])
+
+    self.PlaceControllerPoles([0.50, 0.35, 0.80])
+
+    print "K"
+    print self.K
+    print "Placed controller poles are"
+    print numpy.linalg.eig(self.A - self.B * self.K)[0]
+
+    self.rpl = .05
+    self.ipl = 0.008
+    self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+                             self.rpl - 1j * self.ipl, 0.90])
+    print "Placed observer poles are"
+    print numpy.linalg.eig(self.A - self.L * self.C)[0]
+
+    self.U_max = numpy.matrix([[12.0]])
+    self.U_min = numpy.matrix([[-12.0]])
+
+    self.InitializeState()
+
+
+class ShooterDeltaU(Shooter):
+  def __init__(self, name="Shooter"):
+    super(ShooterDeltaU, self).__init__(name)
+    A_unaugmented = self.A
+    B_unaugmented = self.B
+
+    self.A = numpy.matrix([[0.0, 0.0, 0.0],
+                           [0.0, 0.0, 0.0],
+                           [0.0, 0.0, 1.0]])
+    self.A[0:2, 0:2] = A_unaugmented
+    self.A[0:2, 2] = B_unaugmented
+
+    self.B = numpy.matrix([[0.0],
+                           [0.0],
+                           [1.0]])
+
+    self.C = numpy.matrix([[1.0, 0.0, 0.0]])
+    self.D = numpy.matrix([[0.0]])
+
+    self.PlaceControllerPoles([0.55, 0.45, 0.80])
+
+    print "K"
+    print self.K
+    print "Placed controller poles are"
+    print numpy.linalg.eig(self.A - self.B * self.K)[0]
+
+    self.rpl = .05
+    self.ipl = 0.008
+    self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+                             self.rpl - 1j * self.ipl, 0.90])
+    print "Placed observer poles are"
+    print numpy.linalg.eig(self.A - self.L * self.C)[0]
+
+    self.U_max = numpy.matrix([[12.0]])
+    self.U_min = numpy.matrix([[-12.0]])
+
+    self.InitializeState()
+
+
+def ClipDeltaU(shooter, old_voltage, delta_u):
+  old_u = old_voltage
+  new_u = numpy.clip(old_u + delta_u, shooter.U_min, shooter.U_max)
+  return new_u - old_u
+
+def main(argv):
+  # Simulate the response of the system to a goal.
+  sprung_shooter = SprungShooterDeltaU()
+  raw_sprung_shooter = SprungShooter()
+  close_loop_x = []
+  close_loop_u = []
+  goal_position = -0.3
+  R = numpy.matrix([[goal_position], [0.0], [-sprung_shooter.A[1, 0] / sprung_shooter.A[1, 2] * goal_position]])
+  voltage = numpy.matrix([[0.0]])
+  for _ in xrange(500):
+    U = sprung_shooter.K * (R - sprung_shooter.X_hat)
+    U = ClipDeltaU(sprung_shooter, voltage, U)
+    sprung_shooter.Y = raw_sprung_shooter.Y + 0.01
+    sprung_shooter.UpdateObserver(U)
+    voltage += U;
+    raw_sprung_shooter.Update(voltage)
+    close_loop_x.append(raw_sprung_shooter.X[0, 0] * 10)
+    close_loop_u.append(voltage[0, 0])
+
+  pylab.plot(range(500), close_loop_x)
+  pylab.plot(range(500), close_loop_u)
+  pylab.show()
+
+  shooter = ShooterDeltaU()
+  raw_shooter = Shooter()
+  close_loop_x = []
+  close_loop_u = []
+  goal_position = -0.3
+  R = numpy.matrix([[goal_position], [0.0], [-shooter.A[1, 0] / shooter.A[1, 2] * goal_position]])
+  voltage = numpy.matrix([[0.0]])
+  for _ in xrange(500):
+    U = shooter.K * (R - shooter.X_hat)
+    U = ClipDeltaU(shooter, voltage, U)
+    shooter.Y = raw_shooter.Y + 0.01
+    shooter.UpdateObserver(U)
+    voltage += U;
+    raw_shooter.Update(voltage)
+    close_loop_x.append(raw_shooter.X[0, 0] * 10)
+    close_loop_u.append(voltage[0, 0])
+
+  pylab.plot(range(500), close_loop_x)
+  pylab.plot(range(500), close_loop_u)
+  pylab.show()
+
+  # Write the generated constants out to a file.
+  if len(argv) != 5:
+    print "Expected .h file name and .cc file name for"
+    print "both the plant and unaugmented plant"
+  else:
+    unaug_sprung_shooter = SprungShooter("RawSprungShooter")
+    unaug_shooter = Shooter("RawShooter")
+    unaug_loop_writer = control_loop.ControlLoopWriter("RawShooter",
+                                                       [unaug_sprung_shooter,
+                                                        unaug_shooter])
+    if argv[3][-3:] == '.cc':
+      unaug_loop_writer.Write(argv[4], argv[3])
+    else:
+      unaug_loop_writer.Write(argv[3], argv[4])
+
+    sprung_shooter = SprungShooterDeltaU()
+    shooter = ShooterDeltaU()
+    loop_writer = control_loop.ControlLoopWriter("Shooter", [sprung_shooter,
+                                                             shooter])
+
+    loop_writer.AddConstant(control_loop.Constant("kMaxExtension", "%f",
+                                                  sprung_shooter.max_extension))
+    loop_writer.AddConstant(control_loop.Constant("kSpringConstant", "%f",
+                                                  sprung_shooter.Ks))
+    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))