diff --git a/frc971/control_loops/python/angle_adjust.py b/frc971/control_loops/python/angle_adjust.py
index bee58bc..3c87e86 100755
--- a/frc971/control_loops/python/angle_adjust.py
+++ b/frc971/control_loops/python/angle_adjust.py
@@ -6,8 +6,8 @@
 from matplotlib import pylab
 
 class AngleAdjust(control_loop.ControlLoop):
-  def __init__(self):
-    super(AngleAdjust, self).__init__("AngleAdjust")
+  def __init__(self, name="AngleAdjustRaw"):
+    super(AngleAdjust, self).__init__(name)
     # Stall Torque in N m
     self.stall_torque = .428
     # Stall Current in Amps
@@ -41,11 +41,14 @@
     self.C = numpy.matrix([[1, 0]])
     self.D = numpy.matrix([[0]])
 
-    self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
-                              self.dt, self.C)
+    self.A, self.B = self.ContinuousToDiscrete(
+        self.A_continuous, self.B_continuous, self.dt)
 
     self.PlaceControllerPoles([.45, .8])
 
+    print "Unaugmented controller poles at"
+    print self.K
+
     self.rpl = .05
     self.ipl = 0.008
     self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
@@ -54,6 +57,47 @@
     self.U_max = numpy.matrix([[12.0]])
     self.U_min = numpy.matrix([[-12.0]])
 
+    self.InitializeState()
+
+class AngleAdjustDeltaU(AngleAdjust):
+  def __init__(self, name="AngleAdjust"):
+    super(AngleAdjustDeltaU, 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.60, 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.15])
+    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 main(argv):
   # Simulate the response of the system to a step input.
   angle_adjust_data = numpy.genfromtxt(
@@ -76,9 +120,9 @@
   pylab.show()
 
   # Simulate the closed loop response of the system to a step input.
-  angle_adjust = AngleAdjust()
+  angle_adjust = AngleAdjustDeltaU()
   close_loop_x = []
-  R = numpy.matrix([[1.0], [0.0]])
+  R = numpy.matrix([[1.0], [0.0], [0.0]])
   for _ in xrange(100):
     U = numpy.clip(angle_adjust.K * (R - angle_adjust.X_hat), angle_adjust.U_min, angle_adjust.U_max)
     angle_adjust.UpdateObserver(U)
@@ -89,9 +133,16 @@
   pylab.show()
 
   # Write the generated constants out to a file.
-  if len(argv) != 3:
+  if len(argv) != 5:
     print "Expected .cc file name and .h file name"
   else:
+    loop_writer = control_loop.ControlLoopWriter("RawAngleAdjust",
+                                                 [AngleAdjust()])
+    if argv[3][-3:] == '.cc':
+      loop_writer.Write(argv[4], argv[3])
+    else:
+      loop_writer.Write(argv[3], argv[4])
+
     loop_writer = control_loop.ControlLoopWriter("AngleAdjust", [angle_adjust])
     if argv[1][-3:] == '.cc':
       loop_writer.Write(argv[2], argv[1])
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 4ff2623..754ba62 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -140,20 +140,23 @@
     """
     self._name = name
 
-  def ContinuousToDiscrete(self, A_continuous, B_continuous, dt, C):
-    """Calculates the discrete time values for A and B as well as initializing
-      X and Y to the correct sizes.
+  def ContinuousToDiscrete(self, A_continuous, B_continuous, dt):
+    """Calculates the discrete time values for A and B.
 
       Args:
         A_continuous: numpy.matrix, The continuous time A matrix
         B_continuous: numpy.matrix, The continuous time B matrix
         dt: float, The time step of the control loop
-        C: C
+
+      Returns:
+        (A, B), numpy.matrix, the control matricies.
     """
-    self.A, self.B = controls.c2d(
-        A_continuous, B_continuous, dt)
+    return controls.c2d(A_continuous, B_continuous, dt)
+
+  def InitializeState(self):
+    """Sets X, Y, and X_hat to zero defaults."""
     self.X = numpy.zeros((self.A.shape[0], 1))
-    self.Y = C * self.X
+    self.Y = self.C * self.X
     self.X_hat = numpy.zeros((self.A.shape[0], 1))
 
   def PlaceControllerPoles(self, poles):
diff --git a/frc971/control_loops/python/wrist.py b/frc971/control_loops/python/wrist.py
index f26b99a..f21ed03 100755
--- a/frc971/control_loops/python/wrist.py
+++ b/frc971/control_loops/python/wrist.py
@@ -6,8 +6,8 @@
 from matplotlib import pylab
 
 class Wrist(control_loop.ControlLoop):
-  def __init__(self):
-    super(Wrist, self).__init__("Wrist")
+  def __init__(self, name="RawWrist"):
+    super(Wrist, self).__init__(name)
     # Stall Torque in N m
     self.stall_torque = 1.4
     # Stall Current in Amps
@@ -42,12 +42,10 @@
     self.C = numpy.matrix([[1, 0]])
     self.D = numpy.matrix([[0]])
 
-    self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
-                              self.dt, self.C)
+    self.A, self.B = self.ContinuousToDiscrete(
+        self.A_continuous, self.B_continuous, self.dt)
 
-    self.PlaceControllerPoles([.86, .46])
-
-    print self.K
+    self.PlaceControllerPoles([0.85, 0.45])
 
     self.rpl = .05
     self.ipl = 0.008
@@ -57,9 +55,56 @@
     self.U_max = numpy.matrix([[12.0]])
     self.U_min = numpy.matrix([[-12.0]])
 
+    self.InitializeState()
+
+
+class WristDeltaU(Wrist):
+  def __init__(self, name="Wrist"):
+    super(WristDeltaU, 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.60, 0.37, 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.15])
+    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(wrist, delta_u):
+  old_u = numpy.matrix([[wrist.X[2, 0]]])
+  new_u = numpy.clip(old_u + delta_u, wrist.U_min, wrist.U_max)
+  return new_u - old_u
+
 def main(argv):
   # Simulate the response of the system to a step input.
-  wrist = Wrist()
+  wrist = WristDeltaU()
   simulated_x = []
   for _ in xrange(100):
     wrist.Update(numpy.matrix([[12.0]]))
@@ -69,22 +114,36 @@
   pylab.show()
 
   # Simulate the closed loop response of the system to a step input.
-  wrist = Wrist()
+  wrist = WristDeltaU()
   close_loop_x = []
-  R = numpy.matrix([[1.0], [0.0]])
+  close_loop_u = []
+  R = numpy.matrix([[1.0], [0.0], [0.0]])
+  wrist.X[2, 0] = -5
   for _ in xrange(100):
     U = numpy.clip(wrist.K * (R - wrist.X_hat), wrist.U_min, wrist.U_max)
+    U = ClipDeltaU(wrist, U)
     wrist.UpdateObserver(U)
     wrist.Update(U)
-    close_loop_x.append(wrist.X[0, 0])
+    close_loop_x.append(wrist.X[0, 0] * 10)
+    close_loop_u.append(wrist.X[2, 0])
 
   pylab.plot(range(100), close_loop_x)
+  pylab.plot(range(100), close_loop_u)
   pylab.show()
 
   # Write the generated constants out to a file.
-  if len(argv) != 3:
-    print "Expected .h file name and .cc file name"
+  if len(argv) != 5:
+    print "Expected .h file name and .cc file name for"
+    print "both the plant and unaugmented plant"
   else:
+    unaug_wrist = Wrist("RawWrist")
+    unaug_loop_writer = control_loop.ControlLoopWriter("RawWrist",
+                                                       [unaug_wrist])
+    if argv[3][-3:] == '.cc':
+      unaug_loop_writer.Write(argv[4], argv[3])
+    else:
+      unaug_loop_writer.Write(argv[3], argv[4])
+
     loop_writer = control_loop.ControlLoopWriter("Wrist", [wrist])
     if argv[1][-3:] == '.cc':
       loop_writer.Write(argv[2], argv[1])