diff --git a/bot3/control_loops/python/control_loop.py b/bot3/control_loops/python/control_loop.py
new file mode 100644
index 0000000..754ba62
--- /dev/null
+++ b/bot3/control_loops/python/control_loop.py
@@ -0,0 +1,295 @@
+import controls
+import numpy
+
+class ControlLoopWriter(object):
+  def __init__(self, gain_schedule_name, loops, namespaces=None):
+    """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)])
+
+  def _HeaderGuard(self, header_file):
+    return ('FRC971_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):
+    """Writes the header file to the file named header_file."""
+    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)
+      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 \"frc971/control_loops/%s\"\n' % 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<%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(),
+                  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<%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(),
+                  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 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('  return StateFeedbackController<%d, %d, %d>'
+               '(L, K, 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..a40bfe2
--- /dev/null
+++ b/bot3/control_loops/python/controls.py
@@ -0,0 +1,101 @@
+#!/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
+
+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.
+     Evaluates e^(A dt) for the discrete time version of A, and
+     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]))
+  for eig, count in zip(e, range(0, A.shape[0])):
+    diag[count, count] = numpy.exp(eig * dt)
+    if abs(eig) < 1.0e-16:
+      diage[count, count] = dt
+    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)
diff --git a/bot3/control_loops/python/drivetrain.py b/bot3/control_loops/python/drivetrain.py
new file mode 100755
index 0000000..0e791cb
--- /dev/null
+++ b/bot3/control_loops/python/drivetrain.py
@@ -0,0 +1,162 @@
+#!/usr/bin/python
+
+import control_loop
+import numpy
+import sys
+from matplotlib import pylab
+
+class Drivetrain(control_loop.ControlLoop):
+  def __init__(self):
+    super(Drivetrain, self).__init__("Drivetrain")
+    # Stall Torque in N m
+    self.stall_torque = 2.42
+    # Stall Current in Amps
+    self.stall_current = 133
+    # Free Speed in RPM. Used number from last year.
+    self.free_speed = 4650.0
+    # Free Current in Amps
+    self.free_current = 2.7
+    # Moment of inertia of the drivetrain in kg m^2
+    # Just borrowed from last year.
+    self.J = 6.4
+    # Mass of the robot, in kg.
+    self.m = 68
+    # Radius of the robot, in meters (from last year).
+    self.rb = 0.617998644 / 2.0
+    # Radius of the wheels, in meters.
+    self.r = .04445
+    # Resistance of the motor, divided by the number of motors.
+    self.R = 12.0 / self.stall_current / 6 + 0.03
+    # 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 ratios
+    self.G_low = 16.0 / 60.0 * 19.0 / 50.0
+    self.G_high = 28.0 / 48.0 * 19.0 / 50.0
+    self.G = self.G_low
+    # Control loop time step
+    self.dt = 0.01
+
+    # These describe the way that a given side of a robot will be influenced
+    # by the other side. Units of 1 / kg.
+    self.msp = 1.0 / self.m + self.rb * self.rb / self.J
+    self.msn = 1.0 / self.m - self.rb * self.rb / self.J
+    # The calculations which we will need for A and B.
+    self.tc = -self.Kt / self.Kv / (self.G * self.G * self.R * self.r * self.r)
+    self.mp = self.Kt / (self.G * self.R * self.r)
+
+    # State feedback matrices
+    # X will be of the format
+    # [[position1], [velocity1], [position2], velocity2]]
+    self.A_continuous = numpy.matrix(
+        [[0, 1, 0, 0],
+         [0, self.msp * self.tc, 0, self.msn * self.tc],
+         [0, 0, 0, 1],
+         [0, self.msn * self.tc, 0, self.msp * self.tc]])
+    self.B_continuous = numpy.matrix(
+        [[0, 0],
+         [self.msp * self.mp, self.msn * self.mp],
+         [0, 0],
+         [self.msn * self.mp, self.msp * self.mp]])
+    self.C = numpy.matrix([[1, 0, 0, 0],
+                           [0, 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)
+
+    # Poles from last year.
+    self.hp = 0.65
+    self.lp = 0.83
+    self.PlaceControllerPoles([self.hp, self.hp, self.lp, self.lp])
+
+    print self.K
+
+    self.hlp = 0.07
+    self.llp = 0.09
+    self.PlaceObserverPoles([self.hlp, self.hlp, self.llp, self.llp])
+
+    self.U_max = numpy.matrix([[12.0], [12.0]])
+    self.U_min = numpy.matrix([[-12.0], [-12.0]])
+    self.InitializeState()
+
+def main(argv):
+  # Simulate the response of the system to a step input.
+  drivetrain = Drivetrain()
+  simulated_left = []
+  simulated_right = []
+  for _ in xrange(100):
+    drivetrain.Update(numpy.matrix([[12.0], [12.0]]))
+    simulated_left.append(drivetrain.X[0, 0])
+    simulated_right.append(drivetrain.X[2, 0])
+
+  #pylab.plot(range(100), simulated_left)
+  #pylab.plot(range(100), simulated_right)
+  #pylab.show()
+
+  # Simulate forwards motion.
+  drivetrain = Drivetrain()
+  close_loop_left = []
+  close_loop_right = []
+  R = numpy.matrix([[1.0], [0.0], [1.0], [0.0]])
+  for _ in xrange(100):
+    U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
+                   drivetrain.U_min, drivetrain.U_max)
+    drivetrain.UpdateObserver(U)
+    drivetrain.Update(U)
+    close_loop_left.append(drivetrain.X[0, 0])
+    close_loop_right.append(drivetrain.X[2, 0])
+
+  #pylab.plot(range(100), close_loop_left)
+  #pylab.plot(range(100), close_loop_right)
+  #pylab.show()
+
+  # Try turning in place
+  drivetrain = Drivetrain()
+  close_loop_left = []
+  close_loop_right = []
+  R = numpy.matrix([[-1.0], [0.0], [1.0], [0.0]])
+  for _ in xrange(100):
+    U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
+                   drivetrain.U_min, drivetrain.U_max)
+    drivetrain.UpdateObserver(U)
+    drivetrain.Update(U)
+    close_loop_left.append(drivetrain.X[0, 0])
+    close_loop_right.append(drivetrain.X[2, 0])
+
+  #pylab.plot(range(100), close_loop_left)
+  #pylab.plot(range(100), close_loop_right)
+  #pylab.show()
+
+  # Try turning just one side.
+  drivetrain = Drivetrain()
+  close_loop_left = []
+  close_loop_right = []
+  R = numpy.matrix([[0.0], [0.0], [1.0], [0.0]])
+  for _ in xrange(100):
+    U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
+                   drivetrain.U_min, drivetrain.U_max)
+    drivetrain.UpdateObserver(U)
+    drivetrain.Update(U)
+    close_loop_left.append(drivetrain.X[0, 0])
+    close_loop_right.append(drivetrain.X[2, 0])
+
+  #pylab.plot(range(100), close_loop_left)
+  #pylab.plot(range(100), close_loop_right)
+  #pylab.show()
+
+  # Write the generated constants out to a file.
+  if len(argv) != 3:
+    print "Expected .h file name and .cc file name"
+  else:
+    loop_writer = control_loop.ControlLoopWriter("Drivetrain", [drivetrain])
+    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/bot3/control_loops/python/libcdd.py b/bot3/control_loops/python/libcdd.py
new file mode 100644
index 0000000..a217728
--- /dev/null
+++ b/bot3/control_loops/python/libcdd.py
@@ -0,0 +1,129 @@
+#!/usr/bin/python
+
+"""Wrapper around libcdd, a polytope manipulation library."""
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+import ctypes
+
+# Wrapper around PyFile_AsFile so that we can print out the error messages.
+# Set the arg type and return types of the function call.
+class FILE(ctypes.Structure):
+  pass
+
+ctypes.pythonapi.PyFile_AsFile.argtypes = [ctypes.py_object]
+ctypes.pythonapi.PyFile_AsFile.restype = ctypes.POINTER(FILE)
+
+# Load and init libcdd.  libcdd is a C library that implements algorithm to
+# manipulate half space and vertex representations of polytopes.
+# Unfortunately, the library was compiled with C++ even though it has a lot of C
+# code in it, so all the symbol names are mangled.  Ug.
+libcdd = ctypes.cdll.LoadLibrary('libcdd.so')
+libcdd._Z23dd_set_global_constantsv()
+
+# The variable type mytype that libcdd defines (double[1])
+# See http://docs.python.org/2/library/ctypes.html#arrays for the documentation
+# explaining why ctypes.c_double * 1 => double[1]
+# libcdd defines mytype to various things so it can essentially template its
+# functions.  What a weird library.
+mytype = ctypes.c_double * 1
+
+
+# Forward declaration for the polyhedra data structure.
+class dd_polyhedradata(ctypes.Structure):
+  pass
+
+
+# Definition of dd_matrixdata
+class dd_matrixdata(ctypes.Structure):
+  _fields_ = [
+      ("rowsize", ctypes.c_long),
+      ("linset", ctypes.POINTER(ctypes.c_ulong)),
+      ("colsize", ctypes.c_long),
+      ("representation", ctypes.c_int),
+      ("numbtype", ctypes.c_int),
+      ("matrix", ctypes.POINTER(ctypes.POINTER(mytype))),
+      ("objective", ctypes.c_int),
+      ("rowvec", ctypes.POINTER(mytype)),
+  ]
+
+# Define the input and output types for a bunch of libcdd functions.
+libcdd._Z15dd_CreateMatrixll.restype = ctypes.POINTER(dd_matrixdata)
+libcdd._Z9ddd_get_dPd.argtypes = [mytype]
+libcdd._Z9ddd_get_dPd.restype = ctypes.c_double
+
+libcdd._Z17dd_CopyGeneratorsP16dd_polyhedradata.argtypes = [
+    ctypes.POINTER(dd_polyhedradata)
+]
+libcdd._Z17dd_CopyGeneratorsP16dd_polyhedradata.restype = ctypes.POINTER(dd_matrixdata)
+
+libcdd._Z16dd_DDMatrix2PolyP13dd_matrixdataP12dd_ErrorType.argtypes = [
+    ctypes.POINTER(dd_matrixdata),
+    ctypes.POINTER(ctypes.c_int)
+]
+libcdd._Z16dd_DDMatrix2PolyP13dd_matrixdataP12dd_ErrorType.restype = (
+  ctypes.POINTER(dd_polyhedradata))
+
+libcdd._Z13dd_FreeMatrixP13dd_matrixdata.argtypes = [
+    ctypes.POINTER(dd_matrixdata)
+]
+
+libcdd._Z16dd_FreePolyhedraP16dd_polyhedradata.argtypes = [
+  ctypes.POINTER(dd_polyhedradata)
+]
+
+libcdd._Z9ddd_set_dPdd.argtypes = [
+  mytype,
+  ctypes.c_double
+]
+
+
+# Various enums.
+DD_INEQUALITY = 1
+DD_REAL = 1
+DD_NO_ERRORS = 17
+
+
+def dd_CreateMatrix(rows, cols):
+  return libcdd._Z15dd_CreateMatrixll(
+      ctypes.c_long(rows),
+      ctypes.c_long(cols))
+
+
+def dd_set_d(mytype_address, double_value):
+  libcdd._Z9ddd_set_dPdd(mytype_address,
+      ctypes.c_double(double_value))
+
+
+def dd_CopyGenerators(polyhedraptr):
+  return libcdd._Z17dd_CopyGeneratorsP16dd_polyhedradata(polyhedraptr)
+
+
+def dd_get_d(mytype_address):
+  return libcdd._Z9ddd_get_dPd(mytype_address)
+
+
+def dd_FreeMatrix(matrixptr):
+  libcdd._Z13dd_FreeMatrixP13dd_matrixdata(matrixptr)
+
+
+def dd_FreePolyhedra(polyhedraptr):
+  libcdd._Z16dd_FreePolyhedraP16dd_polyhedradata(polyhedraptr)
+
+
+def dd_DDMatrix2Poly(matrixptr):
+  error = ctypes.c_int()
+  polyhedraptr = libcdd._Z16dd_DDMatrix2PolyP13dd_matrixdataP12dd_ErrorType(
+      matrixptr,
+      ctypes.byref(error))
+
+  # Return None on error.
+  # The error values are enums, so they aren't exposed.
+  if error.value != NO_ERRORS:
+    # Dump out the errors to stderr
+    libcdd._Z21dd_WriteErrorMessagesP8_IO_FILE12dd_ErrorType(
+        ctypes.pythonapi.PyFile_AsFile(ctypes.py_object(sys.stdout)),
+        error)
+    dd_FreePolyhedra(polyhedraptr)
+    return None
+  return polyhedraptr
diff --git a/bot3/control_loops/python/shooter.py b/bot3/control_loops/python/shooter.py
new file mode 100755
index 0000000..7cbe617
--- /dev/null
+++ b/bot3/control_loops/python/shooter.py
@@ -0,0 +1,140 @@
+#!/usr/bin/python
+
+import numpy
+import sys
+from matplotlib import pylab
+import control_loop
+
+class Shooter(control_loop.ControlLoop):
+  def __init__(self):
+    super(Shooter, self).__init__("Shooter")
+    # Stall Torque in N m
+    self.stall_torque = 2.42211227883219
+    # Stall Current in Amps
+    self.stall_current = 133
+    # Free Speed in RPM
+    self.free_speed = 4650.0
+    # Free Current in Amps
+    self.free_current = 2.7
+    # Moment of inertia of the shooter wheel in kg m^2
+    self.J = 0.0032
+    # Resistance of the motor, divided by 2 to account for the 2 motors
+    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 = 40.0 / 34.0
+    # Control loop time step
+    self.dt = 0.01
+
+    # State feedback matrices
+    self.A_continuous = numpy.matrix(
+        [[0, 1],
+         [0, -self.Kt / self.Kv / (self.J * self.G * 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]])
+
+    self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
+                              self.dt, self.C)
+
+    self.PlaceControllerPoles([.6, .981])
+
+    self.rpl = .45
+    self.ipl = 0.07
+    self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+                             self.rpl - 1j * self.ipl])
+
+    self.U_max = numpy.matrix([[12.0]])
+    self.U_min = numpy.matrix([[-12.0]])
+
+
+def main(argv):
+  # Simulate the response of the system to a step input.
+  shooter_data = numpy.genfromtxt('shooter/shooter_data.csv', delimiter=',')
+  shooter = Shooter()
+  simulated_x = []
+  real_x = []
+  x_vel = []
+  initial_x = shooter_data[0, 2]
+  last_x = initial_x
+  for i in xrange(shooter_data.shape[0]):
+    shooter.Update(numpy.matrix([[shooter_data[i, 1]]]))
+    simulated_x.append(shooter.X[0, 0])
+    x_offset = shooter_data[i, 2] - initial_x
+    real_x.append(x_offset)
+    x_vel.append((shooter_data[i, 2] - last_x) * 100.0)
+    last_x = shooter_data[i, 2]
+
+  sim_delay = 1
+  pylab.plot(range(sim_delay, shooter_data.shape[0] + sim_delay),
+             simulated_x, label='Simulation')
+  pylab.plot(range(shooter_data.shape[0]), real_x, label='Reality')
+  pylab.plot(range(shooter_data.shape[0]), x_vel, label='Velocity')
+  pylab.legend()
+  pylab.show()
+
+  # Simulate the closed loop response of the system to a step input.
+  shooter = Shooter()
+  close_loop_x = []
+  close_loop_U = []
+  velocity_goal = 300
+  R = numpy.matrix([[0.0], [velocity_goal]])
+  for _ in pylab.linspace(0,1.99,200):
+    # Iterate the position up.
+    R = numpy.matrix([[R[0, 0] + 10.5], [velocity_goal]])
+    # Prevents the position goal from going beyond what is necessary.
+    velocity_weight_scalar = 0.35
+    max_reference = (
+        (shooter.U_max[0, 0] - velocity_weight_scalar *
+         (velocity_goal - shooter.X_hat[1, 0]) * shooter.K[0, 1]) /
+         shooter.K[0, 0] +
+         shooter.X_hat[0, 0])
+    min_reference = (
+        (shooter.U_min[0, 0] - velocity_weight_scalar *
+         (velocity_goal - shooter.X_hat[1, 0]) * shooter.K[0, 1]) /
+         shooter.K[0, 0] +
+         shooter.X_hat[0, 0])
+    R[0, 0] = numpy.clip(R[0, 0], min_reference, max_reference)
+    U = numpy.clip(shooter.K * (R - shooter.X_hat),
+                   shooter.U_min, shooter.U_max)
+    shooter.UpdateObserver(U)
+    shooter.Update(U)
+    close_loop_x.append(shooter.X[1, 0])
+    close_loop_U.append(U[0, 0])
+
+  #pylab.plotfile("shooter.csv", (0,1))
+  #pylab.plot(pylab.linspace(0,1.99,200), close_loop_U, 'ro')
+  #pylab.plotfile("shooter.csv", (0,2))
+  pylab.plot(pylab.linspace(0,1.99,200), close_loop_x, 'ro')
+  pylab.show()
+
+  # Simulate spin down.
+  spin_down_x = [];
+  R = numpy.matrix([[50.0], [0.0]])
+  for _ in xrange(150):
+    U = 0
+    shooter.UpdateObserver(U)
+    shooter.Update(U)
+    spin_down_x.append(shooter.X[1, 0])
+
+  #pylab.plot(range(150), spin_down_x)
+  #pylab.show()
+
+  if len(argv) != 3:
+    print "Expected .h file name and .cc file name"
+  else:
+    loop_writer = control_loop.ControlLoopWriter("Shooter", [shooter])
+    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/bot3/control_loops/shooter/shooter.cc b/bot3/control_loops/shooter/shooter.cc
new file mode 100644
index 0000000..fbfeafb
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter.cc
@@ -0,0 +1,118 @@
+#include "bot3/control_loops/shooter/shooter.h"
+
+#include "aos/common/control_loop/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+
+#include "bot3/control_loops/shooter/shooter_motor_plant.h"
+
+namespace bot3 {
+namespace control_loops {
+
+ShooterMotor::ShooterMotor(control_loops::ShooterLoop *my_shooter)
+    : aos::control_loops::ControlLoop<control_loops::ShooterLoop>(my_shooter),
+    loop_(new StateFeedbackLoop<2, 1, 1>(MakeShooterLoop())),
+    history_position_(0),
+    position_goal_(0.0),
+    last_position_(0.0),
+    last_velocity_goal_(0) {
+  memset(history_, 0, sizeof(history_));
+}
+
+/*static*/ const double ShooterMotor::dt = 0.01;
+
+void ShooterMotor::RunIteration(
+    const control_loops::ShooterLoop::Goal *goal,
+    const control_loops::ShooterLoop::Position *position,
+    ::aos::control_loops::Output *output,
+    control_loops::ShooterLoop::Status *status) {
+  double velocity_goal = goal->velocity;
+  const double current_position =
+      (position == NULL ? loop_->X_hat(0, 0) : position->position);
+  double output_voltage = 0.0;
+
+/*  if (index_loop.status.FetchLatest() || index_loop.status.get()) {
+    if (index_loop.status->is_shooting) {
+      if (velocity_goal != last_velocity_goal_ &&
+          velocity_goal < 130) {
+        velocity_goal = last_velocity_goal_;
+      }
+    }
+  } else {
+    LOG(WARNING, "assuming index isn't shooting\n");
+  }*/
+  last_velocity_goal_ = velocity_goal;
+
+  // Track the current position if the velocity goal is small.
+  if (velocity_goal <= 1.0) {
+    position_goal_ = current_position;
+  }
+
+  loop_->Y << current_position;
+
+  // Add the position to the history.
+  history_[history_position_] = current_position;
+  history_position_ = (history_position_ + 1) % kHistoryLength;
+
+  // Prevents integral windup by limiting the position error such that the
+  // error can't produce much more than full power.
+  const double kVelocityWeightScalar = 0.35;
+  const double max_reference =
+      (loop_->U_max(0, 0) - kVelocityWeightScalar *
+       (velocity_goal - loop_->X_hat(1, 0)) * loop_->K(0, 1))
+      / loop_->K(0, 0) + loop_->X_hat(0, 0);
+  const double min_reference =
+      (loop_->U_min(0, 0) - kVelocityWeightScalar *
+       (velocity_goal - loop_->X_hat(1, 0)) * loop_->K(0, 1))
+      / loop_->K(0, 0) + loop_->X_hat(0, 0);
+
+  position_goal_ = ::std::max(::std::min(position_goal_, max_reference),
+                              min_reference);
+  loop_->R << position_goal_, velocity_goal;
+  position_goal_ += velocity_goal * dt;
+
+  loop_->Update(position, output == NULL);
+
+  // Kill power at low velocity goals.
+  if (velocity_goal < 1.0) {
+    loop_->U[0] = 0.0;
+  } else {
+    output_voltage = loop_->U[0];
+  }
+
+  LOG(DEBUG,
+      "PWM: %f, raw_pos: %f rotations: %f "
+      "junk velocity: %f, xhat[0]: %f xhat[1]: %f, R[0]: %f R[1]: %f\n",
+      output_voltage, current_position,
+      current_position / (2 * M_PI),
+      (current_position - last_position_) / dt,
+      loop_->X_hat[0], loop_->X_hat[1], loop_->R[0], loop_->R[1]);
+
+  // Calculates the velocity over the last kHistoryLength * .01 seconds
+  // by taking the difference between the current and next history positions.
+  int old_history_position = ((history_position_ == 0) ?
+        kHistoryLength : history_position_) - 1;
+  average_velocity_ = (history_[old_history_position] -
+      history_[history_position_]) * 100.0 / (double)(kHistoryLength - 1);
+
+  status->average_velocity = average_velocity_;
+
+  // Determine if the velocity is close enough to the goal to be ready.
+  if (std::abs(velocity_goal - average_velocity_) < 10.0 &&
+      velocity_goal != 0.0) {
+    LOG(DEBUG, "Steady: ");
+    status->ready = true;
+  } else {
+    LOG(DEBUG, "Not ready: ");
+    status->ready = false;
+  }
+  LOG(DEBUG, "avg = %f goal = %f\n", average_velocity_, velocity_goal);
+  
+  last_position_ = current_position;
+
+  if (output) {
+    output->voltage = output_voltage;
+  }
+}
+
+}  // namespace control_loops
+}  // namespace bot3
diff --git a/bot3/control_loops/shooter/shooter.gyp b/bot3/control_loops/shooter/shooter.gyp
new file mode 100644
index 0000000..ed42647
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter.gyp
@@ -0,0 +1,79 @@
+{
+  'targets': [
+    {
+      'target_name': 'shooter_loop',
+      'type': 'static_library',
+      'sources': ['shooter_motor.q'],
+      'variables': {
+        'header_path': 'bot3/control_loops/shooter',
+      },
+      'dependencies': [
+        '<(AOS)/common/common.gyp:control_loop_queues',
+        '<(AOS)/common/common.gyp:queues',
+      ],
+      'export_dependent_settings': [
+        '<(AOS)/common/common.gyp:control_loop_queues',
+        '<(AOS)/common/common.gyp:queues',
+      ],
+      'includes': ['../../../aos/build/queues.gypi'],
+    },
+    {
+      'target_name': 'shooter_lib',
+      'type': 'static_library',
+      'sources': [
+        'shooter.cc',
+        'shooter_motor_plant.cc',
+      ],
+      'dependencies': [
+        'shooter_loop',
+        '<(AOS)/common/common.gyp:controls',
+        '<(DEPTH)/frc971/frc971.gyp:common',
+        '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+       #'<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
+      ],
+      'export_dependent_settings': [
+        '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+        '<(AOS)/common/common.gyp:controls',
+        'shooter_loop',
+      ],
+    },
+    {
+      'target_name': 'shooter_lib_test',
+      'type': 'executable',
+      'sources': [
+        'shooter_lib_test.cc',
+      ],
+      'dependencies': [
+        '<(EXTERNALS):gtest',
+        'shooter_loop',
+        'shooter_lib',
+        '<(AOS)/common/common.gyp:queue_testutils',
+        '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+      ],
+    },
+    {
+      'target_name': 'shooter_csv',
+      'type': 'executable',
+      'sources': [
+        'shooter_csv.cc',
+      ],
+      'dependencies': [
+        '<(AOS)/common/common.gyp:time',
+        '<(AOS)/common/common.gyp:timing',
+        'shooter_loop',
+      ],
+    },
+    {
+      'target_name': 'shooter',
+      'type': 'executable',
+      'sources': [
+        'shooter_main.cc',
+      ],
+      'dependencies': [
+        '<(AOS)/atom_code/atom_code.gyp:init',
+        'shooter_lib',
+        'shooter_loop',
+      ],
+    },
+  ],
+}
diff --git a/bot3/control_loops/shooter/shooter.h b/bot3/control_loops/shooter/shooter.h
new file mode 100644
index 0000000..6ab1ea5
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter.h
@@ -0,0 +1,57 @@
+#ifndef FRC971_CONTROL_LOOPS_SHOOTER_H_
+#define FRC971_CONTROL_LOOPS_SHOOTER_H_
+
+#include <memory>
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "bot3/control_loops/shooter/shooter_motor.q.h"
+#include "bot3/control_loops/shooter/shooter_motor_plant.h"
+
+namespace bot3 {
+namespace control_loops {
+
+class ShooterMotor
+    : public aos::control_loops::ControlLoop<control_loops::ShooterLoop> {
+ public:
+  explicit ShooterMotor(
+      control_loops::ShooterLoop *my_shooter = &control_loops::shooter);
+
+  // Control loop time step.
+  static const double dt;
+
+  // Maximum speed of the shooter wheel which the encoder is rated for in
+  // rad/sec.
+  static const double kMaxSpeed;
+
+ protected:
+  virtual void RunIteration(
+      const control_loops::ShooterLoop::Goal *goal,
+      const control_loops::ShooterLoop::Position *position,
+      ::aos::control_loops::Output *output,
+      control_loops::ShooterLoop::Status *status);
+
+ private:
+  // The state feedback control loop to talk to.
+  ::std::unique_ptr<StateFeedbackLoop<2, 1, 1>> loop_;
+
+  // History array and stuff for determining average velocity and whether
+  // we are ready to shoot.
+  static const int kHistoryLength = 5;
+  double history_[kHistoryLength];
+  ptrdiff_t history_position_;
+  double average_velocity_;
+
+  double position_goal_;
+  double last_position_;
+
+  // For making sure it keeps spinning if we're shooting.
+  double last_velocity_goal_;
+
+  DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
+};
+
+}  // namespace control_loops
+}  // namespace bot3
+
+#endif // FRC971_CONTROL_LOOPS_SHOOTER_H_
diff --git a/bot3/control_loops/shooter/shooter_csv.cc b/bot3/control_loops/shooter/shooter_csv.cc
new file mode 100644
index 0000000..c929f7e
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter_csv.cc
@@ -0,0 +1,50 @@
+#include "stdio.h"
+
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/time.h"
+#include "bot3/control_loops/shooter/shooter_motor.q.h"
+
+using ::bot3::control_loops::shooter;
+using ::aos::time::Time;
+
+int main(int argc, char * argv[]) {
+  FILE *data_file = NULL;
+  FILE *output_file = NULL;
+
+  if (argc == 2) {
+    data_file = fopen(argv[1], "w");
+    output_file = data_file;
+  } else {
+    printf("Logging to stdout instead\n");
+    output_file = stdout;
+  }
+
+  fprintf(data_file, "time, power, position");
+
+  ::aos::Init();
+
+  Time start_time = Time::Now();
+
+  while (true) {
+    ::aos::time::PhasedLoop10MS(2000);
+    shooter.goal.FetchLatest();
+    shooter.status.FetchLatest();
+    shooter.position.FetchLatest();
+    shooter.output.FetchLatest();
+    if (shooter.output.get() &&
+        shooter.position.get()) {
+      fprintf(output_file, "\n%f, %f, %f",
+              (shooter.position->sent_time - start_time).ToSeconds(),
+              shooter.output->voltage,
+              shooter.position->position);
+    }
+  }
+
+  if (data_file) {
+    fclose(data_file);
+  }
+
+  ::aos::Cleanup();
+  return 0;
+}
+
diff --git a/bot3/control_loops/shooter/shooter_data.csv b/bot3/control_loops/shooter/shooter_data.csv
new file mode 100644
index 0000000..3515070
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter_data.csv
@@ -0,0 +1,638 @@
+0.009404, 0.000000, 1484.878965
+0.019423, 0.000000, 1484.878965
+0.029389, 0.000000, 1484.878965
+0.039354, 0.000000, 1484.878965
+0.049887, 0.000000, 1484.878965
+0.059522, 0.000000, 1484.878965
+0.069479, 0.000000, 1484.878965
+0.079381, 0.000000, 1484.878965
+0.089338, 0.000000, 1484.878965
+0.099357, 0.000000, 1484.878965
+0.109409, 0.000000, 1484.878965
+0.119355, 0.000000, 1484.878965
+0.129358, 0.000000, 1484.878965
+0.139354, 0.000000, 1484.878965
+0.149480, 0.000000, 1484.878965
+0.159363, 0.000000, 1484.878965
+0.169341, 0.000000, 1484.878965
+0.179367, 0.000000, 1484.878965
+0.189636, 0.000000, 1484.878965
+0.199875, 0.000000, 1484.878965
+0.210070, 0.000000, 1484.878965
+0.219349, 0.000000, 1484.878965
+0.229544, 0.000000, 1484.878965
+0.239404, 0.000000, 1484.878965
+0.249410, 0.000000, 1484.878965
+0.259839, 0.000000, 1484.878965
+0.269492, 0.000000, 1484.878965
+0.279847, 0.000000, 1484.878965
+0.290056, 0.000000, 1484.878965
+0.299362, 0.000000, 1484.878965
+0.309457, 0.000000, 1484.878965
+0.319829, 0.000000, 1484.878965
+0.329446, 0.000000, 1484.878965
+0.339818, 0.000000, 1484.878965
+0.349444, 0.000000, 1484.878965
+0.359899, 0.000000, 1484.878965
+0.370053, 0.000000, 1484.878965
+0.379510, 0.000000, 1484.878965
+0.390136, 0.000000, 1484.878965
+0.399366, 0.000000, 1484.878965
+0.409472, 0.000000, 1484.878965
+0.419898, 0.000000, 1484.878965
+0.430131, 0.000000, 1484.878965
+0.439363, 0.000000, 1484.878965
+0.449459, 0.000000, 1484.878965
+0.459840, 0.000000, 1484.878965
+0.469382, 0.000000, 1484.878965
+0.479846, 0.000000, 1484.878965
+0.489432, 0.000000, 1484.878965
+0.499342, 0.000000, 1484.878965
+0.509350, 0.000000, 1484.878965
+0.519406, 0.000000, 1484.878965
+0.530084, 0.000000, 1484.878965
+0.539341, 0.000000, 1484.878965
+0.549406, 0.000000, 1484.878965
+0.559401, 0.000000, 1484.878965
+0.569409, 0.000000, 1484.878965
+0.579831, 0.000000, 1484.878965
+0.589469, 0.000000, 1484.878965
+0.599356, 0.000000, 1484.878965
+0.610099, 0.000000, 1484.878965
+0.619333, 0.000000, 1484.878965
+0.629479, 0.000000, 1484.878965
+0.639805, 0.000000, 1484.878965
+0.650053, 0.000000, 1484.878965
+0.659423, 0.000000, 1484.878965
+0.669413, 0.000000, 1484.878965
+0.679822, 0.000000, 1484.878965
+0.690045, 0.000000, 1484.878965
+0.699411, 0.000000, 1484.878965
+0.709584, 0.000000, 1484.878965
+0.719866, 0.000000, 1484.878965
+0.729475, 0.000000, 1484.878965
+0.739328, 0.000000, 1484.878965
+0.749396, 0.000000, 1484.878965
+0.759836, 0.000000, 1484.878965
+0.769492, 0.000000, 1484.878965
+0.779340, 0.000000, 1484.878965
+0.789401, 0.000000, 1484.878965
+0.799405, 0.000000, 1484.878965
+0.809407, 0.000000, 1484.878965
+0.819830, 0.000000, 1484.878965
+0.829411, 0.000000, 1484.878965
+0.839413, 0.000000, 1484.878965
+0.849409, 0.000000, 1484.878965
+0.859349, 0.000000, 1484.878965
+0.869453, 0.000000, 1484.878965
+0.879831, 0.000000, 1484.878965
+0.889426, 0.000000, 1484.878965
+0.899332, 0.000000, 1484.878965
+0.909439, 0.000000, 1484.878965
+0.919830, 0.000000, 1484.878965
+0.929464, 0.000000, 1484.878965
+0.939328, 0.000000, 1484.878965
+0.949440, 0.000000, 1484.878965
+0.959871, 0.000000, 1484.878965
+0.969393, 0.000000, 1484.878965
+0.980080, 0.000000, 1484.878965
+0.989440, 0.000000, 1484.878965
+0.999370, 0.000000, 1484.878965
+1.009338, 0.000000, 1484.878965
+1.019368, 0.000000, 1484.878965
+1.029492, 0.000000, 1484.878965
+1.039816, 0.000000, 1484.878965
+1.049430, 0.000000, 1484.878965
+1.059324, 0.000000, 1484.878965
+1.069351, 0.000000, 1484.878965
+1.079867, 0.000000, 1484.878965
+1.089417, 0.000000, 1484.878965
+1.099324, 0.000000, 1484.878965
+1.109348, 0.000000, 1484.878965
+1.119389, 0.000000, 1484.878965
+1.129331, 0.000000, 1484.878965
+1.139306, 0.000000, 1484.878965
+1.149394, 0.000000, 1484.878965
+1.159374, 0.000000, 1484.878965
+1.169335, 0.000000, 1484.878965
+1.179817, 0.000000, 1484.878965
+1.189415, 0.000000, 1484.878965
+1.199338, 0.000000, 1484.878965
+1.209349, 0.000000, 1484.878965
+1.219333, 0.000000, 1484.878965
+1.229518, 0.000000, 1484.878965
+1.239329, 0.000000, 1484.878965
+1.249334, 0.000000, 1484.878965
+1.259316, 0.000000, 1484.878965
+1.269388, 0.000000, 1484.878965
+1.279357, 0.000000, 1484.878965
+1.289451, 0.000000, 1484.878965
+1.299350, 0.000000, 1484.878965
+1.309350, 0.000000, 1484.878965
+1.319848, 0.000000, 1484.878965
+1.329384, 0.000000, 1484.878965
+1.339375, 0.000000, 1484.878965
+1.349359, 0.000000, 1484.878965
+1.359384, 0.000000, 1484.878965
+1.369428, 0.000000, 1484.878965
+1.379443, 0.000000, 1484.878965
+1.389498, 0.000000, 1484.878965
+1.399332, 0.000000, 1484.878965
+1.409393, 0.000000, 1484.878965
+1.419325, 0.000000, 1484.878965
+1.430129, 0.000000, 1484.878965
+1.439419, 0.000000, 1484.878965
+1.449510, 0.000000, 1484.878965
+1.459828, 0.000000, 1484.878965
+1.469377, 0.000000, 1484.878965
+1.479834, 0.000000, 1484.878965
+1.489367, 0.000000, 1484.878965
+1.499316, 0.000000, 1484.878965
+1.509405, 0.000000, 1484.878965
+1.519341, 0.000000, 1484.878965
+1.529334, 0.000000, 1484.878965
+1.539305, 0.000000, 1484.878965
+1.550118, 0.000000, 1484.878965
+1.559386, 0.000000, 1484.878965
+1.569647, 0.000000, 1484.878965
+1.579395, 0.000000, 1484.878965
+1.589381, 0.000000, 1484.878965
+1.599819, 0.000000, 1484.878965
+1.609401, 0.000000, 1484.878965
+1.619404, 0.000000, 1484.878965
+1.629335, 0.000000, 1484.878965
+1.639327, 0.000000, 1484.878965
+1.649334, 0.000000, 1484.878965
+1.659341, 0.000000, 1484.878965
+1.669328, 0.000000, 1484.878965
+1.679850, 0.000000, 1484.878965
+1.689423, 0.000000, 1484.878965
+1.699320, 0.000000, 1484.878965
+1.710128, 0.000000, 1484.878965
+1.719388, 0.000000, 1484.878965
+1.730042, 0.000000, 1484.878965
+1.739338, 0.000000, 1484.878965
+1.749483, 0.000000, 1484.878965
+1.759420, 0.000000, 1484.878965
+1.769334, 0.000000, 1484.878965
+1.779289, 0.000000, 1484.878965
+1.789325, 0.000000, 1484.878965
+1.799395, 0.000000, 1484.878965
+1.809493, 0.000000, 1484.878965
+1.819312, 0.000000, 1484.878965
+1.829402, 0.000000, 1484.878965
+1.839317, 0.000000, 1484.878965
+1.849330, 0.000000, 1484.878965
+1.859354, 0.000000, 1484.878965
+1.869394, 0.000000, 1484.878965
+1.879816, 0.000000, 1484.878965
+1.889374, 0.000000, 1484.878965
+1.899381, 0.000000, 1484.878965
+1.909332, 0.000000, 1484.878965
+1.919359, 0.000000, 1484.878965
+1.929338, 0.000000, 1484.878965
+1.939359, 0.000000, 1484.878965
+1.949332, 0.000000, 1484.878965
+1.959325, 0.000000, 1484.878965
+1.969341, 0.000000, 1484.878965
+1.979362, 0.000000, 1484.878965
+1.989330, 0.000000, 1484.878965
+1.999479, 0.000000, 1484.878965
+2.009392, 0.000000, 1484.878965
+2.019318, 0.000000, 1484.878965
+2.029320, 0.000000, 1484.878965
+2.039323, 0.000000, 1484.878965
+2.049387, 0.000000, 1484.878965
+2.059818, 0.000000, 1484.878965
+2.069766, 0.000000, 1484.878965
+2.079835, 0.000000, 1484.878965
+2.089372, 0.000000, 1484.878965
+2.099322, 0.000000, 1484.878965
+2.109357, 0.000000, 1484.878965
+2.119387, 0.000000, 1484.878965
+2.129327, 0.000000, 1484.878965
+2.139458, 0.000000, 1484.878965
+2.149392, 0.000000, 1484.878965
+2.159826, 0.000000, 1484.878965
+2.169591, 0.000000, 1484.878965
+2.179656, 0.000000, 1484.878965
+2.189392, 0.000000, 1484.878965
+2.199491, 0.000000, 1484.878965
+2.209541, 0.000000, 1484.878965
+2.219287, 0.000000, 1484.878965
+2.229123, 0.000000, 1484.878965
+2.239347, 0.000000, 1484.878965
+2.249390, 0.000000, 1484.878965
+2.259407, 0.000000, 1484.878965
+2.269393, 0.000000, 1484.878965
+2.279375, 0.000000, 1484.878965
+2.289416, 0.000000, 1484.878965
+2.299368, 0.000000, 1484.878965
+2.309379, 0.000000, 1484.878965
+2.319382, 0.000000, 1484.878965
+2.329435, 0.000000, 1484.878965
+2.339329, 0.000000, 1484.878965
+2.349389, 0.000000, 1484.878965
+2.359454, 0.000000, 1484.878965
+2.369832, 0.000000, 1484.878965
+2.379390, 0.000000, 1484.878965
+2.389381, 0.000000, 1484.878965
+2.399429, 0.000000, 1484.878965
+2.409394, 0.000000, 1484.878965
+2.419367, 0.000000, 1484.878965
+2.429384, 0.000000, 1484.878965
+2.439408, 0.000000, 1484.878965
+2.449391, 0.000000, 1484.878965
+2.459343, 0.000000, 1484.878965
+2.469424, 0.000000, 1484.878965
+2.479357, 0.000000, 1484.878965
+2.489388, 0.000000, 1484.878965
+2.499413, 0.000000, 1484.878965
+2.510081, 0.000000, 1484.878965
+2.519397, 0.000000, 1484.878965
+2.529342, 0.000000, 1484.878965
+2.539372, 0.000000, 1484.878965
+2.549674, 0.000000, 1484.878965
+2.559586, 0.000000, 1484.878965
+2.569807, 0.000000, 1484.878965
+2.579362, 0.000000, 1484.878965
+2.589325, 0.000000, 1484.878965
+2.599300, 0.000000, 1484.878965
+2.609436, 0.000000, 1484.878965
+2.619476, 0.000000, 1484.878965
+2.629668, 0.000000, 1484.878965
+2.639301, 0.000000, 1484.878965
+2.649411, 0.000000, 1484.878965
+2.659301, 0.000000, 1484.878965
+2.669336, 0.000000, 1484.878965
+2.679460, 0.000000, 1484.878965
+2.689691, 0.000000, 1484.878965
+2.699310, 0.000000, 1484.878965
+2.710046, 0.000000, 1484.878965
+2.719584, 0.000000, 1484.878965
+2.729333, 0.000000, 1484.878965
+2.739288, 0.000000, 1484.878965
+2.749320, 0.000000, 1484.878965
+2.759517, 0.000000, 1484.878965
+2.769811, 0.000000, 1484.878965
+2.779463, 0.000000, 1484.878965
+2.789708, 0.000000, 1484.878965
+2.799310, 12.000000, 1484.878965
+2.809361, 12.000000, 1484.878965
+2.819345, 12.000000, 1484.943934
+2.829470, 12.000000, 1485.117183
+2.839666, 12.000000, 1485.355402
+2.849432, 12.000000, 1485.680245
+2.859547, 12.000000, 1486.091712
+2.869422, 12.000000, 1486.589805
+2.879352, 12.000000, 1487.152866
+2.889431, 12.000000, 1487.802552
+2.899538, 12.000000, 1488.538863
+2.909416, 12.000000, 1489.340142
+2.919676, 12.000000, 1490.163078
+2.929470, 11.856977, 1491.072638
+2.939341, 10.941776, 1492.090480
+2.949422, 9.709468, 1493.086665
+2.959343, 9.484298, 1494.212787
+2.969480, 9.024482, 1495.360566
+2.979482, 8.408468, 1496.616625
+2.989402, 7.584528, 1497.851029
+2.999839, 7.318006, 1499.193713
+3.009487, 6.726255, 1500.601366
+3.019345, 5.691274, 1502.030675
+3.029433, 5.445505, 1503.503297
+3.039661, 5.201068, 1505.019231
+3.049419, 4.805405, 1506.556821
+3.059323, 4.164102, 1508.116067
+3.069465, 3.901448, 1509.696970
+3.079658, 3.715078, 1511.321185
+3.089408, 3.656437, 1512.902087
+3.099346, 3.325052, 1514.504646
+3.109498, 3.310343, 1516.128861
+3.119381, 3.348580, 1517.753076
+3.129434, 3.031678, 1519.377291
+3.139461, 3.165136, 1520.979850
+3.149456, 3.276186, 1522.604064
+3.159650, 3.130954, 1524.228279
+3.169436, 3.017931, 1525.852494
+3.179542, 2.968543, 1527.455053
+3.189425, 3.107515, 1529.079268
+3.199654, 3.010200, 1530.681827
+3.209442, 3.078322, 1532.306042
+3.219518, 2.953745, 1533.908601
+3.229434, 3.021034, 1535.511159
+3.239303, 4.196084, 1537.113718
+3.249474, 2.523334, 1538.716277
+3.259481, 2.549791, 1540.318836
+3.269395, 2.856174, 1541.943051
+3.279668, 2.830169, 1543.545609
+3.289491, 2.903769, 1545.148168
+3.299343, 2.930722, 1546.729071
+3.309430, 2.915555, 1548.331629
+3.319847, 2.887528, 1549.934188
+3.329444, 3.022588, 1551.515091
+3.339468, 2.922583, 1553.095993
+3.349700, 3.155171, 1554.676896
+3.359307, 2.959473, 1556.257798
+3.369439, 2.963462, 1557.817045
+3.379865, 3.151337, 1559.397947
+3.389482, 3.237455, 1560.957194
+3.399670, 3.075969, 1562.538096
+3.409431, 3.127616, 1564.097342
+3.419355, 3.012946, 1565.656589
+3.429472, 3.094794, 1567.237491
+3.439645, 2.986884, 1568.796738
+3.449433, 3.228489, 1570.355984
+3.459540, 3.042079, 1571.915230
+3.469425, 3.052375, 1573.474477
+3.479338, 3.083112, 1575.055379
+3.489491, 2.926137, 1576.614626
+3.499576, 2.990285, 1578.152216
+3.509412, 3.204911, 1579.711462
+3.519368, 3.134930, 1581.270709
+3.529436, 3.050871, 1582.829955
+3.539512, 3.012085, 1584.389201
+3.549482, 3.160836, 1585.948448
+3.559848, 3.076263, 1587.486038
+3.569480, 2.996910, 1589.045284
+3.579466, 2.963210, 1590.604530
+3.589392, 3.113684, 1592.142121
+3.599645, 3.029053, 1593.679711
+3.609466, 3.111230, 1595.238957
+3.619478, 3.001191, 1596.776547
+3.629414, 3.083010, 1598.335794
+3.639639, 2.977469, 1599.873384
+3.649431, 3.061646, 1601.410974
+3.659343, 2.956478, 1602.970220
+3.669437, 3.040410, 1604.507810
+3.679660, 3.096927, 1606.067057
+3.689414, 3.104633, 1607.582991
+3.699466, 3.092523, 1609.120581
+3.709581, 3.079314, 1610.658171
+3.719844, 3.069195, 1612.195761
+3.729455, 3.060854, 1613.755008
+3.739454, 2.890971, 1615.270942
+3.749456, 3.120861, 1616.808532
+3.759652, 2.943141, 1618.367778
+3.769565, 2.963262, 1619.905368
+3.779406, 3.162518, 1621.442958
+3.789419, 3.096059, 1622.958892
+3.799647, 2.857986, 1624.496482
+3.809451, 3.063847, 1626.034073
+3.819535, 3.048330, 1627.550007
+3.829423, 3.158748, 1629.087597
+3.839290, 3.053598, 1630.625187
+3.849534, 2.974064, 1632.162777
+3.859585, 2.947301, 1633.678711
+3.869342, 3.104581, 1635.194645
+3.879651, 3.025376, 1636.753891
+3.889461, 3.112302, 1638.269825
+3.899471, 3.006920, 1639.785759
+3.909503, 3.093474, 1641.323349
+3.919650, 2.992726, 1642.860940
+3.929493, 3.081710, 1644.376873
+3.939457, 2.981346, 1645.914464
+3.949675, 2.908199, 1647.430398
+3.959566, 3.045687, 1648.946332
+3.969420, 3.126825, 1650.505578
+3.979850, 2.816686, 1652.021512
+3.989429, 3.120514, 1653.537446
+3.999650, 3.009544, 1655.053380
+4.009434, 3.053283, 1656.590970
+4.019490, 2.769875, 1658.106904
+4.029473, 2.932334, 1659.644494
+4.039639, 2.903060, 1661.182084
+4.049524, 3.176347, 1662.698018
+4.059463, 2.998063, 1664.213952
+4.069518, 3.013137, 1665.729886
+4.079855, 3.051287, 1667.267476
+4.089451, 3.065561, 1668.783410
+4.099514, 2.901543, 1670.299344
+4.109450, 2.971613, 1671.793622
+4.119835, 3.035151, 1673.331212
+4.129459, 3.052225, 1674.847146
+4.139393, 2.884954, 1676.363080
+4.149517, 3.114730, 1677.879014
+4.159643, 3.101587, 1679.373292
+4.169483, 3.212047, 1680.889226
+4.179478, 2.947840, 1682.405160
+4.189417, 3.111024, 1683.921094
+4.199352, 3.081820, 1685.415371
+4.209534, 3.196368, 1686.931305
+4.219472, 2.937203, 1688.447239
+4.229358, 3.102203, 1689.963173
+4.239581, 4.044265, 1691.479107
+4.249471, 2.568287, 1692.995041
+4.259564, 2.592233, 1694.510975
+4.269489, 2.874029, 1696.048565
+4.279649, 2.848913, 1697.564499
+4.289504, 3.102328, 1699.058777
+4.299467, 2.913154, 1700.574711
+4.309396, 2.926342, 1702.090645
+4.319635, 3.127416, 1703.584923
+4.329463, 3.066398, 1705.122513
+4.339462, 3.157757, 1706.616790
+4.349523, 3.054520, 1708.132724
+4.359685, 2.982710, 1709.627002
+4.369520, 3.124646, 1711.142936
+4.379481, 2.887285, 1712.658870
+4.389476, 3.058346, 1714.174804
+4.399662, 3.028207, 1715.647426
+4.409523, 3.140828, 1717.163360
+4.419532, 3.042732, 1718.679293
+4.429478, 3.131501, 1720.195227
+4.439668, 3.033777, 1721.667849
+4.449451, 3.127131, 1723.183783
+4.459519, 3.031674, 1724.699717
+4.469529, 3.125338, 1726.193995
+4.479652, 3.029621, 1727.709929
+4.489541, 3.123096, 1729.204206
+4.499285, 2.865452, 1730.720140
+4.509567, 3.035110, 1732.236074
+4.519358, 3.169872, 1733.752008
+4.529256, 3.046239, 1735.246286
+4.539423, 2.956041, 1736.762220
+4.549418, 3.092368, 1738.278154
+4.559287, 3.017417, 1739.772432
+4.569392, 3.113609, 1741.266709
+4.579389, 3.015401, 1742.804300
+4.589391, 3.107141, 1744.298577
+4.599271, 3.010950, 1745.792855
+4.609237, 3.104538, 1747.308789
+4.619397, 2.847040, 1748.803067
+4.629258, 3.016751, 1750.340657
+4.639391, 3.151513, 1751.834935
+4.649382, 3.027871, 1753.329212
+4.659249, 2.937667, 1754.845146
+4.669717, 3.073994, 1756.361080
+4.679709, 2.837164, 1757.877014
+4.689305, 3.009530, 1759.371292
+4.699414, 2.979957, 1760.865570
+4.709265, 3.254504, 1762.381504
+4.719267, 2.918277, 1763.875782
+4.729387, 3.014535, 1765.391715
+4.739263, 2.964507, 1766.885993
+4.749394, 3.242657, 1768.401927
+4.759241, 3.073913, 1769.917861
+4.769396, 2.934311, 1771.412139
+4.779436, 2.891905, 1772.928073
+4.789376, 3.055522, 1774.444007
+4.799259, 2.823787, 1775.938285
+4.809430, 2.993720, 1777.454219
+4.819715, 2.961969, 1778.948496
+4.829635, 3.235854, 1780.442774
+4.839637, 3.061570, 1781.958708
+4.849506, 3.081810, 1783.474642
+4.859505, 2.801335, 1784.968920
+4.869352, 3.134735, 1786.484854
+4.879650, 3.036602, 1787.979131
+4.889446, 3.084670, 1789.495065
+4.899459, 2.966217, 1790.989343
+4.909516, 3.056512, 1792.483621
+4.919642, 2.963315, 1793.999555
+4.929515, 3.058695, 1795.493833
+4.939540, 2.963436, 1796.988110
+4.949486, 3.056827, 1798.504044
+4.959635, 2.960935, 1800.019978
+4.969264, 3.054371, 1801.514256
+4.979559, 3.120496, 1803.030190
+4.989369, 2.975943, 1804.502812
+4.999639, 3.049612, 1806.018745
+5.009522, 3.114635, 1807.513023
+5.019539, 2.811284, 1809.028957
+5.029502, 3.124778, 1810.523235
+5.039639, 3.020720, 1812.017513
+5.049531, 3.069436, 1813.511790
+5.059571, 2.952407, 1815.006068
+5.069509, 3.205185, 1816.522002
+5.079646, 3.035861, 1818.016280
+5.089525, 3.224397, 1819.532214
+5.099572, 2.870103, 1821.026492
+5.109284, 3.134886, 1822.542426
+5.119546, 3.015399, 1824.036703
+5.129816, 3.065232, 1825.530981
+5.139566, 2.951614, 1827.025259
+5.149507, 3.044046, 1828.541193
+5.159648, 2.951048, 1830.035471
+5.169497, 3.208013, 1831.551405
+5.179545, 2.874516, 1833.045682
+5.189501, 3.137284, 1834.539960
+5.199633, 3.012938, 1836.034238
+5.209521, 3.222545, 1837.528516
+5.219560, 3.032578, 1839.044450
+5.229503, 3.056672, 1840.538727
+5.239594, 4.237957, 1842.033005
+5.249532, 2.592533, 1843.527283
+5.259542, 2.486074, 1845.043217
+5.269566, 2.894539, 1846.515838
+5.279650, 3.123100, 1848.010116
+5.289252, 3.165125, 1849.504394
+5.299546, 3.307151, 1850.998672
+5.309525, 3.213976, 1852.471293
+5.319632, 3.141194, 1853.943915
+5.329458, 3.284019, 1855.459849
+5.339575, 3.050528, 1856.932470
+5.349266, 3.226635, 1858.448404
+5.359682, 3.201591, 1859.921026
+5.369278, 3.319115, 1861.436960
+5.379549, 3.063938, 1862.931237
+5.389254, 3.071781, 1864.447171
+5.399651, 2.961775, 1865.919793
+5.409641, 3.063785, 1867.435727
+5.419564, 0.000000, 1868.930005
+5.429518, 0.000000, 1870.424282
+5.439596, 0.000000, 1871.918560
+5.449485, 0.000000, 1873.347869
+5.459469, 0.000000, 1874.733866
+5.469640, 0.000000, 1876.098207
+5.479575, 0.000000, 1877.419235
+5.489492, 0.000000, 1878.675294
+5.499454, 0.000000, 1879.909698
+5.509441, 0.000000, 1881.100789
+5.519669, 0.000000, 1882.270223
+5.529268, 0.000000, 1883.396346
+5.539531, 0.000000, 1884.479156
+5.549491, 0.000000, 1885.518653
+5.559591, 0.000000, 1886.536495
+5.569479, 0.000000, 1887.532680
+5.579553, 0.000000, 1888.485553
+5.589263, 0.000000, 1889.416769
+5.599628, 0.000000, 1890.283017
+5.609531, 0.000000, 1891.149265
+5.619452, 0.000000, 1891.993857
+5.629254, 0.000000, 1892.795136
+5.639595, 0.000000, 1893.574759
+5.649382, 0.000000, 1894.332726
+5.659518, 0.000000, 1895.047381
+5.669510, 0.000000, 1895.740379
+5.679630, 0.000000, 1896.433378
+5.689513, 0.000000, 1897.083064
+5.699559, 0.000000, 1897.689437
+5.709897, 0.000000, 1898.295811
+5.719663, 0.000000, 1898.880528
+5.729785, 0.000000, 1899.443590
+5.739550, 0.000000, 1899.984995
+5.749477, 0.000000, 1900.504743
+5.759631, 0.000000, 1901.002836
+5.769499, 0.000000, 1901.500928
+5.779545, 0.000000, 1901.955709
+5.789477, 0.000000, 1902.388833
+5.799588, 0.000000, 1902.821957
+5.809530, 0.000000, 1903.233424
+5.819550, 0.000000, 1903.623236
+5.829288, 0.000000, 1903.991391
+5.839583, 0.000000, 1904.359547
+5.849261, 0.000000, 1904.706046
+5.859488, 0.000000, 1905.030889
+5.869375, 0.000000, 1905.334076
+5.879665, 0.000000, 1905.658919
+5.889507, 0.000000, 1905.918793
+5.899530, 0.000000, 1906.200324
+5.909520, 0.000000, 1906.460198
+5.919633, 0.000000, 1906.720073
+5.929467, 0.000000, 1906.958291
+5.939535, 0.000000, 1907.174853
+5.949423, 0.000000, 1907.391415
+5.959583, 0.000000, 1907.586320
+5.969500, 0.000000, 1907.781226
+5.979548, 0.000000, 1907.954476
+5.989236, 0.000000, 1908.106069
+5.999631, 0.000000, 1908.279319
+6.009506, 0.000000, 1908.409256
+6.019548, 0.000000, 1908.560849
+6.029525, 0.000000, 1908.669130
+6.039582, 0.000000, 1908.799068
+6.049527, 0.000000, 1908.907349
+6.059538, 0.000000, 1909.015630
+6.069537, 0.000000, 1909.102254
+6.079650, 0.000000, 1909.188879
+6.089474, 0.000000, 1909.253848
+6.099533, 0.000000, 1909.318816
+6.109497, 0.000000, 1909.383785
+6.119587, 0.000000, 1909.448754
+6.129255, 0.000000, 1909.492066
+6.139524, 0.000000, 1909.513722
+6.149479, 0.000000, 1909.557035
+6.159584, 0.000000, 1909.578691
+6.169260, 0.000000, 1909.600347
+6.179536, 0.000000, 1909.600347
+6.189396, 0.000000, 1909.600347
+6.199574, 0.000000, 1909.600347
+6.209506, 0.000000, 1909.600347
+6.219521, 0.000000, 1909.600347
+6.229482, 0.000000, 1909.600347
+6.239565, 0.000000, 1909.600347
+6.249254, 0.000000, 1909.600347
+6.259521, 0.000000, 1909.600347
+6.269506, 0.000000, 1909.600347
+6.279589, 0.000000, 1909.600347
+6.289535, 0.000000, 1909.600347
+6.299542, 0.000000, 1909.600347
+6.309460, 0.000000, 1909.600347
+6.319858, 0.000000, 1909.600347
+6.329537, 0.000000, 1909.600347
+6.339535, 0.000000, 1909.600347
+6.349231, 0.000000, 1909.600347
+6.359649, 0.000000, 1909.600347
+6.369534, 0.000000, 1909.600347
+6.379535, 0.000000, 1909.600347
diff --git a/bot3/control_loops/shooter/shooter_lib_test.cc b/bot3/control_loops/shooter/shooter_lib_test.cc
new file mode 100644
index 0000000..062e507
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter_lib_test.cc
@@ -0,0 +1,174 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "bot3/control_loops/shooter/shooter_motor.q.h"
+#include "bot3/control_loops/shooter/shooter.h"
+#include "frc971/constants.h"
+
+using ::aos::time::Time;
+
+namespace bot3 {
+namespace control_loops {
+namespace testing {
+
+// Class which simulates the shooter and sends out queue messages with the
+// position.
+class ShooterMotorSimulation {
+ public:
+  // Constructs a shooter simulation. I'm not sure how the construction of
+  // the queue (my_shooter_loop_) actually works (same format as wrist).
+  ShooterMotorSimulation()
+      : shooter_plant_(new StateFeedbackPlant<2, 1, 1>(MakeShooterPlant())),
+        my_shooter_loop_(".bot3.control_loops.shooter",
+          0x78d8e372, ".bot3.control_loops.shooter.goal",
+          ".bot3.control_loops.shooter.position",
+          ".bot3.control_loops.shooter.output",
+          ".bot3.control_loops.shooter.status") {
+  }
+
+  // Sends a queue message with the position of the shooter.
+  void SendPositionMessage() {
+    ::aos::ScopedMessagePtr<control_loops::ShooterLoop::Position> position =
+      my_shooter_loop_.position.MakeMessage();
+    position->position = shooter_plant_->Y(0, 0);
+    position.Send();
+  }
+
+  // Simulates shooter for a single timestep.
+  void Simulate() {
+    EXPECT_TRUE(my_shooter_loop_.output.FetchLatest());
+    shooter_plant_->U << my_shooter_loop_.output->voltage;
+    shooter_plant_->Update();
+  }
+
+  ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> shooter_plant_;
+
+ private:
+  ShooterLoop my_shooter_loop_;
+};
+
+class ShooterTest : public ::testing::Test {
+ protected:
+  ShooterTest() : my_shooter_loop_(".bot3.control_loops.shooter",
+                                   0x78d8e372,
+                                   ".bot3.control_loops.shooter.goal",
+                                   ".bot3.control_loops.shooter.position",
+                                   ".bot3.control_loops.shooter.output",
+                                   ".bot3.control_loops.shooter.status"),
+                  shooter_motor_(&my_shooter_loop_),
+                  shooter_motor_plant_() {
+    // Flush the robot state queue so we can use clean shared memory for this
+    // test.
+    ::aos::robot_state.Clear();
+    SendDSPacket(true);
+  }
+
+  virtual ~ShooterTest() {
+    ::aos::robot_state.Clear();
+  }
+
+  // Update the robot state. Without this, the Iteration of the control loop
+  // will stop all the motors and the shooter won't go anywhere.
+  void SendDSPacket(bool enabled) {
+    ::aos::robot_state.MakeWithBuilder().enabled(enabled)
+                                        .autonomous(false)
+                                        .team_id(971).Send();
+    ::aos::robot_state.FetchLatest();
+  }
+
+  void VerifyNearGoal() {
+    my_shooter_loop_.goal.FetchLatest();
+    my_shooter_loop_.status.FetchLatest();
+    EXPECT_NEAR(my_shooter_loop_.goal->velocity,
+                my_shooter_loop_.status->average_velocity,
+                10.0);
+  }
+
+  // Bring up and down Core.
+  ::aos::common::testing::GlobalCoreInstance my_core;
+
+  // Create a new instance of the test queue so that it invalidates the queue
+  // that it points to.  Otherwise, we will have a pointed to
+  // shared memory that is no longer valid.
+  ShooterLoop my_shooter_loop_;
+
+  // Create a control loop and simulation.
+  ShooterMotor shooter_motor_;
+  ShooterMotorSimulation shooter_motor_plant_;
+};
+
+// Tests that the shooter does nothing when the goal is zero.
+TEST_F(ShooterTest, DoesNothing) {
+  my_shooter_loop_.goal.MakeWithBuilder().velocity(0.0).Send();
+  SendDSPacket(true);
+  shooter_motor_plant_.SendPositionMessage();
+  shooter_motor_.Iterate();
+  shooter_motor_plant_.Simulate();
+  VerifyNearGoal();
+  my_shooter_loop_.output.FetchLatest();
+  EXPECT_EQ(my_shooter_loop_.output->voltage, 0.0);
+}
+
+// Tests that the shooter spins up to speed and that it then spins down
+// without applying any power.
+TEST_F(ShooterTest, SpinUpAndDown) {
+  my_shooter_loop_.goal.MakeWithBuilder().velocity(450.0).Send();
+  bool is_done = false;
+  while (!is_done) {
+    shooter_motor_plant_.SendPositionMessage();
+    shooter_motor_.Iterate();
+    shooter_motor_plant_.Simulate();
+    SendDSPacket(true);
+    EXPECT_TRUE(my_shooter_loop_.status.FetchLatest());
+    is_done = my_shooter_loop_.status->ready;
+  }
+  VerifyNearGoal();
+
+  my_shooter_loop_.goal.MakeWithBuilder().velocity(0.0).Send();
+  for (int i = 0; i < 100; ++i) {
+    shooter_motor_plant_.SendPositionMessage();
+    shooter_motor_.Iterate();
+    shooter_motor_plant_.Simulate();
+    SendDSPacket(true);
+    EXPECT_TRUE(my_shooter_loop_.output.FetchLatest());
+    EXPECT_EQ(0.0, my_shooter_loop_.output->voltage);
+  }
+}
+
+// Tests that the shooter can spin up nicely while missing position packets.
+TEST_F(ShooterTest, MissingPositionMessages) {
+  my_shooter_loop_.goal.MakeWithBuilder().velocity(200.0).Send();
+  for (int i = 0; i < 100; ++i) {
+    if (i % 7) {
+      shooter_motor_plant_.SendPositionMessage();
+    }
+    shooter_motor_.Iterate();
+    shooter_motor_plant_.Simulate();
+    SendDSPacket(true);
+  }
+
+  VerifyNearGoal();
+}
+
+// Tests that the shooter can spin up nicely while disabled for a while.
+TEST_F(ShooterTest, Disabled) {
+  my_shooter_loop_.goal.MakeWithBuilder().velocity(200.0).Send();
+  for (int i = 0; i < 100; ++i) {
+    if (i % 7) {
+      shooter_motor_plant_.SendPositionMessage();
+    }
+    shooter_motor_.Iterate();
+    shooter_motor_plant_.Simulate();
+    SendDSPacket(i > 50);
+  }
+
+  VerifyNearGoal();
+}
+
+}  // namespace testing
+}  // namespace control_loops
+}  // namespace bot3
diff --git a/bot3/control_loops/shooter/shooter_main.cc b/bot3/control_loops/shooter/shooter_main.cc
new file mode 100644
index 0000000..9907cfe
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter_main.cc
@@ -0,0 +1,11 @@
+#include "bot3/control_loops/shooter/shooter.h"
+
+#include "aos/atom_code/init.h"
+
+int main() {
+  ::aos::Init();
+  bot3::control_loops::ShooterMotor shooter;
+  shooter.Run();
+  ::aos::Cleanup();
+  return 0;
+}
diff --git a/bot3/control_loops/shooter/shooter_motor.q b/bot3/control_loops/shooter/shooter_motor.q
new file mode 100644
index 0000000..87eaa16
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter_motor.q
@@ -0,0 +1,31 @@
+package bot3.control_loops;
+
+import "aos/common/control_loop/control_loops.q";
+
+queue_group ShooterLoop {
+  implements aos.control_loops.ControlLoop;
+
+  message Goal {
+    // Goal velocity in rad/sec
+    double velocity;
+  };
+
+  message Status {
+    // True if the shooter is up to speed.
+    bool ready;
+    // The average velocity over the last 0.1 seconds.
+    double average_velocity;
+  };
+
+  message Position {
+    // The angle of the shooter wheel measured in rad/sec.
+    double position;
+  };
+
+  queue Goal goal;
+  queue Position position;
+  queue aos.control_loops.Output output;
+  queue Status status;
+};
+
+queue_group ShooterLoop shooter;
diff --git a/bot3/control_loops/shooter/shooter_motor_plant.cc b/bot3/control_loops/shooter/shooter_motor_plant.cc
new file mode 100644
index 0000000..0617554
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "bot3/control_loops/shooter/shooter_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace bot3 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeShooterPlantCoefficients() {
+  Eigen::Matrix<double, 2, 2> A;
+  A << 1.0, 0.00992127884628, 0.0, 0.984297191531;
+  Eigen::Matrix<double, 2, 1> B;
+  B << 0.00398899915072, 0.795700859132;
+  Eigen::Matrix<double, 1, 2> C;
+  C << 1, 0;
+  Eigen::Matrix<double, 1, 1> D;
+  D << 0;
+  Eigen::Matrix<double, 1, 1> U_max;
+  U_max << 12.0;
+  Eigen::Matrix<double, 1, 1> U_min;
+  U_min << -12.0;
+  return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<2, 1, 1> MakeShooterController() {
+  Eigen::Matrix<double, 2, 1> L;
+  L << 1.08429719153, 29.2677479765;
+  Eigen::Matrix<double, 1, 2> K;
+  K << 0.955132813139, 0.50205697652;
+  return StateFeedbackController<2, 1, 1>(L, K, MakeShooterPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 1, 1> MakeShooterPlant() {
+  ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
+  plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeShooterPlantCoefficients());
+  return StateFeedbackPlant<2, 1, 1>(plants);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeShooterLoop() {
+  ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
+  controllers[0] = new StateFeedbackController<2, 1, 1>(MakeShooterController());
+  return StateFeedbackLoop<2, 1, 1>(controllers);
+}
+
+}  // namespace control_loops
+}  // namespace bot3
diff --git a/bot3/control_loops/shooter/shooter_motor_plant.h b/bot3/control_loops/shooter/shooter_motor_plant.h
new file mode 100644
index 0000000..1721d30
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace bot3 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeShooterPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeShooterController();
+
+StateFeedbackPlant<2, 1, 1> MakeShooterPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeShooterLoop();
+
+}  // namespace control_loops
+}  // namespace bot3
+
+#endif  // FRC971_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_