diff --git a/frc971/control_loops/python/arm.py b/frc971/control_loops/python/arm.py
new file mode 100755
index 0000000..c910f4c
--- /dev/null
+++ b/frc971/control_loops/python/arm.py
@@ -0,0 +1,249 @@
+#!/usr/bin/python
+
+import control_loop
+import controls
+import polytope
+import polydrivetrain
+import numpy
+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 = 3000.0
+
+    # State is [average position, average velocity,
+    #           position difference/2, velocity difference/2]
+    # Position difference is 1 - 2
+    # Input is [Voltage 1, Voltage 2]
+
+    C1 = self.spring / (self.J * 0.5)
+    C2 = self.Kt * self.G / (self.J * 0.5 * self.R)
+    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, -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)
+
+    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.01
+    q_vel_diff = 0.15
+    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]])
+
+    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 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
+
+  # Test moving the arm 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(arm, initial_X, R, controller_arm=arm_controller,
+           observer_arm=observer_arm)
+
+  # Write the generated constants out to a file.
+  if len(argv) != 3:
+    print "Expected .h file name and .cc file name for the arm."
+  else:
+    arm = Arm("Arm")
+    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])
+
+if __name__ == '__main__':
+  sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index b5ea6a1..1a25f95 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -6,357 +6,99 @@
 import polydrivetrain
 import numpy
 import sys
+import matplotlib
 from matplotlib import pylab
 
 class Claw(control_loop.ControlLoop):
-  def __init__(self, name="RawClaw"):
+  def __init__(self, name="Claw", mass=None):
     super(Claw, self).__init__(name)
     # Stall Torque in N m
-    self.stall_torque = 2.42
+    self.stall_torque = 0.476
     # Stall Current in Amps
-    self.stall_current = 133
+    self.stall_current = 80.730
     # Free Speed in RPM
-    self.free_speed = 5500.0
+    self.free_speed = 13906.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
+    self.free_current = 5.820
+    # Mass of the claw
+    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) /
-               (13.5 - self.R * self.free_current))
+               (12.0 - 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
+    self.G = (56.0 / 12.0) * (54.0 / 14.0) * (64.0 / 14.0) * (72.0 / 18.0)
+    # Claw length
+    self.r = 18 * 0.0254
 
-    # 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.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, 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]])
+         [0, -C1]])
 
-    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))
+    # Start with the unmodified input
     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]])
+         [C2]])
 
-    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.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.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"
+    q_pos = 0.03
+    q_vel = 0.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 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
+    self.rpl = 0.20
+    self.ipl = 0.05
+    self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+                             self.rpl - 1j * self.ipl])
 
-    #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]])
+    # 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()
 
-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.
-  """
+def run_test(claw, initial_X, goal, max_separation_error=0.01,
+             show_graph=True, iterations=200, controller_claw=None,
+             observer_claw=None):
+  """Runs the claw plant with an initial condition and goal.
 
-  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 
+    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
@@ -367,112 +109,76 @@
       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."""
+      iterations: Number of timesteps to run the model for.
+      controller_claw: claw object to get K from, or None if we should
+          use claw.
+      observer_claw: claw object to use for the observer, or None if we should
+          use the actual state.
+  """
 
   claw.X = initial_X
 
+  if controller_claw is None:
+    controller_claw = claw
+
+  if observer_claw is not None:
+    observer_claw.X_hat = initial_X + 0.01
+    observer_claw.X_hat = initial_X
+
   # Various lists for graphing things.
   t = []
-  x_bottom = []
-  x_top = []
-  u_bottom = []
-  u_top = []
-  x_separation = []
+  x = []
+  v = []
+  x_hat = []
+  u = []
 
-  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
+  sep_plot_gain = 100.0
 
   for i in xrange(iterations):
-    U = claw.K * (goal - claw.X)
-    U = ScaleU(claw, U, claw.K, goal - claw.X)
+    X_hat = claw.X
+    if observer_claw is not None:
+      X_hat = observer_claw.X_hat
+      x_hat.append(observer_claw.X_hat[0, 0])
+    U = controller_claw.K * (goal - X_hat)
+    U[0, 0] = numpy.clip(U[0, 0], -12, 12)
+    x.append(claw.X[0, 0])
+    v.append(claw.X[1, 0])
+    if observer_claw is not None:
+      observer_claw.PredictObserver(U)
     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)
+    if observer_claw is not None:
+      observer_claw.Y = claw.Y
+      observer_claw.CorrectObserver(U)
 
     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)
+    u.append(U[0, 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.subplot(2, 1, 1)
+    pylab.plot(t, x, label='x')
+    if observer_claw is not None:
+      pylab.plot(t, x_hat, label='x_hat')
+    pylab.legend()
+
+    pylab.subplot(2, 1, 2)
+    pylab.plot(t, u, label='u')
     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()
+  loaded_mass = 5
+  #loaded_mass = 0
+  claw = Claw(mass=5 + loaded_mass)
+  claw_controller = Claw(mass=5 + 5)
+  observer_claw = Claw(mass=5 + 5)
+  #observer_claw = None
 
   # 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
+  initial_X = numpy.matrix([[0.0], [0.0]])
+  R = numpy.matrix([[1.0], [0.0]])
+  run_test(claw, initial_X, R, controller_claw=claw_controller,
+           observer_claw=observer_claw)
 
   # Write the generated constants out to a file.
   if len(argv) != 3:
@@ -480,8 +186,6 @@
   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:
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index c3dd23e..ba4a43c 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -2,16 +2,16 @@
 import numpy
 
 class Constant(object):
-    def __init__ (self, name, formatt, value):
-        self.name = name
-        self.formatt = formatt
-        self.value = value
-        self.formatToType = {}
-        self.formatToType['%f'] = "double";
-        self.formatToType['%d'] = "int";
-    def __str__ (self):
-        return str("\nstatic constexpr %s %s = "+ self.formatt +";\n") % \
-            (self.formatToType[self.formatt], self.name, self.value)
+  def __init__ (self, name, formatt, value):
+    self.name = name
+    self.formatt = formatt
+    self.value = value
+    self.formatToType = {}
+    self.formatToType['%f'] = "double";
+    self.formatToType['%d'] = "int";
+  def __str__ (self):
+    return str("\nstatic constexpr %s %s = "+ self.formatt +";\n") % \
+        (self.formatToType[self.formatt], self.name, self.value)
 
 
 class ControlLoopWriter(object):
@@ -140,24 +140,24 @@
 
       fd.write('%s Make%sPlant() {\n' %
                (self._PlantType(), self._gain_schedule_name))
-      fd.write('  ::std::vector<%s *> plants(%d);\n' % (
+      fd.write('  ::std::vector< ::std::unique_ptr<%s>> plants(%d);\n' % (
           self._CoeffType(), len(self._loops)))
       for index, loop in enumerate(self._loops):
-        fd.write('  plants[%d] = new %s(%s);\n' %
-                 (index, self._CoeffType(),
+        fd.write('  plants[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
+                 (index, self._CoeffType(), self._CoeffType(),
                   loop.PlantFunction()))
-      fd.write('  return %s(plants);\n' % self._PlantType())
+      fd.write('  return %s(&plants);\n' % self._PlantType())
       fd.write('}\n\n')
 
       fd.write('%s Make%sLoop() {\n' %
                (self._LoopType(), self._gain_schedule_name))
-      fd.write('  ::std::vector<%s *> controllers(%d);\n' % (
+      fd.write('  ::std::vector< ::std::unique_ptr<%s>> controllers(%d);\n' % (
           self._ControllerType(), len(self._loops)))
       for index, loop in enumerate(self._loops):
-        fd.write('  controllers[%d] = new %s(%s);\n' %
-                 (index, self._ControllerType(),
+        fd.write('  controllers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
+                 (index, self._ControllerType(), self._ControllerType(),
                   loop.ControllerFunction()))
-      fd.write('  return %s(controllers);\n' % self._LoopType())
+      fd.write('  return %s(&controllers);\n' % self._LoopType())
       fd.write('}\n\n')
 
       fd.write(self._namespace_end)
@@ -216,6 +216,15 @@
     self.X = self.A * self.X + self.B * U
     self.Y = self.C * self.X + self.D * U
 
+  def PredictObserver(self, U):
+    """Runs the predict step of the observer update."""
+    self.X_hat = (self.A * self.X_hat + self.B * U)
+
+  def CorrectObserver(self, U):
+    """Runs the correct step of the observer update."""
+    self.X_hat += numpy.linalg.inv(self.A) * self.L * (
+        self.Y - self.C * self.X_hat - self.D * U)
+
   def UpdateObserver(self, U):
     """Updates the observer given the provided U."""
     self.X_hat = (self.A * self.X_hat + self.B * U +
@@ -320,9 +329,10 @@
 
     ans.append(self._DumpMatrix('L', self.L))
     ans.append(self._DumpMatrix('K', self.K))
+    ans.append(self._DumpMatrix('A_inv', numpy.linalg.inv(self.A)))
 
     ans.append('  return StateFeedbackController<%d, %d, %d>'
-               '(L, K, Make%sPlantCoefficients());\n' % (num_states, num_inputs,
-                                             num_outputs, self._name))
+               '(L, K, A_inv, Make%sPlantCoefficients());\n' % (
+                   num_states, num_inputs, num_outputs, self._name))
     ans.append('}\n')
     return ''.join(ans)
diff --git a/frc971/control_loops/python/controls.py b/frc971/control_loops/python/controls.py
index 64bd52d..57268a9 100644
--- a/frc971/control_loops/python/controls.py
+++ b/frc971/control_loops/python/controls.py
@@ -89,8 +89,8 @@
      integral(e^(A t) * B, 0, dt).
      Returns (A, B).  C and D are unchanged."""
   e, P = numpy.linalg.eig(A)
-  diag = numpy.matrix(numpy.eye(A.shape[0]))
-  diage = numpy.matrix(numpy.eye(A.shape[0]))
+  diag = numpy.matrix(numpy.eye(A.shape[0]), dtype=numpy.complex128)
+  diage = numpy.matrix(numpy.eye(A.shape[0]), dtype=numpy.complex128)
   for eig, count in zip(e, range(0, A.shape[0])):
     diag[count, count] = numpy.exp(eig * dt)
     if abs(eig) < 1.0e-16:
@@ -98,7 +98,13 @@
     else:
       diage[count, count] = (numpy.exp(eig * dt) - 1.0) / eig
 
-  return (P * diag * numpy.linalg.inv(P), P * diage * numpy.linalg.inv(P) * B)
+  ans_a = P * diag * numpy.linalg.inv(P)
+  ans_b = P * diage * numpy.linalg.inv(P) * B
+  if numpy.abs(ans_a.imag).sum() / numpy.abs(ans_a).sum() < 1e-6:
+    ans_a = ans_a.real
+  if numpy.abs(ans_b.imag).sum() / numpy.abs(ans_b).sum() < 1e-6:
+    ans_b = ans_b.real
+  return (ans_a, ans_b)
 
 def ctrb(A, B):
   """Returns the controlability matrix.
@@ -123,7 +129,8 @@
 
   # P = (A.T * P * A) - (A.T * P * B * numpy.linalg.inv(R + B.T * P *B) * (A.T * P.T * B).T + Q
 
-  P, rcond, w, S, T = slycot.sb02od(A.shape[0], B.shape[1], A, B, Q, R, 'D')
+  P, rcond, w, S, T = slycot.sb02od(
+      n=A.shape[0], m=B.shape[1], A=A, B=B, Q=Q, R=R, dico='D')
 
   F = numpy.linalg.inv(R + B.T * P *B) * B.T * P * A
   return F
diff --git a/frc971/control_loops/python/elevator.py b/frc971/control_loops/python/elevator.py
new file mode 100755
index 0000000..2b42945
--- /dev/null
+++ b/frc971/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 = 3000.0
+
+    # State is [average position, average velocity,
+    #           position difference/2, velocity difference/2]
+    # Input is [V1, V2]
+
+    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))