diff --git a/frc971/control_loops/python/controls.py b/frc971/control_loops/python/controls.py
new file mode 100644
index 0000000..7d34a85
--- /dev/null
+++ b/frc971/control_loops/python/controls.py
@@ -0,0 +1,83 @@
+#!/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
diff --git a/frc971/control_loops/python/libcdd.py b/frc971/control_loops/python/libcdd.py
new file mode 100644
index 0000000..a217728
--- /dev/null
+++ b/frc971/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/frc971/control_loops/python/polytope.py b/frc971/control_loops/python/polytope.py
new file mode 100644
index 0000000..e08a160
--- /dev/null
+++ b/frc971/control_loops/python/polytope.py
@@ -0,0 +1,224 @@
+#!/usr/bin/python
+
+"""
+Polyhedral set library.
+
+This library implements convex regions of the form H x <= k, where H, x, and k
+are matricies.  It also provides convenient methods to find all the verticies.
+"""
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+
+import libcdd
+import numpy
+import string
+import sys
+
+
+def _PiecewiseConcat(*args):
+  """Concatenates strings inside lists, elementwise.
+
+  Given ['a', 's'] and ['d', 'f'], returns ['ad', 'sf']
+  """
+  return map(''.join, zip(*args))
+
+
+def _SplitAndPad(s):
+  """Splits a string on newlines, and pads the lines to be the same width."""
+  split_string = s.split('\n')
+  width = max(len(stringpiece) for stringpiece in split_string) + 1
+
+  padded_strings = [string.ljust(stringpiece, width, ' ')
+                        for stringpiece in split_string]
+  return padded_strings
+
+
+def _PadHeight(padded_array, min_height):
+  """Adds lines of spaces to the top and bottom of an array symmetrically."""
+  height = len(padded_array)
+  if height < min_height:
+    pad_array = [' ' * len(padded_array[0])]
+    height_error = min_height - height
+    return (pad_array * ((height_error) / 2) +
+            padded_array +
+            pad_array * ((height_error + 1) / 2))
+  return padded_array
+
+
+class HPolytope(object):
+  """This object represents a H-polytope.
+
+  Polytopes are convex regions in n-dimensional space.
+  For H-polytopes, this is represented as the intersection of a set of half
+  planes.  The mathematic equation that represents this is H x <= k.
+  """
+
+  def __init__(self, H, k):
+    """Constructs a H-polytope from the H and k matricies.
+
+    Args:
+      H: numpy.Matrix (n by k), where n is the number of constraints, and k is
+        the number of dimensions in the space.  Does not copy the matrix.
+      k: numpy.Matrix (n by 1).  Does not copy the matrix.
+    """
+    self._H = H
+    self._k = k
+
+  @property
+  def k(self):
+    """Returns the k in H x <= k."""
+    return self._k
+
+  @property
+  def H(self):
+    """Returns the H in H x <= k."""
+    return self._H
+
+  @property
+  def ndim(self):
+    """Returns the dimension of the set."""
+    return self._H.shape[1]
+
+  @property
+  def num_constraints(self):
+    """Returns the number of constraints defining the set."""
+    return self._k.shape[0]
+
+  def IsInside(self, point):
+    """Returns true if the point is inside the polytope, edges included."""
+    return (self._H * point <= self._k).all()
+
+  def Vertices(self):
+    """Returns a matrix with the vertices of the set in its rows."""
+    # TODO(aschuh): It would be better to write some small C/C++ function that
+    # does all of this and takes in a numpy array.
+    # The copy is expensive in Python and cheaper in C.
+
+    # Create an empty matrix with the correct size.
+    matrixptr = libcdd.dd_CreateMatrix(self.num_constraints, self.ndim + 1)
+    matrix = matrixptr.contents
+
+    try:
+      # Copy the data into the matrix.
+      for i in xrange(self.num_constraints):
+        libcdd.dd_set_d(matrix.matrix[i][0], self._k[i, 0])
+        for j in xrange(self.ndim):
+          libcdd.dd_set_d(matrix.matrix[i][j + 1], -self._H[i, j])
+
+      # Set enums to the correct values.
+      matrix.representation = libcdd.DD_INEQUALITY
+      matrix.numbtype = libcdd.DD_REAL
+
+      # TODO(aschuh): Set linearity if it is useful.
+      # This would be useful if we had any constraints saying B - A x = 0
+
+      # Build a Polyhedra
+      polyhedraptr = libcdd.dd_DDMatrix2Poly(matrixptr)
+
+      if not polyhedraptr:
+        return None
+
+      try:
+        # Return None on error.
+        # The error values are enums, so they aren't exposed.
+
+        # Magic happens here.  Computes the vertices
+        vertex_matrixptr = libcdd.dd_CopyGenerators(
+            polyhedraptr)
+        vertex_matrix = vertex_matrixptr.contents
+
+        try:
+          # Count the number of vertices and rays in the result.
+          num_vertices = 0
+          num_rays = 0
+          for i in xrange(vertex_matrix.rowsize):
+            if libcdd.dd_get_d(vertex_matrix.matrix[i][0]) == 0:
+              num_rays += 1
+            else:
+              num_vertices += 1
+
+          # Build zeroed matricies for the new vertices and rays.
+          vertices = numpy.matrix(numpy.zeros((num_vertices,
+                                               vertex_matrix.colsize - 1)))
+          rays = numpy.matrix(numpy.zeros((num_rays,
+                                           vertex_matrix.colsize - 1)))
+
+          ray_index = 0
+          vertex_index = 0
+
+          # Copy the data out of the matrix.
+          for index in xrange(vertex_matrix.rowsize):
+            if libcdd.dd_get_d(vertex_matrix.matrix[index][0]) == 0.0:
+              for j in xrange(vertex_matrix.colsize - 1):
+                rays[ray_index, j] = libcdd.dd_get_d(
+                    vertex_matrix.matrix[index][j + 1])
+              ray_index += 1
+            else:
+              for j in xrange(vertex_matrix.colsize - 1):
+                vertices[vertex_index, j] = libcdd.dd_get_d(
+                    vertex_matrix.matrix[index][j + 1])
+              vertex_index += 1
+        finally:
+          # Free everything.
+          libcdd.dd_FreeMatrix(vertex_matrixptr)
+
+      finally:
+        libcdd.dd_FreePolyhedra(polyhedraptr)
+
+    finally:
+      libcdd.dd_FreeMatrix(matrixptr)
+
+    # Rays are unsupported right now.  This may change in the future.
+    assert(rays.shape[0] == 0)
+
+    return vertices
+
+
+  def __str__(self):
+    """Returns a formatted version of the polytope.
+
+    The dump will look something like the following, which prints out the matrix
+    comparison.
+
+    [[ 1  0]            [[12]
+     [-1  0]  [[x0]  <=  [12]
+     [ 0  1]   [x1]]     [12]
+     [ 0 -1]]            [12]]
+    """
+    height = max(self.ndim, self.num_constraints)
+
+    # Split the print up into 4 parts and concatenate them all.
+    H_strings = _PadHeight(_SplitAndPad(str(self.H)), height)
+    v_strings = _PadHeight(_SplitAndPad(str(self.k)), height)
+    x_strings = _PadHeight(self._MakeXStrings(), height)
+    cmp_strings = self._MakeCmpStrings(height)
+
+    return '\n'.join(_PiecewiseConcat(H_strings, x_strings,
+                                      cmp_strings, v_strings))
+
+  def _MakeXStrings(self):
+    """Builds an array of strings with constraint names in it for printing."""
+    x_strings = []
+    if self.ndim == 1:
+      x_strings = ["[[x0]] "]
+    else:
+      for index in xrange(self.ndim):
+        if index == 0:
+          x = "[[x%d]  " % index
+        elif index == self.ndim - 1:
+          x = " [x%d]] " % index
+        else:
+          x = " [x%d]  " % index
+        x_strings.append(x)
+    return x_strings
+
+  def _MakeCmpStrings(self, height):
+    """Builds an array of strings with the comparison in it for printing."""
+    cmp_strings = []
+    for index in xrange(height):
+      if index == (height - 1) / 2:
+        cmp_strings.append("<= ")
+      else:
+        cmp_strings.append("   ")
+    return cmp_strings
diff --git a/frc971/control_loops/python/polytope_test.py b/frc971/control_loops/python/polytope_test.py
new file mode 100755
index 0000000..9a35ebe
--- /dev/null
+++ b/frc971/control_loops/python/polytope_test.py
@@ -0,0 +1,192 @@
+#!/usr/bin/python
+
+import numpy
+from numpy.testing import *
+import polytope
+import unittest
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+def MakePoint(*args):
+  """Makes a point from a set of arguments."""
+  return numpy.matrix([[arg] for arg in args])
+
+class TestHPolytope(unittest.TestCase):
+  def setUp(self):
+    """Builds a simple box polytope."""
+    self.H = numpy.matrix([[1, 0],
+                           [-1, 0],
+                           [0, 1],
+                           [0, -1]])
+    self.k = numpy.matrix([[12],
+                           [12],
+                           [12],
+                           [12]])
+    self.p = polytope.HPolytope(self.H, self.k)
+
+  def test_Hk(self):
+    """Tests that H and k are saved correctly."""
+    assert_array_equal(self.p.H, self.H)
+    assert_array_equal(self.p.k, self.k)
+
+  def test_IsInside(self):
+    """Tests IsInside for various points."""
+    inside_points = [
+      MakePoint(0, 0),
+      MakePoint(6, 6),
+      MakePoint(12, 6),
+      MakePoint(-6, 10)]
+    outside_points = [
+      MakePoint(14, 0),
+      MakePoint(-14, 0),
+      MakePoint(0, 14),
+      MakePoint(0, -14),
+      MakePoint(14, -14)]
+
+    for inside_point in inside_points:
+      self.assertTrue(self.p.IsInside(inside_point),
+                      msg='Point is' + str(inside_point))
+
+    for outside_point in outside_points:
+      self.assertFalse(self.p.IsInside(outside_point),
+                       msg='Point is' + str(outside_point))
+
+  def AreVertices(self, p, vertices):
+    """Checks that all the vertices are on corners of the set."""
+    for i in xrange(vertices.shape[0]):
+      # Check that all the vertices have the correct number of active
+      # constraints.
+      lmda = p.H * vertices[i,:].T - p.k
+      num_active_constraints = 0
+      for j in xrange(lmda.shape[0]):
+        # Verify that the constraints are either active, or not violated.
+        if numpy.abs(lmda[j, 0]) <= 1e-9:
+          num_active_constraints += 1
+        else:
+          self.assertLessEqual(lmda[j, 0], 0.0)
+
+      self.assertEqual(p.ndim, num_active_constraints)
+
+  def HasSamePoints(self, expected, actual):
+    """Verifies that the points in expected are in actual."""
+    found_points = set()
+    self.assertEqual(expected.shape, actual.shape)
+    for index in xrange(expected.shape[0]):
+      expected_point = expected[index, :]
+      for actual_index in xrange(actual.shape[0]):
+        actual_point = actual[actual_index, :]
+        if numpy.abs(expected_point - actual_point).max() <= 1e-4:
+          found_points.add(actual_index)
+          break
+
+    self.assertEqual(len(found_points), actual.shape[0],
+        msg="Expected:\n" + str(expected) + "\nActual:\n" + str(actual))
+
+  def test_Skewed_Nonsym_Vertices(self):
+    """Tests the vertices of a severely skewed space."""
+    self.H = numpy.matrix([[10, -1],
+                           [-1, -1],
+                           [-1, 10],
+                           [10, 10]])
+    self.k = numpy.matrix([[2],
+                           [2],
+                           [2],
+                           [2]])
+    self.p = polytope.HPolytope(self.H, self.k)
+    vertices = self.p.Vertices()
+    self.AreVertices(self.p, vertices)
+
+    self.HasSamePoints(
+        numpy.matrix([[0., 0.2],
+                      [0.2, 0.],
+                      [-2., 0.],
+                      [0., -2.]]),
+        vertices)
+
+  def test_Vertices_Nonsym(self):
+    """Tests the vertices of a nonsymetric space."""
+    self.k = numpy.matrix([[6],
+                           [12],
+                           [2],
+                           [10]])
+    self.p = polytope.HPolytope(self.H, self.k)
+    vertices = self.p.Vertices()
+    self.AreVertices(self.p, vertices)
+
+    self.HasSamePoints(
+        numpy.matrix([[6., 2.],
+                      [6., -10.],
+                      [-12., -10.],
+                      [-12., 2.]]),
+        vertices)
+
+  def test_Vertices(self):
+    """Tests the vertices of a nonsymetric space."""
+    self.HasSamePoints(self.p.Vertices(),
+                       numpy.matrix([[12., 12.],
+                                     [12., -12.],
+                                     [-12., -12.],
+                                     [-12., 12.]]))
+
+  def test_concat(self):
+    """Tests that the concat function works for simple inputs."""
+    self.assertEqual(["asd", "qwe"],
+                     polytope._PiecewiseConcat(["a", "q"],
+                                               ["s", "w"],
+                                               ["d", "e"]))
+
+  def test_str(self):
+    """Verifies that the str method works for the provided p."""
+    self.assertEqual('[[ 1  0]            [[12]  \n'
+                     ' [-1  0]  [[x0]  <=  [12]  \n'
+                     ' [ 0  1]   [x1]]     [12]  \n'
+                     ' [ 0 -1]]            [12]] ',
+                     str(self.p))
+
+  def MakePWithDims(self, num_constraints, num_dims):
+    """Makes a zeroed out polytope with the correct size."""
+    self.p = polytope.HPolytope(
+        numpy.matrix(numpy.zeros((num_constraints, num_dims))),
+        numpy.matrix(numpy.zeros((num_constraints, 1))))
+
+  def test_few_constraints_odd_constraint_even_dims_str(self):
+    """Tests printing out the set with odd constraints and even dimensions."""
+    self.MakePWithDims(num_constraints=5, num_dims=2)
+    self.assertEqual('[[ 0.  0.]            [[ 0.]  \n'
+                     ' [ 0.  0.]  [[x0]      [ 0.]  \n'
+                     ' [ 0.  0.]   [x1]] <=  [ 0.]  \n'
+                     ' [ 0.  0.]             [ 0.]  \n'
+                     ' [ 0.  0.]]            [ 0.]] ',
+                     str(self.p))
+
+  def test_few_constraints_odd_constraint_small_dims_str(self):
+    """Tests printing out the set with odd constraints and odd dimensions."""
+    self.MakePWithDims(num_constraints=5, num_dims=1)
+    self.assertEqual('[[ 0.]            [[ 0.]  \n'
+                     ' [ 0.]             [ 0.]  \n'
+                     ' [ 0.]  [[x0]] <=  [ 0.]  \n'
+                     ' [ 0.]             [ 0.]  \n'
+                     ' [ 0.]]            [ 0.]] ',
+                     str(self.p))
+
+  def test_few_constraints_odd_constraint_odd_dims_str(self):
+    """Tests printing out the set with odd constraints and odd dimensions."""
+    self.MakePWithDims(num_constraints=5, num_dims=3)
+    self.assertEqual('[[ 0.  0.  0.]            [[ 0.]  \n'
+                     ' [ 0.  0.  0.]  [[x0]      [ 0.]  \n'
+                     ' [ 0.  0.  0.]   [x1]  <=  [ 0.]  \n'
+                     ' [ 0.  0.  0.]   [x2]]     [ 0.]  \n'
+                     ' [ 0.  0.  0.]]            [ 0.]] ',
+                     str(self.p))
+
+  def test_many_constraints_even_constraint_odd_dims_str(self):
+    """Tests printing out the set with even constraints and odd dimensions."""
+    self.MakePWithDims(num_constraints=2, num_dims=3)
+    self.assertEqual('[[ 0.  0.  0.]  [[x0]     [[ 0.]  \n'
+                     ' [ 0.  0.  0.]]  [x1]  <=  [ 0.]] \n'
+                     '                 [x2]]            ',
+                     str(self.p))
+
+
+if __name__ == '__main__':
+  unittest.main()