diff --git a/bot3/control_loops/python/control_loop.py b/bot3/control_loops/python/control_loop.py
new file mode 100644
index 0000000..881880f
--- /dev/null
+++ b/bot3/control_loops/python/control_loop.py
@@ -0,0 +1,338 @@
+import controls
+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)
+
+
+class ControlLoopWriter(object):
+  def __init__(self, gain_schedule_name, loops, namespaces=None, write_constants=False):
+    """Constructs a control loop writer.
+
+    Args:
+      gain_schedule_name: string, Name of the overall controller.
+      loops: array[ControlLoop], a list of control loops to gain schedule
+        in order.
+      namespaces: array[string], a list of names of namespaces to nest in
+        order.  If None, the default will be used.
+    """
+    self._gain_schedule_name = gain_schedule_name
+    self._loops = loops
+    if namespaces:
+      self._namespaces = namespaces
+    else:
+      self._namespaces = ['frc971', 'control_loops']
+
+    self._namespace_start = '\n'.join(
+        ['namespace %s {' % name for name in self._namespaces])
+
+    self._namespace_end = '\n'.join(
+        ['}  // namespace %s' % name for name in reversed(self._namespaces)])
+    
+    self._constant_list = []
+
+  def AddConstant(self, constant):
+    """Adds a constant to write.
+
+    Args:
+      constant: Constant, the constant to add to the header.
+    """
+    self._constant_list.append(constant)
+
+  def _TopDirectory(self):
+    return self._namespaces[0]
+
+  def _HeaderGuard(self, header_file):
+    return (self._TopDirectory().upper() + '_CONTROL_LOOPS_' +
+            header_file.upper().replace('.', '_').replace('/', '_') +
+            '_')
+
+  def Write(self, header_file, cc_file):
+    """Writes the loops to the specified files."""
+    self.WriteHeader(header_file)
+    self.WriteCC(header_file, cc_file)
+
+  def _GenericType(self, typename):
+    """Returns a loop template using typename for the type."""
+    num_states = self._loops[0].A.shape[0]
+    num_inputs = self._loops[0].B.shape[1]
+    num_outputs = self._loops[0].C.shape[0]
+    return '%s<%d, %d, %d>' % (
+        typename, num_states, num_inputs, num_outputs)
+
+  def _ControllerType(self):
+    """Returns a template name for StateFeedbackController."""
+    return self._GenericType('StateFeedbackController')
+
+  def _LoopType(self):
+    """Returns a template name for StateFeedbackLoop."""
+    return self._GenericType('StateFeedbackLoop')
+
+  def _PlantType(self):
+    """Returns a template name for StateFeedbackPlant."""
+    return self._GenericType('StateFeedbackPlant')
+
+  def _CoeffType(self):
+    """Returns a template name for StateFeedbackPlantCoefficients."""
+    return self._GenericType('StateFeedbackPlantCoefficients')
+
+  def WriteHeader(self, header_file, double_appendage=False, MoI_ratio=0.0):
+    """Writes the header file to the file named header_file.
+       Set double_appendage to true in order to include a ratio of
+       moments of inertia constant. Currently, only used for 2014 claw."""
+    with open(header_file, 'w') as fd:
+      header_guard = self._HeaderGuard(header_file)
+      fd.write('#ifndef %s\n'
+               '#define %s\n\n' % (header_guard, header_guard))
+      fd.write('#include \"frc971/control_loops/state_feedback_loop.h\"\n')
+      fd.write('\n')
+
+      fd.write(self._namespace_start)
+
+      for const in self._constant_list:
+          fd.write(str(const))
+
+      fd.write('\n\n')
+      for loop in self._loops:
+        fd.write(loop.DumpPlantHeader())
+        fd.write('\n')
+        fd.write(loop.DumpControllerHeader())
+        fd.write('\n')
+
+      fd.write('%s Make%sPlant();\n\n' %
+               (self._PlantType(), self._gain_schedule_name))
+
+      fd.write('%s Make%sLoop();\n\n' %
+               (self._LoopType(), self._gain_schedule_name))
+
+      fd.write(self._namespace_end)
+      fd.write('\n\n')
+      fd.write("#endif  // %s\n" % header_guard)
+
+  def WriteCC(self, header_file_name, cc_file):
+    """Writes the cc file to the file named cc_file."""
+    with open(cc_file, 'w') as fd:
+      fd.write('#include \"%s/control_loops/%s\"\n' %
+               (self._TopDirectory(), header_file_name))
+      fd.write('\n')
+      fd.write('#include <vector>\n')
+      fd.write('\n')
+      fd.write('#include \"frc971/control_loops/state_feedback_loop.h\"\n')
+      fd.write('\n')
+      fd.write(self._namespace_start)
+      fd.write('\n\n')
+      for loop in self._loops:
+        fd.write(loop.DumpPlant())
+        fd.write('\n')
+
+      for loop in self._loops:
+        fd.write(loop.DumpController())
+        fd.write('\n')
+
+      fd.write('%s Make%sPlant() {\n' %
+               (self._PlantType(), self._gain_schedule_name))
+      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] = ::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('}\n\n')
+
+      fd.write('%s Make%sLoop() {\n' %
+               (self._LoopType(), self._gain_schedule_name))
+      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] = ::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('}\n\n')
+
+      fd.write(self._namespace_end)
+      fd.write('\n')
+
+
+class ControlLoop(object):
+  def __init__(self, name):
+    """Constructs a control loop object.
+
+    Args:
+      name: string, The name of the loop to use when writing the C++ files.
+    """
+    self._name = name
+
+  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
+
+      Returns:
+        (A, B), numpy.matrix, the control matricies.
+    """
+    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 = self.C * self.X
+    self.X_hat = numpy.zeros((self.A.shape[0], 1))
+
+  def PlaceControllerPoles(self, poles):
+    """Places the controller poles.
+
+    Args:
+      poles: array, An array of poles.  Must be complex conjegates if they have
+        any imaginary portions.
+    """
+    self.K = controls.dplace(self.A, self.B, poles)
+
+  def PlaceObserverPoles(self, poles):
+    """Places the observer poles.
+
+    Args:
+      poles: array, An array of poles.  Must be complex conjegates if they have
+        any imaginary portions.
+    """
+    self.L = controls.dplace(self.A.T, self.C.T, poles).T
+
+  def Update(self, U):
+    """Simulates one time step with the provided U."""
+    #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
+
+  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 +
+                  self.L * (self.Y - self.C * self.X_hat - self.D * U))
+
+  def _DumpMatrix(self, matrix_name, matrix):
+    """Dumps the provided matrix into a variable called matrix_name.
+
+    Args:
+      matrix_name: string, The variable name to save the matrix to.
+      matrix: The matrix to dump.
+
+    Returns:
+      string, The C++ commands required to populate a variable named matrix_name
+        with the contents of matrix.
+    """
+    ans = ['  Eigen::Matrix<double, %d, %d> %s;\n' % (
+        matrix.shape[0], matrix.shape[1], matrix_name)]
+    first = True
+    for x in xrange(matrix.shape[0]):
+      for y in xrange(matrix.shape[1]):
+        element = matrix[x, y]
+        if first:
+          ans.append('  %s << ' % matrix_name)
+          first = False
+        else:
+          ans.append(', ')
+        ans.append(str(element))
+
+    ans.append(';\n')
+    return ''.join(ans)
+
+  def DumpPlantHeader(self):
+    """Writes out a c++ header declaration which will create a Plant object.
+
+    Returns:
+      string, The header declaration for the function.
+    """
+    num_states = self.A.shape[0]
+    num_inputs = self.B.shape[1]
+    num_outputs = self.C.shape[0]
+    return 'StateFeedbackPlantCoefficients<%d, %d, %d> Make%sPlantCoefficients();\n' % (
+        num_states, num_inputs, num_outputs, self._name)
+
+  def DumpPlant(self):
+    """Writes out a c++ function which will create a PlantCoefficients object.
+
+    Returns:
+      string, The function which will create the object.
+    """
+    num_states = self.A.shape[0]
+    num_inputs = self.B.shape[1]
+    num_outputs = self.C.shape[0]
+    ans = ['StateFeedbackPlantCoefficients<%d, %d, %d>'
+           ' Make%sPlantCoefficients() {\n' % (
+        num_states, num_inputs, num_outputs, self._name)]
+
+    ans.append(self._DumpMatrix('A', self.A))
+    ans.append(self._DumpMatrix('B', self.B))
+    ans.append(self._DumpMatrix('C', self.C))
+    ans.append(self._DumpMatrix('D', self.D))
+    ans.append(self._DumpMatrix('U_max', self.U_max))
+    ans.append(self._DumpMatrix('U_min', self.U_min))
+
+    ans.append('  return StateFeedbackPlantCoefficients<%d, %d, %d>'
+               '(A, B, C, D, U_max, U_min);\n' % (num_states, num_inputs,
+                                                  num_outputs))
+    ans.append('}\n')
+    return ''.join(ans)
+
+  def PlantFunction(self):
+    """Returns the name of the plant coefficient function."""
+    return 'Make%sPlantCoefficients()' % self._name
+
+  def ControllerFunction(self):
+    """Returns the name of the controller function."""
+    return 'Make%sController()' % self._name
+
+  def DumpControllerHeader(self):
+    """Writes out a c++ header declaration which will create a Controller object.
+
+    Returns:
+      string, The header declaration for the function.
+    """
+    num_states = self.A.shape[0]
+    num_inputs = self.B.shape[1]
+    num_outputs = self.C.shape[0]
+    return 'StateFeedbackController<%d, %d, %d> %s;\n' % (
+        num_states, num_inputs, num_outputs, self.ControllerFunction())
+
+  def DumpController(self):
+    """Returns a c++ function which will create a Controller object.
+
+    Returns:
+      string, The function which will create the object.
+    """
+    num_states = self.A.shape[0]
+    num_inputs = self.B.shape[1]
+    num_outputs = self.C.shape[0]
+    ans = ['StateFeedbackController<%d, %d, %d> %s {\n' % (
+        num_states, num_inputs, num_outputs, self.ControllerFunction())]
+
+    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, A_inv, Make%sPlantCoefficients());\n' % (
+                   num_states, num_inputs, num_outputs, self._name))
+    ans.append('}\n')
+    return ''.join(ans)
diff --git a/bot3/control_loops/python/controls.py b/bot3/control_loops/python/controls.py
new file mode 100644
index 0000000..b66bd56
--- /dev/null
+++ b/bot3/control_loops/python/controls.py
@@ -0,0 +1,153 @@
+#!/usr/bin/python
+
+"""
+Control loop pole placement library.
+
+This library will grow to support many different pole placement methods.
+Currently it only supports direct pole placement.
+"""
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+import numpy
+import slycot
+import scipy.signal.cont2discrete
+
+class Error (Exception):
+  """Base class for all control loop exceptions."""
+
+
+class PolePlacementError(Error):
+  """Exception raised when pole placement fails."""
+
+
+# TODO(aschuh): dplace should take a control system object.
+# There should also exist a function to manipulate laplace expressions, and
+# something to plot bode plots and all that.
+def dplace(A, B, poles, alpha=1e-6):
+  """Set the poles of (A - BF) to poles.
+
+  Args:
+    A: numpy.matrix(n x n), The A matrix.
+    B: numpy.matrix(n x m), The B matrix.
+    poles: array(imaginary numbers), The poles to use.  Complex conjugates poles
+      must be in pairs.
+
+  Raises:
+    ValueError: Arguments were the wrong shape or there were too many poles.
+    PolePlacementError: Pole placement failed.
+
+  Returns:
+    numpy.matrix(m x n), K
+  """
+  # See http://www.icm.tu-bs.de/NICONET/doc/SB01BD.html for a description of the
+  # fortran code that this is cleaning up the interface to.
+  n = A.shape[0]
+  if A.shape[1] != n:
+    raise ValueError("A must be square")
+  if B.shape[0] != n:
+    raise ValueError("B must have the same number of states as A.")
+  m = B.shape[1]
+
+  num_poles = len(poles)
+  if num_poles > n:
+    raise ValueError("Trying to place more poles than states.")
+
+  out = slycot.sb01bd(n=n,
+                      m=m,
+                      np=num_poles,
+                      alpha=alpha,
+                      A=A,
+                      B=B,
+                      w=numpy.array(poles),
+                      dico='D')
+
+  A_z = numpy.matrix(out[0])
+  num_too_small_eigenvalues = out[2]
+  num_assigned_eigenvalues = out[3]
+  num_uncontrollable_eigenvalues = out[4]
+  K = numpy.matrix(-out[5])
+  Z = numpy.matrix(out[6])
+
+  if num_too_small_eigenvalues != 0:
+    raise PolePlacementError("Number of eigenvalues that are too small "
+                             "and are therefore unmodified is %d." %
+                             num_too_small_eigenvalues)
+  if num_assigned_eigenvalues != num_poles:
+    raise PolePlacementError("Did not place all the eigenvalues that were "
+                             "requested. Only placed %d eigenvalues." %
+                             num_assigned_eigenvalues)
+  if num_uncontrollable_eigenvalues != 0:
+    raise PolePlacementError("Found %d uncontrollable eigenvlaues." %
+                             num_uncontrollable_eigenvalues)
+
+  return K
+
+
+def c2d(A, B, dt):
+  """Converts from continuous time state space representation to discrete time.
+     Returns (A, B).  C and D are unchanged."""
+
+  ans_a, ans_b, _, _, _ = scipy.signal.cont2discrete((A, B, None, None), dt)
+  return numpy.matrix(ans_a), numpy.matrix(ans_b)
+
+def ctrb(A, B):
+  """Returns the controlability matrix.
+
+    This matrix must have full rank for all the states to be controlable.
+  """
+  n = A.shape[0]
+  output = B
+  intermediate = B
+  for i in xrange(0, n):
+    intermediate = A * intermediate
+    output = numpy.concatenate((output, intermediate), axis=1)
+
+  return output
+
+def dlqr(A, B, Q, R):
+  """Solves for the optimal lqr controller.
+
+    x(n+1) = A * x(n) + B * u(n)
+    J = sum(0, inf, x.T * Q * x + u.T * R * u)
+  """
+
+  # 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(
+      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
+
+def kalman(A, B, C, Q, R):
+  """Solves for the steady state kalman gain and covariance matricies.
+
+    Args:
+      A, B, C: SS matricies.
+      Q: The model uncertantity
+      R: The measurement uncertainty
+
+    Returns:
+      KalmanGain, Covariance.
+  """
+  P = Q
+
+  I = numpy.matrix(numpy.eye(P.shape[0]))
+  At = A.T
+  Ct = C.T
+  i = 0
+
+  while True:
+    last_P = P
+    P_prior = A * P * At + Q
+    S = C * P_prior * Ct + R
+    K = P_prior * Ct * numpy.linalg.inv(S)
+    P = (I - K * C) * P_prior
+
+    diff = P - last_P
+    i += 1
+    if numpy.linalg.norm(diff) / numpy.linalg.norm(P) < 1e-9:
+      break
+
+  return K, P
diff --git a/bot3/control_loops/python/elevator3.py b/bot3/control_loops/python/elevator3.py
new file mode 100755
index 0000000..120e8cc
--- /dev/null
+++ b/bot3/control_loops/python/elevator3.py
@@ -0,0 +1,273 @@
+#!/usr/bin/python
+
+import control_loop
+import controls
+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 = 2.402
+    # Stall Current in Amps
+    self.stall_current = 126.145
+    # Free Speed in RPM
+    self.free_speed = 5015.562
+    # Free Current in Amps
+    self.free_current = 1.170
+    # Mass of the Elevator
+    if mass is None:
+      self.mass = 5.0
+    else:
+      self.mass = mass
+
+    # Number of motors
+    self.num_motors = 2.0
+    # Resistance of the motor
+    self.resistance = 12.0 / self.stall_current
+    # Motor velocity constant
+    self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+               (12.0 - self.resistance * self.free_current))
+    # Torque constant
+    self.Kt = (self.num_motors * self.stall_torque) / self.stall_current
+    # Gear ratio
+    self.G = 8
+    # Radius of pulley
+    self.r = 0.0254
+
+    # Control loop time step
+    self.dt = 0.005
+
+    # State is [position, velocity]
+    # Input is [Voltage]
+
+    C1 = self.Kt * self.G * self.G / (self.Kv * self.resistance * self.r * self.r * self.mass)
+    C2 = self.G * self.Kt / (self.resistance * self.r * self.mass)
+
+    self.A_continuous = numpy.matrix(
+        [[0, 1],
+         [0, -C1]])
+
+    # Start with the unmodified input
+    self.B_continuous = numpy.matrix(
+        [[0],
+         [C2]])
+
+    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)
+
+    controlability = controls.ctrb(self.A, self.B);
+
+    q_pos = 0.015
+    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 'K', self.K
+    print 'Poles are', numpy.linalg.eig(self.A - self.B * self.K)[0]
+
+    self.rpl = 0.30
+    self.ipl = 0.10
+    self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+                             self.rpl - 1j * self.ipl])
+
+    #    print 'L is', self.L
+
+    q_pos = 0.05
+    q_vel = 2.65
+    self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0],
+                           [0.0, (q_vel ** 2.0)]])
+
+    r_volts = 0.025
+    self.R = numpy.matrix([[(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)
+
+    #    print 'Kal', self.KalmanGain
+    self.L = self.A * self.KalmanGain
+    print 'KalL is', self.L
+
+    # 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()
+
+class IntegralElevator(Elevator):
+  def __init__(self, name="IntegralElevator", mass=None):
+    super(IntegralElevator, self).__init__(name=name, mass=mass)
+
+    self.A_continuous_unaugmented = self.A_continuous
+    self.B_continuous_unaugmented = self.B_continuous
+
+    self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+    self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
+    self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
+
+    self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+    self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
+
+    self.C_unaugmented = self.C
+    self.C = numpy.matrix(numpy.zeros((1, 3)))
+    self.C[0:1, 0:2] = self.C_unaugmented
+
+    self.A, self.B = self.ContinuousToDiscrete(self.A_continuous, self.B_continuous, self.dt)
+
+    q_pos = 0.08
+    q_vel = 0.40
+    q_voltage = 6.0
+    self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0],
+                           [0.0, (q_vel ** 2.0), 0.0],
+                           [0.0, 0.0, (q_voltage ** 2.0)]])
+
+    r_pos = 0.05
+    self.R = numpy.matrix([[(r_pos ** 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.L = self.A * self.KalmanGain
+
+    self.K_unaugmented = self.K
+    self.K = numpy.matrix(numpy.zeros((1, 3)))
+    self.K[0, 0:2] = self.K_unaugmented
+    self.K[0, 2] = 1
+
+    self.InitializeState()
+
+
+class ScenarioPlotter(object):
+  def __init__(self):
+    # Various lists for graphing things.
+    self.t = []
+    self.x = []
+    self.v = []
+    self.a = []
+    self.x_hat = []
+    self.u = []
+
+  def run_test(self, elevator, goal,
+               iterations=200, controller_elevator=None,
+               observer_elevator=None):
+    """Runs the Elevator plant with an initial condition and goal.
+
+      Args:
+        Elevator: elevator object to use.
+        initial_X: starting state.
+        goal: goal state.
+        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.
+    """
+
+    if controller_elevator is None:
+      controller_elevator = elevator
+
+    vbat = 10.0
+    if self.t:
+      initial_t = self.t[-1] + elevator.dt
+    else:
+      initial_t = 0
+    for i in xrange(iterations):
+      X_hat = elevator.X
+      if observer_elevator is not None:
+        X_hat = observer_elevator.X_hat
+        self.x_hat.append(observer_elevator.X_hat[0, 0])
+      gravity_compensation =  9.8 * elevator.mass * elevator.r / elevator.G / elevator.Kt * elevator.resistance
+
+      U = controller_elevator.K * (goal - X_hat)
+      U[0, 0] = numpy.clip(U[0, 0], -vbat , vbat )
+      self.x.append(elevator.X[0, 0])
+      if self.v:
+        last_v = self.v[-1]
+      else:
+        last_v = 0
+      self.v.append(elevator.X[1, 0])
+      self.a.append((self.v[-1] - last_v) / elevator.dt)
+
+      if observer_elevator is not None:
+        observer_elevator.Y = elevator.Y
+        observer_elevator.CorrectObserver(U)
+
+      elevator.Update(U - gravity_compensation)
+
+      if observer_elevator is not None:
+        observer_elevator.PredictObserver(U)
+
+      self.t.append(initial_t + i * elevator.dt)
+      self.u.append(U[0, 0])
+#      if numpy.abs((goal - X_hat)[0:2, 0]).sum() < .025:
+#        print "Time: ", self.t[-1]
+#        break
+
+    print "Time: ", self.t[-1]
+
+
+  def Plot(self):
+    pylab.subplot(3, 1, 1)
+    pylab.plot(self.t, self.x, label='x')
+    pylab.plot(self.t, self.x_hat, label='x_hat')
+    pylab.legend()
+
+    pylab.subplot(3, 1, 2)
+    pylab.plot(self.t, self.u, label='u')
+
+    pylab.subplot(3, 1, 3)
+    pylab.plot(self.t, self.a, label='a')
+
+    pylab.legend()
+    pylab.show()
+
+
+def main(argv):
+  loaded_mass = 7+4.0
+  #loaded_mass = 0
+  #observer_elevator = None
+
+  # Test moving the Elevator
+  initial_X = numpy.matrix([[0.0], [0.0]])
+  up_R = numpy.matrix([[0.4572], [0.0], [0.0]])
+  down_R = numpy.matrix([[0.0], [0.0], [0.0]])
+  totemass = 3.54
+  scenario_plotter = ScenarioPlotter()
+
+  elevator_controller = IntegralElevator(mass=4*totemass + loaded_mass)
+  observer_elevator = IntegralElevator(mass=4*totemass + loaded_mass)
+
+  for i in xrange(0, 7):
+    elevator = Elevator(mass=i*totemass + loaded_mass)
+    print 'Actual poles are', numpy.linalg.eig(elevator.A - elevator.B * elevator_controller.K[0, 0:2])[0]
+
+    elevator.X = initial_X
+    scenario_plotter.run_test(elevator, goal=up_R, controller_elevator=elevator_controller,
+                              observer_elevator=observer_elevator, iterations=200)
+    scenario_plotter.run_test(elevator, goal=down_R, controller_elevator=elevator_controller,
+                              observer_elevator=observer_elevator, iterations=200)
+
+  scenario_plotter.Plot()
+
+  # 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))