diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index 3d6b9fc..b1f378e 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -1,6 +1,7 @@
 #!/usr/bin/python
 
 import control_loop
+import controls
 import numpy
 import sys
 from matplotlib import pylab
@@ -32,28 +33,47 @@
     # Control loop time step
     self.dt = 0.01
 
+    # State is [bottom position, top - bottom position,
+    #           bottom velocity, top - bottom velocity]
+    # Input is [bottom power, top power]
+    # Motor time constant.
+    self.motor_timeconstant = self.Kt / self.Kv / (self.J * self.G * self.G * self.R)
     # State feedback matrices
     self.A_continuous = numpy.matrix(
-        [[0, 1],
-         [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+        [[0, 0, 1, 0],
+         [0, 0, 0, 1],
+         [0, 0, -self.motor_timeconstant, 0],
+         [0, 0, 0, -self.motor_timeconstant]])
+
+    self.motor_feedforward = self.Kt / (self.J * 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]])
+        [[0, 0],
+         [0, 0],
+         [self.motor_feedforward, 0],
+         [-self.motor_feedforward, self.motor_feedforward]])
+    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.PlaceControllerPoles([0.85, 0.45])
+    #controlability = controls.ctrb(self.A, self.B);
+    #print "Rank of controlability matrix.", numpy.linalg.matrix_rank(controlability)
+
+    self.PlaceControllerPoles([0.85, 0.45, 0.45, 0.85])
 
     self.rpl = .05
     self.ipl = 0.008
     self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+                             self.rpl - 1j * self.ipl,
+                             self.rpl + 1j * self.ipl,
                              self.rpl - 1j * self.ipl])
 
-    self.U_max = numpy.matrix([[12.0]])
-    self.U_min = numpy.matrix([[-12.0]])
+    self.U_max = numpy.matrix([[12.0], [12.0]])
+    self.U_min = numpy.matrix([[-12.0], [-12.0]])
 
     self.InitializeState()
 
@@ -63,72 +83,138 @@
     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, 1.0]])
-    self.A[0:2, 0:2] = A_unaugmented
-    self.A[0:2, 2] = B_unaugmented
+    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],
-                           [1.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.matrix([[1.0, 0.0, 0.0]])
-    self.D = numpy.matrix([[0.0]])
+    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.80])
+    #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,
+    self.PlaceObserverPoles([self.rpl + 1j * self.ipl, 0.10, 0.09,
                              self.rpl - 1j * self.ipl, 0.90])
-    print "Placed observer poles are"
-    print numpy.linalg.eig(self.A - self.L * self.C)[0]
+    #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.U_max = numpy.matrix([[12.0]])
-    self.U_min = numpy.matrix([[-12.0]])
+    #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 ClipDeltaU(claw, delta_u):
-  old_u = numpy.matrix([[claw.X[2, 0]]])
-  new_u = numpy.clip(old_u + delta_u, claw.U_min, claw.U_max)
-  return new_u - old_u
+def ClipDeltaU(claw, U):
+  delta_u = U[0, 0]
+  top_u = U[1, 0]
+  old_bottom_u = claw.X[4, 0]
+
+  # TODO(austin): Preserve the difference between the top and bottom power.
+  new_unclipped_bottom_u = old_bottom_u + delta_u
+
+  #print "Bottom is", new_unclipped_bottom_u, "Top is", top_u
+  if new_unclipped_bottom_u > claw.U_max[0, 0]:
+    #print "Bottom is too big.  Was", new_unclipped_bottom_u, "changing top by", new_unclipped_bottom_u - claw.U_max[0, 0]
+    top_u -= new_unclipped_bottom_u - claw.U_max[0, 0]
+    new_unclipped_bottom_u = claw.U_max[0, 0]
+  if top_u > claw.U_max[1, 0]:
+    new_unclipped_bottom_u -= top_u - claw.U_max[1, 0]
+    top_u = claw.U_max[1, 0]
+  if top_u < claw.U_min[1, 0]:
+    new_unclipped_bottom_u -= top_u - claw.U_min[1, 0]
+    top_u = claw.U_min[1, 0]
+  if new_unclipped_bottom_u < claw.U_min[0, 0]:
+    top_u -= new_unclipped_bottom_u - claw.U_min[0, 0]
+    new_unclipped_bottom_u = claw.U_min[0, 0]
+
+  new_bottom_u = numpy.clip(new_unclipped_bottom_u, claw.U_min[0, 0], claw.U_max[0, 0])
+  new_top_u = numpy.clip(top_u, claw.U_min[1, 0], claw.U_max[1, 0])
+
+  return numpy.matrix([[new_bottom_u - old_bottom_u], [new_top_u]])
 
 def main(argv):
   # Simulate the response of the system to a step input.
-  claw = ClawDeltaU()
-  simulated_x = []
-  for _ in xrange(100):
-    claw.Update(numpy.matrix([[12.0]]))
-    simulated_x.append(claw.X[0, 0])
+  #claw = ClawDeltaU()
+  #simulated_x = []
+  #for _ in xrange(100):
+  #  claw.Update(numpy.matrix([[12.0]]))
+  #  simulated_x.append(claw.X[0, 0])
 
-  pylab.plot(range(100), simulated_x)
-  pylab.show()
+  #pylab.plot(range(100), simulated_x)
+  #pylab.show()
 
   # Simulate the closed loop response of the system to a step input.
   top_claw = ClawDeltaU("TopClaw")
-  close_loop_x = []
-  close_loop_u = []
-  R = numpy.matrix([[1.0], [0.0], [0.0]])
-  top_claw.X[2, 0] = -5
-  for _ in xrange(100):
-    U = numpy.clip(top_claw.K * (R - top_claw.X_hat), top_claw.U_min, top_claw.U_max)
+  close_loop_x_bottom = []
+  close_loop_x_sep = []
+  close_loop_u_bottom = []
+  close_loop_u_top = []
+  R = numpy.matrix([[1.0], [0.0], [0.0], [0.0], [0.0]])
+  top_claw.X[0, 0] = 0
+  for _ in xrange(50):
+    #print "Error is", (R - top_claw.X_hat)
+    U = top_claw.K * (R - top_claw.X_hat)
     U = ClipDeltaU(top_claw, U)
     top_claw.UpdateObserver(U)
     top_claw.Update(U)
-    close_loop_x.append(top_claw.X[0, 0] * 10)
-    close_loop_u.append(top_claw.X[2, 0])
+    close_loop_x_bottom.append(top_claw.X[0, 0] * 10)
+    close_loop_u_bottom.append(top_claw.X[4, 0])
+    close_loop_x_sep.append(top_claw.X[1, 0] * 10)
+    close_loop_u_top.append(U[1, 0])
 
-  pylab.plot(range(100), close_loop_x)
-  pylab.plot(range(100), close_loop_u)
+  pylab.plot(range(50), close_loop_x_bottom, label='x bottom')
+  pylab.plot(range(50), close_loop_u_bottom, label='u bottom')
+  pylab.plot(range(50), close_loop_x_sep, label='seperation')
+  pylab.plot(range(50), close_loop_u_top, label='u top')
+  pylab.legend()
   pylab.show()
 
   # Write the generated constants out to a file.