diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index a801649..b0767fb 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -40,14 +40,14 @@
     # 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
+    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, 0, 0],
-         [0, 0, 0, 0],
+        [[0, 0, 1, 0],
+         [0, 0, 0, 1],
          [0, 0, self.bottom_bottom, 0],
          [0, 0, self.difference_bottom, self.difference_difference]])
 
@@ -59,9 +59,9 @@
         [[0, 1],
          [0, self.difference_difference]])
 
-    self.A_continuous[0:2, 0:2] = self.A_bottom_cont
-    self.A_continuous[2:4, 2:4] = self.A_diff_cont
-    self.A_continuous[3, 1] = self.difference_bottom
+  # self.A_continuous[0:2, 0:2] = self.A_bottom_cont
+  # self.A_continuous[2:4, 2:4] = self.A_diff_cont
+  # self.A_continuous[3, 1] = self.difference_bottom
 
     self.motor_feedforward = self.Kt / (self.G * self.R)
     self.motor_feedforward_bottom = self.motor_feedforward / self.J_bottom
@@ -70,11 +70,13 @@
         self.motor_feedforward * (1 / self.J_bottom - 1 / self.J_top))
     self.B_continuous = numpy.matrix(
         [[0, 0],
-         [self.motor_feedforward_bottom, 0],
          [0, 0],
-         [0,#self.motor_feedforward_difference_bottom,
+         [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]])
@@ -84,7 +86,7 @@
          [self.motor_feedforward_difference]])
 
     self.C = numpy.matrix([[1, 0, 0, 0],
-                           [1, 0, 1, 0]])
+                           [1, 1, 0, 0]])
     self.D = numpy.matrix([[0, 0],
                            [0, 0]])
 
@@ -96,28 +98,27 @@
     self.A_diff, self.B_diff = controls.c2d(
         self.A_diff_cont, self.B_diff_cont, self.dt)
 
-    print "A, A_bot, A_diff:"
-    print self.A, self.A_bottom, self.A_diff
-    print "B, B_bot, B_diff:"
-    print self.B, self.B_bottom, self.B_diff
-    
-    # If B should equal [[B_bot, 0],[0, B_diff]], then the B
-    # generated by ContinuousToDiscrete adds in a couple extra
-    # numbers which make it impossible to control 4 values in K.
-    # Here, I make B equal what I had thought it should, thereby
-    # allowing us to control 4 values in K.
-    self.B_actual = numpy.matrix(self.B)
-    self.B[2, 0] = 0.0
-    self.B[3, 0] = 0.0
-    # If we do the above, with setting values of B to zero,
-    # then we can no longer make all the necessary values of A - B * K
-    # zero, because the discrete transform added a term affecting position
-    # over the timestep and that term can no longer be cancelled out if we
-    # are to also cancel out the term where the velocity of the bottom
-    # affects the velocity of the separation.
-    self.A_actual = numpy.matrix(self.A)
-    self.A[2, 1] = self.A[3, 1] * self.B[2, 1] / self.B[3, 1]
+    print "A"
+    print self.A
+    print "B"
+    print self.B
 
+    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
+    
     #controlability = controls.ctrb(self.A, self.B);
     #print "Rank of controlability matrix.", numpy.linalg.matrix_rank(controlability)
 
@@ -130,20 +131,20 @@
                            [0.0, (1.0 / (5.0 ** 2.0))]])
    #self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
 
-    # TODO(james): Fix this for discrete time domain.
-    self.K = numpy.matrix([[0, 0, 0.0, 0.0],
-                           [0.0, self.A[3, 1] / self.B[3, 1], 0, 0]])
-    self.K_bottom = controls.dplace(self.A_bottom, self.B_bottom, [0.6, 0.7])
-    self.K_diff = controls.dplace(self.A_diff, self.B_diff, [0.3, 0.5])
-    self.K[0, 0:2] = self.K_bottom
-    self.K[1, 2:4] = self.K_diff
-   #lstsq_A = numpy.identity(2)
-   #lstsq_A[0] = self.B[1]
-   #lstsq_A[1] = self.B[3]
+    self.K = numpy.matrix([[50, 0.0, 1, 0.0],
+                           [0.0, 300, 0.0, 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)
+    self.K[1, 0] = -lstsq_A[0, 0] * self.K[0, 0] / lstsq_A[0, 1]
    #self.K[0:2, 0] = numpy.linalg.lstsq(lstsq_A, numpy.matrix([[0.0], [0.0]]))[0]
-   #self.K[0:2, 2] = numpy.linalg.lstsq(
-   #                     lstsq_A,
-   #                     numpy.matrix([[self.A[1, 2]], [self.A[3, 2]]]))[0]
+    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
@@ -277,20 +278,47 @@
 
   return numpy.matrix([[bottom_u], [top_u - bottom_u]])
 
-def AverageUFix(claw, U):
-  bottom_u = U[0, 0]
-  top_u = bottom_u + U[1, 0]
-  top_u = bottom_u * claw.J_top / claw.J_bottom + U[1, 0] 
+def AverageUFix(claw, U, preserve_v3=False):
+  """Clips U as necessary.
 
-  #print "Bottom is", new_unclipped_bottom_u, "Top is", top_u
-  if (bottom_u > claw.U_max[0, 0] or top_u > claw.U_max[0, 0] or
-      top_u < claw.U_min[0, 0] or bottom_u < claw.U_min[0, 0]):
-    scalar = 12.0 / max(numpy.abs(top_u), numpy.abs(bottom_u))
+    Args:
+      claw: claw object containing moments of inertia and U limits.
+      U: Input matrix to clip as necessary.
+      preserve_v3: There are two ways to attempt to clip the voltages:
+        -If you preserve the imaginary v3, then it will attempt to 
+          preserve the effect on the separation of the two claws.
+          If it is not able to do this, then it calls itself with preserve_v3
+          set to False.
+        -If you preserve the ratio of the voltage of the bottom and the top,
+          then it will attempt to preserve the ratio of those two. This is
+          equivalent to preserving the ratio of v3 and the bottom voltage.
+  """
+  bottom_u = U[0, 0]
+  top_u = U[1, 0]
+  seperation_u = top_u - bottom_u * claw.J_top / claw.J_bottom
+
+  top_big = top_u > claw.U_max[0, 0]
+  top_small = top_u < claw.U_min[0, 0]
+  bot_big = bottom_u > claw.U_max[0, 0]
+  bot_small = top_u < claw.U_min[0, 0]
+  bottom_bad = bot_big or bot_small
+  top_bad = top_big or top_small
+  scalar = claw.U_max[0, 0] / max(numpy.abs(top_u), numpy.abs(bottom_u))
+  if bottom_bad and preserve_v3:
+    bottom_u *= scalar
+    top_u = seperation_u + bottom_u * claw.J_top / claw.J_bottom
+    if abs(top_u) > claw.U_max[0, 0]:
+      return AverageUFix(claw, U, preserve_v3=False)
+  elif top_bad and preserve_v3:
+    top_u *= scalar
+    bottom_u = (top_u - seperation_u) * claw.J_bottom / claw.J_top
+    if abs(bottom_u) > claw.U_max[0, 0]:
+      return AverageUFix(claw, U, preserve_v3=False)
+  elif (bottom_bad or top_bad) and not preserve_v3:
     top_u *= scalar
     bottom_u *= scalar
 
- #return numpy.matrix([[bottom_u], [top_u - bottom_u]])
-  return numpy.matrix([[bottom_u], [top_u - bottom_u * claw.J_top / claw.J_bottom]])
+  return numpy.matrix([[bottom_u], [top_u]])
 
 def ClipDeltaU(claw, U):
   delta_u = U[0, 0]
@@ -343,35 +371,34 @@
   close_loop_u_top = []
   R = numpy.matrix([[0.0], [0.00], [0.0], [0.0]])
   claw.X[0, 0] = 1
-  claw.X_hat[0, 0] = 1
-  X_actual = claw.X
-  print "B actual"
-  print claw.B_actual
+  claw.X[1, 0] = .0
+  claw.X_hat = claw.X
+ #X_actual = claw.X
   for i in xrange(100):
     #print "Error is", (R - claw.X_hat)
-    U = claw.K * (R - claw.X_hat)
+    U = claw.K * (R - claw.X)
     #U = numpy.clip(claw.K * (R - claw.X_hat), claw.U_min, claw.U_max)
     #U = FullSeparationPriority(claw, U)
-   #U = AverageUFix(claw, U)
+   #U = AverageUFix(claw, U, preserve_v3=True)
     #U = claw.K * (R - claw.X_hat)
     #U = ClipDeltaU(claw, U)
     claw.UpdateObserver(U)
-   #claw.Update(U)
-    X_actual = claw.A_actual * X_actual + claw.B_actual * U
-    claw.Y = claw.C * X_actual
-    close_loop_x_bottom.append(claw.X_hat[0, 0] * 10)
+    claw.Update(U)
+   #X_actual = claw.A_actual * X_actual + claw.B_actual * U
+   #claw.Y = claw.C * X_actual
+    close_loop_x_bottom.append(claw.X[0, 0] * 10)
     close_loop_u_bottom.append(U[0, 0])
-    actual_sep.append(X_actual[2, 0] * 100)
-    actual_x_bottom.append(X_actual[0, 0] * 10)
-    close_loop_x_sep.append(claw.X_hat[2, 0] * 100)
-    close_loop_x_top.append((claw.X_hat[2, 0] + claw.X_hat[0, 0]) * 10)
-    close_loop_u_top.append(U[1, 0] + U[0, 0] * claw.J_top / claw.J_bottom)
+   #actual_sep.append(X_actual[2, 0] * 100)
+   #actual_x_bottom.append(X_actual[0, 0] * 10)
+    close_loop_x_sep.append(claw.X[1, 0] * 100)
+    close_loop_x_top.append((claw.X[1, 0] + claw.X[0, 0]) * 10)
+    close_loop_u_top.append(U[1, 0])
     t.append(0.01 * i)
 
   pylab.plot(t, close_loop_x_bottom, label='x bottom')
   pylab.plot(t, close_loop_x_sep, label='separation')
-  pylab.plot(t, actual_x_bottom, label='true x bottom')
-  pylab.plot(t, actual_sep, label='true separation')
+ #pylab.plot(t, actual_x_bottom, label='true x bottom')
+ #pylab.plot(t, actual_sep, label='true separation')
   pylab.plot(t, close_loop_x_top, label='x top')
   pylab.plot(t, close_loop_u_bottom, label='u bottom')
   pylab.plot(t, close_loop_u_top, label='u top')
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 4b63aec..0610225 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -183,7 +183,7 @@
 
   def Update(self, U):
     """Simulates one time step with the provided U."""
-    U = numpy.clip(U, self.U_min, self.U_max)
+   #U = numpy.clip(U, self.U_min, self.U_max)
     self.X = self.A * self.X + self.B * U
     self.Y = self.C * self.X + self.D * U