diff --git a/aos/build/download_externals.sh b/aos/build/download_externals.sh
index f671ac3..13c8eff 100755
--- a/aos/build/download_externals.sh
+++ b/aos/build/download_externals.sh
@@ -121,3 +121,18 @@
   CFLAGS='-m32' CXXFLAGS='-m32' LDFLAGS='-m32' \
   bash -c "cd ${LIBEVENT_DIR} && ./configure \
   --prefix=`readlink -f ${LIBEVENT_PREFIX}` && make && make install"
+
+# get and build libcdd
+LIBCDD_VERSION=094g
+LIBCDD_DIR=${EXTERNALS}/libcdd-${LIBCDD_VERSION}
+LIBCDD_PREFIX=${LIBCDD_DIR}-prefix
+LIBCDD_LIB=${LIBCDD_PREFIX}/lib/libcdd.a
+LIBCDD_URL=ftp://ftp.ifor.math.ethz.ch/pub/fukuda/cdd/cddlib-${LIBCDD_VERSION}.tar.gz
+[ -f ${LIBCDD_DIR}.tar.gz ] || \
+        wget ${LIBCDD_URL} -O ${LIBCDD_DIR}.tar.gz
+[ -d ${LIBCDD_DIR} ] || ( mkdir ${LIBCDD_DIR} && tar \
+        --strip-components=1 -C ${LIBCDD_DIR} -xf ${LIBCDD_DIR}.tar.gz )
+[ -f ${LIBCDD_LIB} ] || env -i PATH="${PATH}" \
+        CFLAGS='-m32' CXXFLAGS='-m32' LDFLAGS='-m32' \
+        bash -c "cd ${LIBCDD_DIR} && ./configure --disable-shared \
+        --prefix=`readlink -f ${LIBCDD_PREFIX}` && make && make install"
diff --git a/aos/build/externals.gyp b/aos/build/externals.gyp
index 2bfee69..99b26cf 100644
--- a/aos/build/externals.gyp
+++ b/aos/build/externals.gyp
@@ -15,6 +15,7 @@
     'libusb_apiversion': '1.0',
     'compiler_rt_version': 'RELEASE_32_final',
     'libevent_version': '2.0.21',
+    'libcdd_version': '094g',
   },
   'targets': [
     {
@@ -207,6 +208,16 @@
         'include_dirs': ['<(externals)/libusb-<(libusb_version)-prefix/include'],
       },
     },
+    {
+      'target_name': 'libcdd',
+      'type': 'none',
+      'link_settings': {
+        'libraries': ['<(externals_abs)/libcdd-<(libcdd_version)-prefix/lib/libcdd.a'],
+      },
+      'direct_dependent_settings': {
+        'include_dirs': ['<(externals_abs)/libcdd-<(libcdd_version)-prefix/include'],
+      },
+    },
   ],
   'includes': [
     'libgcc-additions/libgcc-additions.gypi',
diff --git a/aos/controls/polytope.h b/aos/controls/polytope.h
new file mode 100644
index 0000000..ed4b36d
--- /dev/null
+++ b/aos/controls/polytope.h
@@ -0,0 +1,126 @@
+#ifndef _AOS_CONTROLS_POLYTOPE_H_
+#define _AOS_CONTROLS_POLYTOPE_H_
+
+#include "Eigen/Dense"
+#include "aos/externals/libcdd-094g-prefix/include/setoper.h"
+#include "aos/externals/libcdd-094g-prefix/include/cdd.h"
+
+namespace aos {
+namespace controls {
+
+// A n dimension polytope.
+template <int number_of_dimensions>
+class HPolytope {
+ public:
+  // Constructs a polytope given the H and k matricies.
+  HPolytope(Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions> H,
+            Eigen::Matrix<double, Eigen::Dynamic, 1> k)
+      : H_(H),
+        k_(k) {
+  }
+
+  static void Init() {
+    dd_set_global_constants();
+  }
+
+  // Returns a reference to H.
+  const Eigen::Matrix<double, Eigen::Dynamic,
+                      number_of_dimensions> &H() const {
+    return H_;
+  }
+
+  // Returns a reference to k.
+  const Eigen::Matrix<double, Eigen::Dynamic,
+                      1> &k() const {
+    return k_;
+  }
+
+  // Returns the number of dimensions in the polytope.
+  int ndim() const { return number_of_dimensions; }
+
+  // Returns the number of constraints currently in the polytope.
+  int num_constraints() const { return k_.rows(); }
+
+  // Returns true if the point is inside the polytope.
+  bool IsInside(Eigen::Matrix<double, number_of_dimensions, 1> point);
+
+  // Returns the list of vertices inside the polytope.
+  Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> Vertices();
+
+ private:
+  Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions> H_;
+  Eigen::Matrix<double, Eigen::Dynamic, 1> k_;
+};
+
+template <int number_of_dimensions>
+bool HPolytope<number_of_dimensions>::IsInside(
+    Eigen::Matrix<double, number_of_dimensions, 1> point) {
+  auto ev = H_ * point;
+  for (int i = 0; i < num_constraints(); ++i) {
+    if (ev(i, 0) > k_(i, 0)) {
+      return false;
+    }
+  }
+  return true;
+}
+
+template <int number_of_dimensions>
+Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic>
+    HPolytope<number_of_dimensions>::Vertices() {
+  dd_MatrixPtr matrix = dd_CreateMatrix(num_constraints(), ndim() + 1);
+
+  // Copy the data over. TODO(aschuh): Is there a better way?  I hate copying...
+  for (int i = 0; i < num_constraints(); ++i) {
+    dd_set_d(matrix->matrix[i][0], k_(i, 0));
+    for (int j = 0; j < ndim(); ++j) {
+      dd_set_d(matrix->matrix[i][j + 1], -H_(i, j));
+    }
+  }
+
+  matrix->representation = dd_Inequality;
+  matrix->numbtype = dd_Real;
+
+  dd_ErrorType error;
+  dd_PolyhedraPtr polyhedra = dd_DDMatrix2Poly(matrix, &error);
+  if (error != dd_NoError || polyhedra == NULL) {
+    dd_WriteErrorMessages(stderr, error);
+    dd_FreeMatrix(matrix);
+    Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> ans(0, 0);
+    return ans;
+  }
+
+  dd_MatrixPtr vertex_matrix = dd_CopyGenerators(polyhedra);
+
+  int num_vertices = 0;
+  int num_rays = 0;
+  for (int i = 0; i < vertex_matrix->rowsize; ++i) {
+    if (dd_get_d(vertex_matrix->matrix[i][0]) == 0) {
+      num_rays += 1;
+    } else {
+      num_vertices += 1;
+    }
+  }
+
+  Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> vertices(
+      number_of_dimensions, num_vertices);
+
+  int vertex_index = 0;
+  for (int i = 0; i < vertex_matrix->rowsize; ++i) {
+    if (dd_get_d(vertex_matrix->matrix[i][0]) != 0) {
+      for (int j = 0; j < number_of_dimensions; ++j) {
+        vertices(j, vertex_index) = dd_get_d(vertex_matrix->matrix[i][j + 1]);
+      }
+      ++vertex_index;
+    }
+  }
+  dd_FreeMatrix(vertex_matrix);
+  dd_FreePolyhedra(polyhedra);
+  dd_FreeMatrix(matrix);
+
+  return vertices;
+}
+
+}  // namespace controls
+}  // namespace aos
+
+#endif  // _AOS_CONTROLS_POLYTOPE_H_
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 548113e..122e2d2 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -4,11 +4,15 @@
 #include <sched.h>
 #include <cmath>
 #include <memory>
+#include "Eigen/Dense"
 
 #include "aos/common/logging/logging.h"
 #include "aos/common/queue.h"
+#include "aos/controls/polytope.h"
+#include "aos/common/commonmath.h"
 #include "frc971/control_loops/state_feedback_loop.h"
 #include "frc971/control_loops/drivetrain/drivetrain_motor_plant.h"
+#include "frc971/control_loops/drivetrain/polydrivetrain_motor_plant.h"
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "frc971/queues/GyroAngle.q.h"
 #include "frc971/queues/Piston.q.h"
@@ -21,6 +25,53 @@
 // Width of the robot.
 const double width = 22.0 / 100.0 * 2.54;
 
+Eigen::Matrix<double, 2, 1> CoerceGoal(aos::controls::HPolytope<2> &region,
+                                       const Eigen::Matrix<double, 1, 2> &K,
+                                       double w,
+                                       const Eigen::Matrix<double, 2, 1> &R) {
+  if (region.IsInside(R)) {
+    return R;
+  }
+  Eigen::Matrix<double, 2, 1> parallel_vector;
+  Eigen::Matrix<double, 2, 1> perpendicular_vector;
+  perpendicular_vector = K.transpose().normalized();
+  parallel_vector << perpendicular_vector(1, 0), -perpendicular_vector(0, 0);
+
+  aos::controls::HPolytope<1> t_poly(
+      region.H() * parallel_vector,
+      region.k() - region.H() * perpendicular_vector * w);
+
+  Eigen::Matrix<double, 1, Eigen::Dynamic> vertices = t_poly.Vertices();
+  if (vertices.innerSize() > 0) {
+    double min_distance_sqr = 0;
+    Eigen::Matrix<double, 2, 1> closest_point;
+    for (int i = 0; i < vertices.innerSize(); i++) {
+      Eigen::Matrix<double, 2, 1> point;
+      point = parallel_vector * vertices(0, i) + perpendicular_vector * w;
+      const double length = (R - point).squaredNorm();
+      if (i == 0 || length < min_distance_sqr) {
+        closest_point = point;
+        min_distance_sqr = length;
+      }
+    }
+    return closest_point;
+  } else {
+    Eigen::Matrix<double, 2, Eigen::Dynamic> region_vertices =
+        region.Vertices();
+    double min_distance;
+    int closest_i = 0;
+    for (int i = 0; i < region_vertices.outerSize(); i++) {
+      const double length = ::std::abs(
+          (perpendicular_vector.transpose() * (region_vertices.col(i)))(0, 0));
+      if (i == 0 || length < min_distance) {
+        closest_i = i;
+        min_distance = length;
+      }
+    }
+    return region_vertices.col(closest_i);
+  }
+}
+
 class DrivetrainMotorsSS {
  public:
   DrivetrainMotorsSS ()
@@ -86,23 +137,155 @@
   bool _control_loop_driving;
 };
 
+class PolyDrivetrain {
+ public:
+  PolyDrivetrain()
+      : U_Poly_((Eigen::Matrix<double, 4, 2>() << /*[[*/ 1, 0 /*]*/,
+                 /*[*/ -1, 0 /*]*/,
+                 /*[*/ 0, 1 /*]*/,
+                 /*[*/ 0, -1 /*]]*/).finished(),
+                (Eigen::Matrix<double, 4, 1>() << /*[[*/ 12 /*]*/,
+                 /*[*/ 12 /*]*/,
+                 /*[*/ 12 /*]*/,
+                 /*[*/ 12 /*]]*/).finished()),
+        loop_(new StateFeedbackLoop<2, 2, 2>(MakeVDrivetrainLoop())) {
+
+    ttrust_ = 1.0;
+
+    wheel_ = 0.0;
+    throttle_ = 0.0;
+    quickturn_ = false;
+    highgear_ = true;
+  }
+  void SetGoal(double wheel, double throttle, bool quickturn, bool highgear) {
+    const double kWheelNonLinearity = 0.1;
+    // Apply a sin function that's scaled to make it feel better.
+    const double angular_range = M_PI_2 * kWheelNonLinearity;
+    wheel_ = sin(angular_range * wheel) / sin(angular_range);
+    wheel_ = sin(angular_range * wheel_) / sin(angular_range);
+    throttle_ = throttle;
+    quickturn_ = quickturn;
+    highgear_ = highgear;
+    if (highgear_) {
+      loop_->set_controller_index(3);
+    } else {
+      loop_->set_controller_index(0);
+    }
+  }
+  double FilterVelocity(double throttle) {
+    const Eigen::Matrix<double, 2, 2> FF =
+        loop_->B().inverse() *
+        (Eigen::Matrix<double, 2, 2>::Identity() - loop_->A());
+
+    constexpr int kHighGearController = 3;
+    const Eigen::Matrix<double, 2, 2> FF_high =
+        loop_->controller(kHighGearController).plant.B.inverse() *
+        (Eigen::Matrix<double, 2, 2>::Identity() -
+         loop_->controller(kHighGearController).plant.A);
+
+    ::Eigen::Matrix<double, 1, 2> FF_sum = FF.colwise().sum();
+    int min_FF_sum_index;
+    const double min_FF_sum = FF_sum.minCoeff(&min_FF_sum_index);
+    const double min_K_sum = loop_->K().col(min_FF_sum_index).sum();
+    const double high_min_FF_sum = FF_high.col(0).sum();
+
+    const double adjusted_ff_voltage = ::aos::Clip(
+        throttle * 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0);
+    return ((adjusted_ff_voltage +
+             ttrust_ * min_K_sum * (loop_->X_hat(0, 0) + loop_->X_hat(1, 0)) /
+                 2.0) /
+            (ttrust_ * min_K_sum + min_FF_sum));
+  }
+
+  void Update() {
+    // FF * X = U (steady state)
+    const Eigen::Matrix<double, 2, 2> FF =
+        loop_->B().inverse() *
+        (Eigen::Matrix<double, 2, 2>::Identity() - loop_->A());
+
+    // Invert the plant to figure out how the velocity filter would have to work
+    // out in order to filter out the forwards negative inertia.
+    // This math assumes that the left and right power and velocity are equals,
+    // and that the plant is the same on the left and right.
+    const double fvel = FilterVelocity(throttle_);
+
+    const double sign_svel = wheel_ * ((fvel > 0.0) ? 1.0 : -1.0);
+    const double svel = ::std::abs(fvel) * wheel_;
+    const double left_velocity = fvel - svel;
+    const double right_velocity = fvel + svel;
+
+    // K * R = w
+    Eigen::Matrix<double,1,2> equality_k;
+    equality_k << 1 + sign_svel, -(1 - sign_svel);
+    const double equality_w = 0.0;
+
+    // Integrate velocity to get the position.
+    // This position is used to get integral control.
+    loop_->R << left_velocity, right_velocity;
+
+    // Construct a constraint on R by manipulating the constraint on U
+    ::aos::controls::HPolytope<2> R_poly = ::aos::controls::HPolytope<2>(
+        U_Poly_.H() * (loop_->K() + FF),
+        U_Poly_.k() + U_Poly_.H() * loop_->K() * loop_->X_hat);
+
+    // Limit R back inside the box.
+    const Eigen::Matrix<double, 2, 1> boxed_R =
+        CoerceGoal(R_poly, equality_k, equality_w, loop_->R);
+
+    const Eigen::Matrix<double, 2, 1> FF_volts = FF * boxed_R;
+    const Eigen::Matrix<double, 2, 1> U_ideal =
+        loop_->K() * (boxed_R - loop_->X_hat) + FF_volts;
+
+    for (int i = 0; i < 2; i++) {
+      loop_->U[i] = ::aos::Clip(U_ideal[i], -12, 12);
+    }
+
+    // TODO(austin): Feed back?
+    loop_->X_hat = loop_->A() * loop_->X_hat + loop_->B() * loop_->U;
+  }
+
+  void SendMotors(Drivetrain::Output *output) {
+    LOG(DEBUG, "left pwm: %f right pwm: %f wheel: %f throttle: %f\n",
+        loop_->U(0, 0), loop_->U(1, 0), wheel_, throttle_);
+    output->left_voltage = loop_->U(0, 0);
+    output->right_voltage = loop_->U(1, 0);
+    if (highgear_) {
+      shifters.MakeWithBuilder().set(false).Send();
+    } else {
+      shifters.MakeWithBuilder().set(true).Send();
+    }
+  }
+
+ private:
+  const ::aos::controls::HPolytope<2> U_Poly_;
+
+  ::std::unique_ptr<StateFeedbackLoop<2, 2, 2>> loop_;
+
+  double ttrust_;
+  double wheel_;
+  double throttle_;
+  bool quickturn_;
+  bool highgear_;
+};
+
+
 class DrivetrainMotorsOL {
  public:
   DrivetrainMotorsOL() {
     _old_wheel = 0.0;
-    _wheel = 0.0;
-    _throttle = 0.0;
-    _quickturn = false;
-    _highgear = true;
+    wheel_ = 0.0;
+    throttle_ = 0.0;
+    quickturn_ = false;
+    highgear_ = true;
     _neg_inertia_accumulator = 0.0;
     _left_pwm = 0.0;
     _right_pwm = 0.0;
   }
   void SetGoal(double wheel, double throttle, bool quickturn, bool highgear) {
-    _wheel = wheel;
-    _throttle = throttle;
-    _quickturn = quickturn;
-    _highgear = highgear;
+    wheel_ = wheel;
+    throttle_ = throttle;
+    quickturn_ = quickturn;
+    highgear_ = highgear;
     _left_pwm = 0.0;
     _right_pwm = 0.0;
   }
@@ -113,35 +296,35 @@
     float linear_power;
     double wheel;
 
-    double neg_inertia = _wheel - _old_wheel;
-    _old_wheel = _wheel;
+    double neg_inertia = wheel_ - _old_wheel;
+    _old_wheel = wheel_;
 
     double wheelNonLinearity;
-    if (_highgear) {
+    if (highgear_) {
       wheelNonLinearity = 0.1;  // used to be csvReader->TURN_NONLIN_HIGH
       // Apply a sin function that's scaled to make it feel better.
       const double angular_range = M_PI / 2.0 * wheelNonLinearity;
-      wheel = sin(angular_range * _wheel) / sin(angular_range);
+      wheel = sin(angular_range * wheel_) / sin(angular_range);
       wheel = sin(angular_range * wheel) / sin(angular_range);
     } else {
       wheelNonLinearity = 0.2;  // used to be csvReader->TURN_NONLIN_LOW
       // Apply a sin function that's scaled to make it feel better.
       const double angular_range = M_PI / 2.0 * wheelNonLinearity;
-      wheel = sin(angular_range * _wheel) / sin(angular_range);
+      wheel = sin(angular_range * wheel_) / sin(angular_range);
       wheel = sin(angular_range * wheel) / sin(angular_range);
       wheel = sin(angular_range * wheel) / sin(angular_range);
     }
 
     static const double kThrottleDeadband = 0.05;
-    if (::std::abs(_throttle) < kThrottleDeadband) {
-      _throttle = 0;
+    if (::std::abs(throttle_) < kThrottleDeadband) {
+      throttle_ = 0;
     } else {
-      _throttle = copysign((::std::abs(_throttle) - kThrottleDeadband) /
-                           (1.0 - kThrottleDeadband), _throttle);
+      throttle_ = copysign((::std::abs(throttle_) - kThrottleDeadband) /
+                           (1.0 - kThrottleDeadband), throttle_);
     }
 
     double neg_inertia_scalar;
-    if (_highgear) {
+    if (highgear_) {
       neg_inertia_scalar = 8.0;  // used to be csvReader->NEG_INTERTIA_HIGH
       sensitivity = 1.22; // used to be csvReader->SENSE_HIGH
     } else {
@@ -168,9 +351,9 @@
       _neg_inertia_accumulator = 0;
     }
 
-    linear_power = _throttle;
+    linear_power = throttle_;
 
-    if (_quickturn) {
+    if (quickturn_) {
       double qt_angular_power = wheel;
       if (::std::abs(linear_power) < 0.2) {
         if (qt_angular_power > 1) qt_angular_power = 1.0;
@@ -179,7 +362,7 @@
         qt_angular_power = 0.0;
       }
       overPower = 1.0;
-      if (_highgear) {
+      if (highgear_) {
         sensitivity = 1.0;
       } else {
         sensitivity = 1.0;
@@ -187,7 +370,7 @@
       angular_power = wheel;
     } else {
       overPower = 0.0;
-      angular_power = ::std::abs(_throttle) * wheel * sensitivity;
+      angular_power = ::std::abs(throttle_) * wheel * sensitivity;
     }
 
     _right_pwm = _left_pwm = linear_power;
@@ -211,12 +394,12 @@
 
   void SendMotors(Drivetrain::Output *output) {
     LOG(DEBUG, "left pwm: %f right pwm: %f wheel: %f throttle: %f\n",
-        _left_pwm, _right_pwm, _wheel, _throttle);
+        _left_pwm, _right_pwm, wheel_, throttle_);
     if (output) {
       output->left_voltage = _left_pwm * 12.0;
       output->right_voltage = _right_pwm * 12.0;
     }
-    if (_highgear) {
+    if (highgear_) {
       shifters.MakeWithBuilder().set(false).Send();
     } else {
       shifters.MakeWithBuilder().set(true).Send();
@@ -225,10 +408,10 @@
 
  private:
   double _old_wheel;
-  double _wheel;
-  double _throttle;
-  bool _quickturn;
-  bool _highgear;
+  double wheel_;
+  double throttle_;
+  bool quickturn_;
+  bool highgear_;
   double _neg_inertia_accumulator;
   double _left_pwm;
   double _right_pwm;
@@ -240,7 +423,7 @@
                                   Drivetrain::Status * /*status*/) {
   // TODO(aschuh): These should be members of the class.
   static DrivetrainMotorsSS dt_closedloop;
-  static DrivetrainMotorsOL dt_openloop;
+  static PolyDrivetrain dt_openloop;
 
   bool bad_pos = false;
   if (position == NULL) {
@@ -257,20 +440,19 @@
   double left_goal = goal->left_goal;
   double right_goal = goal->right_goal;
 
-  dt_closedloop.SetGoal(left_goal, goal->left_velocity_goal,
-                        right_goal, goal->right_velocity_goal);
+  dt_closedloop.SetGoal(left_goal, goal->left_velocity_goal, right_goal,
+                        goal->right_velocity_goal);
   if (!bad_pos) {
     const double left_encoder = position->left_encoder;
     const double right_encoder = position->right_encoder;
     if (gyro.FetchLatest()) {
-      dt_closedloop.SetPosition(left_encoder, right_encoder,
-          gyro->angle, control_loop_driving);
+      dt_closedloop.SetPosition(left_encoder, right_encoder, gyro->angle,
+                                control_loop_driving);
     } else {
       dt_closedloop.SetRawPosition(left_encoder, right_encoder);
     }
   }
   dt_closedloop.Update(position, output == NULL);
-  //dt_closedloop.PrintMotors();
   dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
   dt_openloop.Update();
   if (control_loop_driving) {
diff --git a/frc971/control_loops/drivetrain/drivetrain.gyp b/frc971/control_loops/drivetrain/drivetrain.gyp
index 8e98e4b..89a2321 100644
--- a/frc971/control_loops/drivetrain/drivetrain.gyp
+++ b/frc971/control_loops/drivetrain/drivetrain.gyp
@@ -23,15 +23,18 @@
       'sources': [
         'drivetrain.cc',
         'drivetrain_motor_plant.cc',
+        'polydrivetrain_motor_plant.cc',
       ],
       'dependencies': [
         'drivetrain_loop',
         '<(AOS)/common/common.gyp:controls',
         '<(DEPTH)/frc971/frc971.gyp:common',
+        '<(DEPTH)/aos/build/externals.gyp:libcdd',
         '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
         '<(DEPTH)/frc971/queues/queues.gyp:queues',
       ],
       'export_dependent_settings': [
+        '<(DEPTH)/aos/build/externals.gyp:libcdd',
         '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
         '<(AOS)/common/common.gyp:controls',
         'drivetrain_loop',
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 244f3e5..cdfe744 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -1,12 +1,20 @@
 #ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_H_
 #define FRC971_CONTROL_LOOPS_DRIVETRAIN_H_
 
+#include "Eigen/Dense"
+
+#include "aos/controls/polytope.h"
 #include "aos/common/control_loop/ControlLoop.h"
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
 
 namespace frc971 {
 namespace control_loops {
 
+Eigen::Matrix<double, 2, 1> CoerceGoal(aos::controls::HPolytope<2> &region,
+                                       const Eigen::Matrix<double, 1, 2> &K,
+                                       double w,
+                                       const Eigen::Matrix<double, 2, 1> &R);
+
 class DrivetrainLoop
     : public aos::control_loops::ControlLoop<control_loops::Drivetrain, true, false> {
  public:
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index d15de44..43b6443 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -5,6 +5,8 @@
 #include "gtest/gtest.h"
 #include "aos/common/queue.h"
 #include "aos/common/queue_testutils.h"
+#include "aos/controls/polytope.h"
+
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "frc971/control_loops/drivetrain/drivetrain.h"
 #include "frc971/control_loops/state_feedback_loop.h"
@@ -18,6 +20,17 @@
 namespace control_loops {
 namespace testing {
 
+class Environment : public ::testing::Environment {
+ public:
+  virtual ~Environment() {}
+  // how to set up the environment.
+  virtual void SetUp() {
+    aos::controls::HPolytope<0>::Init();
+  }
+};
+::testing::Environment* const holder_env =
+  ::testing::AddGlobalTestEnvironment(new Environment);
+
 
 // Class which simulates the drivetrain and sends out queue messages containing the
 // position.
@@ -182,6 +195,108 @@
   VerifyNearGoal();
 }
 
+::aos::controls::HPolytope<2> MakeBox(double x1_min, double x1_max,
+                                      double x2_min, double x2_max) {
+  Eigen::Matrix<double, 4, 2> box_H;
+  box_H << /*[[*/ 1.0, 0.0 /*]*/,
+            /*[*/-1.0, 0.0 /*]*/,
+            /*[*/ 0.0, 1.0 /*]*/,
+            /*[*/ 0.0,-1.0 /*]]*/;
+  Eigen::Matrix<double, 4, 1> box_k;
+  box_k << /*[[*/ x1_max /*]*/,
+            /*[*/-x1_min /*]*/,
+            /*[*/ x2_max /*]*/,
+            /*[*/-x2_min /*]]*/;
+  ::aos::controls::HPolytope<2> t_poly(box_H, box_k);
+  return t_poly;
+}
+
+class CoerceGoalTest : public ::testing::Test {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+// WHOOOHH!
+TEST_F(CoerceGoalTest, Inside) {
+  ::aos::controls::HPolytope<2> box = MakeBox(1, 2, 1, 2);
+
+  Eigen::Matrix<double, 1, 2> K;
+  K << /*[[*/ 1, -1 /*]]*/;
+
+  Eigen::Matrix<double, 2, 1> R;
+  R << /*[[*/ 1.5, 1.5 /*]]*/;
+
+  Eigen::Matrix<double, 2, 1> output =
+      ::frc971::control_loops::CoerceGoal(box, K, 0, R);
+
+  EXPECT_EQ(R(0, 0), output(0, 0));
+  EXPECT_EQ(R(1, 0), output(1, 0));
+}
+
+TEST_F(CoerceGoalTest, Outside_Inside_Intersect) {
+  ::aos::controls::HPolytope<2> box = MakeBox(1, 2, 1, 2);
+
+  Eigen::Matrix<double, 1, 2> K;
+  K << 1, -1;
+
+  Eigen::Matrix<double, 2, 1> R;
+  R << 5, 5;
+
+  Eigen::Matrix<double, 2, 1> output =
+      ::frc971::control_loops::CoerceGoal(box, K, 0, R);
+
+  EXPECT_EQ(2.0, output(0, 0));
+  EXPECT_EQ(2.0, output(1, 0));
+}
+
+TEST_F(CoerceGoalTest, Outside_Inside_no_Intersect) {
+  ::aos::controls::HPolytope<2> box = MakeBox(3, 4, 1, 2);
+
+  Eigen::Matrix<double, 1, 2> K;
+  K << 1, -1;
+
+  Eigen::Matrix<double, 2, 1> R;
+  R << 5, 5;
+
+  Eigen::Matrix<double, 2, 1> output =
+      ::frc971::control_loops::CoerceGoal(box, K, 0, R);
+
+  EXPECT_EQ(3.0, output(0, 0));
+  EXPECT_EQ(2.0, output(1, 0));
+}
+
+TEST_F(CoerceGoalTest, Middle_Of_Edge) {
+  ::aos::controls::HPolytope<2> box = MakeBox(0, 4, 1, 2);
+
+  Eigen::Matrix<double, 1, 2> K;
+  K << -1, 1;
+
+  Eigen::Matrix<double, 2, 1> R;
+  R << 5, 5;
+
+  Eigen::Matrix<double, 2, 1> output =
+      ::frc971::control_loops::CoerceGoal(box, K, 0, R);
+
+  EXPECT_EQ(2.0, output(0, 0));
+  EXPECT_EQ(2.0, output(1, 0));
+}
+
+TEST_F(CoerceGoalTest, PerpendicularLine) {
+  ::aos::controls::HPolytope<2> box = MakeBox(1, 2, 1, 2);
+
+  Eigen::Matrix<double, 1, 2> K;
+  K << 1, 1;
+
+  Eigen::Matrix<double, 2, 1> R;
+  R << 5, 5;
+
+  Eigen::Matrix<double, 2, 1> output =
+      ::frc971::control_loops::CoerceGoal(box, K, 0, R);
+
+  EXPECT_EQ(1.0, output(0, 0));
+  EXPECT_EQ(1.0, output(1, 0));
+}
+
 }  // namespace testing
 }  // namespace control_loops
 }  // namespace frc971
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.cc b/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.cc
new file mode 100644
index 0000000..31a029f
--- /dev/null
+++ b/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.cc
@@ -0,0 +1,125 @@
+#include "frc971/control_loops/drivetrain/polydrivetrain_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowLowPlantCoefficients() {
+  Eigen::Matrix<double, 2, 2> A;
+  A << 0.899177606502, 0.000686937184856, 0.000686937184856, 0.899177606502;
+  Eigen::Matrix<double, 2, 2> B;
+  B << 0.0186835844877, -0.000127297602107, -0.000127297602107, 0.0186835844877;
+  Eigen::Matrix<double, 2, 2> C;
+  C << 1.0, 0.0, 0.0, 1.0;
+  Eigen::Matrix<double, 2, 2> D;
+  D << 0.0, 0.0, 0.0, 0.0;
+  Eigen::Matrix<double, 2, 1> U_max;
+  U_max << 12.0, 12.0;
+  Eigen::Matrix<double, 2, 1> U_min;
+  U_min << -12.0, -12.0;
+  return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowHighPlantCoefficients() {
+  Eigen::Matrix<double, 2, 2> A;
+  A << 0.89917740051, 0.000149762601927, 0.000716637450627, 0.978035563679;
+  Eigen::Matrix<double, 2, 2> B;
+  B << 0.0186836226605, -6.07092175404e-05, -0.00013280141337, 0.0089037164526;
+  Eigen::Matrix<double, 2, 2> C;
+  C << 1.0, 0.0, 0.0, 1.0;
+  Eigen::Matrix<double, 2, 2> D;
+  D << 0.0, 0.0, 0.0, 0.0;
+  Eigen::Matrix<double, 2, 1> U_max;
+  U_max << 12.0, 12.0;
+  Eigen::Matrix<double, 2, 1> U_min;
+  U_min << -12.0, -12.0;
+  return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighLowPlantCoefficients() {
+  Eigen::Matrix<double, 2, 2> A;
+  A << 0.978035563679, 0.000716637450627, 0.000149762601927, 0.89917740051;
+  Eigen::Matrix<double, 2, 2> B;
+  B << 0.0089037164526, -0.00013280141337, -6.07092175404e-05, 0.0186836226605;
+  Eigen::Matrix<double, 2, 2> C;
+  C << 1.0, 0.0, 0.0, 1.0;
+  Eigen::Matrix<double, 2, 2> D;
+  D << 0.0, 0.0, 0.0, 0.0;
+  Eigen::Matrix<double, 2, 1> U_max;
+  U_max << 12.0, 12.0;
+  Eigen::Matrix<double, 2, 1> U_min;
+  U_min << -12.0, -12.0;
+  return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighHighPlantCoefficients() {
+  Eigen::Matrix<double, 2, 2> A;
+  A << 0.978035518136, 0.000156145735499, 0.000156145735499, 0.978035518136;
+  Eigen::Matrix<double, 2, 2> B;
+  B << 0.0089037349145, -6.32967463335e-05, -6.32967463335e-05, 0.0089037349145;
+  Eigen::Matrix<double, 2, 2> C;
+  C << 1.0, 0.0, 0.0, 1.0;
+  Eigen::Matrix<double, 2, 2> D;
+  D << 0.0, 0.0, 0.0, 0.0;
+  Eigen::Matrix<double, 2, 1> U_max;
+  U_max << 12.0, 12.0;
+  Eigen::Matrix<double, 2, 1> U_min;
+  U_min << -12.0, -12.0;
+  return StateFeedbackPlantCoefficients<2, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowLowController() {
+  Eigen::Matrix<double, 2, 2> L;
+  L << 0.879177606502, 0.000686937184856, 0.000686937184856, 0.879177606502;
+  Eigen::Matrix<double, 2, 2> K;
+  K << 32.0714744818, 0.255280724401, 0.255280724401, 32.0714744818;
+  return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowLowPlantCoefficients());
+}
+
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController() {
+  Eigen::Matrix<double, 2, 2> L;
+  L << 0.879178111554, 0.000716636558747, 0.000716636558747, 0.958034852635;
+  Eigen::Matrix<double, 2, 2> K;
+  K << 32.0714744826, 0.255470827831, 0.558842435881, 76.1557821586;
+  return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowHighPlantCoefficients());
+}
+
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController() {
+  Eigen::Matrix<double, 2, 2> L;
+  L << 0.958040379369, 0.000149803514919, 0.000149803514919, 0.87917258482;
+  Eigen::Matrix<double, 2, 2> K;
+  K << 76.1557821586, 0.558842435881, 0.255470827831, 32.0714744826;
+  return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighLowPlantCoefficients());
+}
+
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController() {
+  Eigen::Matrix<double, 2, 2> L;
+  L << 0.958035518136, 0.000156145735499, 0.000156145735499, 0.958035518136;
+  Eigen::Matrix<double, 2, 2> K;
+  K << 76.1557821586, 0.558929371597, 0.558929371597, 76.1557821586;
+  return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighHighPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 2, 2> MakeVDrivetrainPlant() {
+  ::std::vector<StateFeedbackPlantCoefficients<2, 2, 2> *> plants(4);
+  plants[0] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowLowPlantCoefficients());
+  plants[1] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowHighPlantCoefficients());
+  plants[2] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighLowPlantCoefficients());
+  plants[3] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighHighPlantCoefficients());
+  return StateFeedbackPlant<2, 2, 2>(plants);
+}
+
+StateFeedbackLoop<2, 2, 2> MakeVDrivetrainLoop() {
+  ::std::vector<StateFeedbackController<2, 2, 2> *> controllers(4);
+  controllers[0] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowLowController());
+  controllers[1] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowHighController());
+  controllers[2] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighLowController());
+  controllers[3] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighHighController());
+  return StateFeedbackLoop<2, 2, 2>(controllers);
+}
+
+}  // namespace control_loops
+}  // namespace frc971
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.h b/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.h
new file mode 100644
index 0000000..99b113e
--- /dev/null
+++ b/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.h
@@ -0,0 +1,32 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowLowPlantCoefficients();
+
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowLowController();
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowHighPlantCoefficients();
+
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController();
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighLowPlantCoefficients();
+
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController();
+
+StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighHighPlantCoefficients();
+
+StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController();
+
+StateFeedbackPlant<2, 2, 2> MakeVDrivetrainPlant();
+
+StateFeedbackLoop<2, 2, 2> MakeVDrivetrainLoop();
+
+}  // namespace control_loops
+}  // namespace frc971
+
+#endif  // FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index 0e791cb..fe20228 100755
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -5,8 +5,51 @@
 import sys
 from matplotlib import pylab
 
-class Drivetrain(control_loop.ControlLoop):
+
+class CIM(control_loop.ControlLoop):
   def __init__(self):
+    super(CIM, self).__init__("CIM")
+    # Stall Torque in N m
+    self.stall_torque = 2.42
+    # 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 CIM in kg m^2
+    self.J = 0.0001
+    # 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
+    # Control loop time step
+    self.dt = 0.01
+
+    # State feedback matrices
+    self.A_continuous = numpy.matrix(
+        [[-self.Kt / self.Kv / (self.J * self.R)]])
+    self.B_continuous = numpy.matrix(
+        [[self.Kt / (self.J * self.R)]])
+    self.C = numpy.matrix([[1]])
+    self.D = numpy.matrix([[0]])
+
+    self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+                                               self.B_continuous, self.dt)
+
+    self.PlaceControllerPoles([0.01])
+
+    self.U_max = numpy.matrix([[12.0]])
+    self.U_min = numpy.matrix([[-12.0]])
+
+    self.InitializeState()
+
+
+class Drivetrain(control_loop.ControlLoop):
+  def __init__(self, left_low=True, right_low=True):
     super(Drivetrain, self).__init__("Drivetrain")
     # Stall Torque in N m
     self.stall_torque = 2.42
@@ -26,7 +69,7 @@
     # 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
+    self.R = (12.0 / self.stall_current / 4 + 0.03) / (0.93 ** 2.0)
     # Motor velocity constant
     self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
                (12.0 - self.R * self.free_current))
@@ -35,7 +78,14 @@
     # 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
+    if left_low:
+      self.Gl = self.G_low
+    else:
+      self.Gl = self.G_high
+    if right_low:
+      self.Gr = self.G_low
+    else:
+      self.Gr = self.G_high
     # Control loop time step
     self.dt = 0.01
 
@@ -44,22 +94,24 @@
     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)
+    self.tcl = -self.Kt / self.Kv / (self.Gl * self.Gl * self.R * self.r * self.r)
+    self.tcr = -self.Kt / self.Kv / (self.Gr * self.Gr * self.R * self.r * self.r)
+    self.mpl = self.Kt / (self.Gl * self.R * self.r)
+    self.mpr = self.Kt / (self.Gr * self.R * self.r)
 
     # State feedback matrices
     # X will be of the format
-    # [[position1], [velocity1], [position2], velocity2]]
+    # [[positionl], [velocityl], [positionr], velocityr]]
     self.A_continuous = numpy.matrix(
         [[0, 1, 0, 0],
-         [0, self.msp * self.tc, 0, self.msn * self.tc],
+         [0, self.msp * self.tcl, 0, self.msn * self.tcr],
          [0, 0, 0, 1],
-         [0, self.msn * self.tc, 0, self.msp * self.tc]])
+         [0, self.msn * self.tcl, 0, self.msp * self.tcr]])
     self.B_continuous = numpy.matrix(
         [[0, 0],
-         [self.msp * self.mp, self.msn * self.mp],
+         [self.msp * self.mpl, self.msn * self.mpr],
          [0, 0],
-         [self.msn * self.mp, self.msp * self.mp]])
+         [self.msn * self.mpl, self.msp * self.mpr]])
     self.C = numpy.matrix([[1, 0, 0, 0],
                            [0, 0, 1, 0]])
     self.D = numpy.matrix([[0, 0],
@@ -73,8 +125,6 @@
     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])
diff --git a/frc971/control_loops/python/libcdd.py b/frc971/control_loops/python/libcdd.py
index a217728..6305aaf 100644
--- a/frc971/control_loops/python/libcdd.py
+++ b/frc971/control_loops/python/libcdd.py
@@ -119,7 +119,7 @@
 
   # Return None on error.
   # The error values are enums, so they aren't exposed.
-  if error.value != NO_ERRORS:
+  if error.value != DD_NO_ERRORS:
     # Dump out the errors to stderr
     libcdd._Z21dd_WriteErrorMessagesP8_IO_FILE12dd_ErrorType(
         ctypes.pythonapi.PyFile_AsFile(ctypes.py_object(sys.stdout)),
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
new file mode 100755
index 0000000..9fa7841
--- /dev/null
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -0,0 +1,405 @@
+#!/usr/bin/python
+
+import numpy
+import sys
+import polytope
+import drivetrain
+import control_loop
+import controls
+from matplotlib import pylab
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+
+def CoerceGoal(region, K, w, R):
+  """Intersects a line with a region, and finds the closest point to R.
+
+  Finds a point that is closest to R inside the region, and on the line
+  defined by K X = w.  If it is not possible to find a point on the line,
+  finds a point that is inside the region and closest to the line.  This
+  function assumes that
+
+  Args:
+    region: HPolytope, the valid goal region.
+    K: numpy.matrix (2 x 1), the matrix for the equation [K1, K2] [x1; x2] = w
+    w: float, the offset in the equation above.
+    R: numpy.matrix (2 x 1), the point to be closest to.
+
+  Returns:
+    numpy.matrix (2 x 1), the point.
+  """
+
+  if region.IsInside(R):
+    return R
+
+  perpendicular_vector = K.T / numpy.linalg.norm(K)
+  parallel_vector = numpy.matrix([[perpendicular_vector[1, 0]],
+                                  [-perpendicular_vector[0, 0]]])
+  
+  # We want to impose the constraint K * X = w on the polytope H * X <= k.
+  # We do this by breaking X up into parallel and perpendicular components to
+  # the half plane.  This gives us the following equation.
+  #
+  #  parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) = X
+  #
+  # Then, substitute this into the polytope.
+  #
+  #  H * (parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) <= k
+  #
+  # Substitute K * X = w
+  #
+  # H * parallel * (parallel.T \dot X) + H * perpendicular * w <= k
+  #
+  # Move all the knowns to the right side.
+  #
+  # H * parallel * ([parallel1 parallel2] * X) <= k - H * perpendicular * w
+  #
+  # Let t = parallel.T \dot X, the component parallel to the surface.
+  #
+  # H * parallel * t <= k - H * perpendicular * w
+  #
+  # This is a polytope which we can solve, and use to figure out the range of X
+  # that we care about!
+
+  t_poly = polytope.HPolytope(
+      region.H * parallel_vector,
+      region.k - region.H * perpendicular_vector * w)
+
+  vertices = t_poly.Vertices()
+
+  if vertices.shape[0]:
+    # The region exists!
+    # Find the closest vertex
+    min_distance = numpy.infty
+    closest_point = None
+    for vertex in vertices:
+      point = parallel_vector * vertex + perpendicular_vector * w
+      length = numpy.linalg.norm(R - point)
+      if length < min_distance:
+        min_distance = length
+        closest_point = point
+
+    return closest_point
+  else:
+    # Find the vertex of the space that is closest to the line.
+    region_vertices = region.Vertices()
+    min_distance = numpy.infty
+    closest_point = None
+    for vertex in region_vertices:
+      point = vertex.T
+      length = numpy.abs((perpendicular_vector.T * point)[0, 0])
+      if length < min_distance:
+        min_distance = length
+        closest_point = point
+
+    return closest_point
+
+
+class VelocityDrivetrainModel(control_loop.ControlLoop):
+  def __init__(self, left_low=True, right_low=True, name="VelocityDrivetrainModel"):
+    super(VelocityDrivetrainModel, self).__init__(name)
+    self._drivetrain = drivetrain.Drivetrain(left_low=left_low,
+                                             right_low=right_low)
+    self.dt = 0.01
+    self.A_continuous = numpy.matrix(
+        [[self._drivetrain.A_continuous[1, 1], self._drivetrain.A_continuous[1, 3]],
+         [self._drivetrain.A_continuous[3, 1], self._drivetrain.A_continuous[3, 3]]])
+
+    self.B_continuous = numpy.matrix(
+        [[self._drivetrain.B_continuous[1, 0], self._drivetrain.B_continuous[1, 1]],
+         [self._drivetrain.B_continuous[3, 0], self._drivetrain.B_continuous[3, 1]]])
+    self.C = numpy.matrix(numpy.eye(2));
+    self.D = numpy.matrix(numpy.zeros((2, 2)));
+
+    self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+                                               self.B_continuous, self.dt)
+
+    # FF * X = U (steady state)
+    self.FF = self.B.I * (numpy.eye(2) - self.A)
+
+    self.PlaceControllerPoles([0.3, 0.3])
+    self.PlaceObserverPoles([0.02, 0.02])
+
+    self.G_high = self._drivetrain.G_high
+    self.G_low = self._drivetrain.G_low
+    self.R = self._drivetrain.R
+    self.r = self._drivetrain.r
+    self.Kv = self._drivetrain.Kv
+    self.Kt = self._drivetrain.Kt
+
+    self.U_max = self._drivetrain.U_max
+    self.U_min = self._drivetrain.U_min
+
+
+class VelocityDrivetrain(object):
+  def __init__(self):
+    self.drivetrain_low_low = VelocityDrivetrainModel(left_low=True, right_low=True, name='VelocityDrivetrainLowLow')
+    self.drivetrain_low_high = VelocityDrivetrainModel(left_low=True, right_low=False, name='VelocityDrivetrainLowHigh')
+    self.drivetrain_high_low = VelocityDrivetrainModel(left_low=False, right_low=True, name = 'VelocityDrivetrainHighLow')
+    self.drivetrain_high_high = VelocityDrivetrainModel(left_low=False, right_low=False, name = 'VelocityDrivetrainHighHigh')
+
+    # X is [lvel, rvel]
+    self.X = numpy.matrix(
+        [[0.0],
+         [0.0]])
+
+    self.U_poly = polytope.HPolytope(
+        numpy.matrix([[1, 0],
+                      [-1, 0],
+                      [0, 1],
+                      [0, -1]]),
+        numpy.matrix([[12],
+                      [12],
+                      [12],
+                      [12]]))
+
+    self.U_max = numpy.matrix(
+        [[12.0],
+         [12.0]])
+    self.U_min = numpy.matrix(
+        [[-12.0000000000],
+         [-12.0000000000]])
+
+    self.dt = 0.01
+
+    self.R = numpy.matrix(
+        [[0.0],
+         [0.0]])
+
+    # ttrust is the comprimise between having full throttle negative inertia,
+    # and having no throttle negative inertia.  A value of 0 is full throttle
+    # inertia.  A value of 1 is no throttle negative inertia.
+    self.ttrust = 1.0
+
+    self.left_high = False
+    self.right_high = False
+
+  def CurrentDrivetrain(self):
+    if self.left_high:
+      if self.right_high:
+        return self.drivetrain_high_high
+      else:
+        return self.drivetrain_high_low
+    else:
+      if self.right_high:
+        return self.drivetrain_low_high
+      else:
+        return self.drivetrain_low_low
+
+  def ComputeGear(self, wheel_velocity, should_print=False, current_gear=False, gear_name=None):
+    high_omega = (wheel_velocity / self.CurrentDrivetrain().G_high /
+                  self.CurrentDrivetrain().r)
+    low_omega = (wheel_velocity / self.CurrentDrivetrain().G_low /
+                 self.CurrentDrivetrain().r)
+    print gear_name, "Motor Energy Difference.", 0.5 * 0.000140032647 * (low_omega * low_omega - high_omega * high_omega), "joules"
+    high_torque = ((12.0 - high_omega / self.CurrentDrivetrain().Kv) *
+                   self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().R)
+    low_torque = ((12.0 - low_omega / self.CurrentDrivetrain().Kv) *
+                  self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().R)
+    high_power = high_torque * high_omega
+    low_power = low_torque * low_omega
+    if should_print:
+      print gear_name, "High omega", high_omega, "Low omega", low_omega
+      print gear_name, "High torque", high_torque, "Low torque", low_torque
+      print gear_name, "High power", high_power, "Low power", low_power
+      if (high_power > low_power) != current_gear:
+        if high_power > low_power:
+          print gear_name, "Shifting to high"
+        else:
+          print gear_name, "Shifting to low"
+
+    # Shift algorithm improvements.
+    # TODO(aschuh):
+    # It takes time to shift.  Shifting down for 1 cycle doesn't make sense
+    # because you will end up slower than without shifting.  Figure out how
+    # to include that info.
+    # If the driver is still in high gear, but isn't asking for the extra power
+    # from low gear, don't shift until he asks for it.
+    return high_power > low_power
+
+  def FilterVelocity(self, throttle):
+    # Invert the plant to figure out how the velocity filter would have to work
+    # out in order to filter out the forwards negative inertia.
+    # This math assumes that the left and right power and velocity are equal.
+
+    # The throttle filter should filter such that the motor in the highest gear
+    # should be controlling the time constant.
+    # Do this by finding the index of FF that has the lowest value, and computing
+    # the sums using that index.
+    FF_sum = self.CurrentDrivetrain().FF.sum(axis=1)
+    min_FF_sum_index = numpy.argmin(FF_sum)
+    min_FF_sum = FF_sum[min_FF_sum_index, 0]
+    min_K_sum = self.CurrentDrivetrain().K[min_FF_sum_index, :].sum()
+    # Compute the FF sum for high gear.
+    high_min_FF_sum = self.drivetrain_high_high.FF[0, :].sum()
+
+    # U = self.K[0, :].sum() * (R - x_avg) + self.FF[0, :].sum() * R
+    # throttle * 12.0 = (self.K[0, :].sum() + self.FF[0, :].sum()) * R
+    #                   - self.K[0, :].sum() * x_avg
+
+    # R = (throttle * 12.0 + self.K[0, :].sum() * x_avg) /
+    #     (self.K[0, :].sum() + self.FF[0, :].sum())
+
+    # U = (K + FF) * R - K * X
+    # (K + FF) ^-1 * (U + K * X) = R
+
+    # Scale throttle by min_FF_sum / high_min_FF_sum.  This will make low gear
+    # have the same velocity goal as high gear, and so that the robot will hold
+    # the same speed for the same throttle for all gears.
+    adjusted_ff_voltage = numpy.clip(throttle * 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0)
+    return ((adjusted_ff_voltage + self.ttrust * min_K_sum * (self.X[0, 0] + self.X[1, 0]) / 2.0)
+            / (self.ttrust * min_K_sum + min_FF_sum))
+
+  def Update(self, throttle, steering):
+    # Shift into the gear which sends the most power to the floor.
+    # This is the same as sending the most torque down to the floor at the
+    # wheel.
+
+    self.left_high = self.ComputeGear(self.X[0, 0], should_print=True, current_gear=self.left_high, gear_name="left")
+    self.right_high = self.ComputeGear(self.X[1, 0], should_print=True, current_gear=self.right_high, gear_name="right")
+
+    FF_sum = self.CurrentDrivetrain().FF.sum(axis=1)
+
+    # Filter the throttle to provide a nicer response.
+
+    # TODO(austin): fn
+    fvel = self.FilterVelocity(throttle)
+
+    # Constant radius means that angualar_velocity / linear_velocity = constant.
+    # Compute the left and right velocities.
+    left_velocity = fvel - steering * numpy.abs(fvel)
+    right_velocity = fvel + steering * numpy.abs(fvel)
+
+    # Write this constraint in the form of K * R = w
+    # angular velocity / linear velocity = constant
+    # (left - right) / (left + right) = constant
+    # left - right = constant * left + constant * right
+
+    # (fvel - steering * numpy.abs(fvel) - fvel - steering * numpy.abs(fvel)) /
+    #  (fvel - steering * numpy.abs(fvel) + fvel + steering * numpy.abs(fvel)) =
+    #       constant
+    # (- 2 * steering * numpy.abs(fvel)) / (2 * fvel) = constant
+    # (-steering * sign(fvel)) = constant
+    # (-steering * sign(fvel)) * (left + right) = left - right
+    # (steering * sign(fvel) + 1) * left + (steering * sign(fvel) - 1) * right = 0
+
+    equality_k = numpy.matrix(
+        [[1 + steering * numpy.sign(fvel), -(1 - steering * numpy.sign(fvel))]])
+    equality_w = 0.0
+
+    self.R[0, 0] = left_velocity
+    self.R[1, 0] = right_velocity
+
+    # Construct a constraint on R by manipulating the constraint on U
+    # Start out with H * U <= k
+    # U = FF * R + K * (R - X)
+    # H * (FF * R + K * R - K * X) <= k
+    # H * (FF + K) * R <= k + H * K * X
+    R_poly = polytope.HPolytope(
+        self.U_poly.H * (self.CurrentDrivetrain().K + self.CurrentDrivetrain().FF),
+        self.U_poly.k + self.U_poly.H * self.CurrentDrivetrain().K * self.X)
+
+    # Limit R back inside the box.
+    self.boxed_R = CoerceGoal(R_poly, equality_k, equality_w, self.R)
+
+    FF_volts = self.CurrentDrivetrain().FF * self.boxed_R
+    self.U_ideal = self.CurrentDrivetrain().K * (self.boxed_R - self.X) + FF_volts
+
+    self.U = numpy.clip(self.U_ideal, self.U_min, self.U_max)
+    self.X = self.CurrentDrivetrain().A * self.X + self.CurrentDrivetrain().B * self.U
+    print "U is", self.U[0, 0], self.U[1, 0]
+
+
+def main(argv):
+  vdrivetrain = VelocityDrivetrain()
+
+  if len(argv) != 3:
+    print "Expected .h file name and .cc file name"
+  else:
+    loop_writer = control_loop.ControlLoopWriter(
+        "VDrivetrain", [vdrivetrain.drivetrain_low_low,
+                        vdrivetrain.drivetrain_low_high,
+                        vdrivetrain.drivetrain_high_low,
+                        vdrivetrain.drivetrain_high_high])
+
+    if argv[1][-3:] == '.cc':
+      loop_writer.Write(argv[2], argv[1])
+    else:
+      loop_writer.Write(argv[1], argv[2])
+    return
+
+
+  vl_plot = []
+  vr_plot = []
+  ul_plot = []
+  ur_plot = []
+  radius_plot = []
+  t_plot = []
+  left_gear_plot = []
+  right_gear_plot = []
+  vdrivetrain.left_high = True
+  vdrivetrain.right_high = True
+
+  print "K is", vdrivetrain.CurrentDrivetrain().K
+
+  if vdrivetrain.left_high:
+    print "Left is high"
+  else:
+    print "Left is low"
+  if vdrivetrain.right_high:
+    print "Right is high"
+  else:
+    print "Right is low"
+
+  for t in numpy.arange(0, 2.0, vdrivetrain.dt):
+    if t < 1.0:
+      vdrivetrain.Update(throttle=0.60, steering=0.3)
+    elif t < 1.5:
+      vdrivetrain.Update(throttle=0.60, steering=-0.3)
+    else:
+      vdrivetrain.Update(throttle=0.0, steering=0.3)
+    t_plot.append(t)
+    vl_plot.append(vdrivetrain.X[0, 0])
+    vr_plot.append(vdrivetrain.X[1, 0])
+    ul_plot.append(vdrivetrain.U[0, 0])
+    ur_plot.append(vdrivetrain.U[1, 0])
+    left_gear_plot.append(vdrivetrain.left_high * 2.0 - 10.0)
+    right_gear_plot.append(vdrivetrain.right_high * 2.0 - 10.0)
+
+    fwd_velocity = (vdrivetrain.X[1, 0] + vdrivetrain.X[0, 0]) / 2
+    turn_velocity = (vdrivetrain.X[1, 0] - vdrivetrain.X[0, 0])
+    if fwd_velocity < 0.0000001:
+      radius_plot.append(turn_velocity)
+    else:
+      radius_plot.append(turn_velocity / fwd_velocity)
+
+  cim_velocity_plot = []
+  cim_voltage_plot = []
+  cim_time = []
+  cim = drivetrain.CIM()
+  R = numpy.matrix([[300]])
+  for t in numpy.arange(0, 0.5, cim.dt):
+    U = numpy.clip(cim.K * (R - cim.X) + R / cim.Kv, cim.U_min, cim.U_max)
+    cim.Update(U)
+    cim_velocity_plot.append(cim.X[0, 0])
+    cim_voltage_plot.append(U[0, 0] * 10)
+    cim_time.append(t)
+  pylab.plot(cim_time, cim_velocity_plot, label='cim spinup')
+  pylab.plot(cim_time, cim_voltage_plot, label='cim voltage')
+  pylab.legend()
+  pylab.show()
+
+
+  pylab.plot(t_plot, vl_plot, label='left velocity')
+  pylab.plot(t_plot, vr_plot, label='right velocity')
+  pylab.plot(t_plot, ul_plot, label='left voltage')
+  pylab.plot(t_plot, ur_plot, label='right voltage')
+  pylab.plot(t_plot, radius_plot, label='radius')
+  pylab.plot(t_plot, left_gear_plot, label='left gear high')
+  pylab.plot(t_plot, right_gear_plot, label='right gear high')
+  pylab.legend()
+  pylab.show()
+  return 0
+
+if __name__ == '__main__':
+  sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/python/polydrivetrain_test.py b/frc971/control_loops/python/polydrivetrain_test.py
new file mode 100755
index 0000000..434cdca
--- /dev/null
+++ b/frc971/control_loops/python/polydrivetrain_test.py
@@ -0,0 +1,82 @@
+#!/usr/bin/python
+
+import polydrivetrain
+import numpy
+from numpy.testing import *
+import polytope
+import unittest
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+
+class TestVelocityDrivetrain(unittest.TestCase):
+  def MakeBox(self, x1_min, x1_max, x2_min, x2_max):
+    H = numpy.matrix([[1, 0],
+                      [-1, 0],
+                      [0, 1],
+                      [0, -1]])
+    K = numpy.matrix([[x1_max],
+                      [-x1_min],
+                      [x2_max],
+                      [-x2_min]])
+    return polytope.HPolytope(H, K)
+
+  def test_coerce_inside(self):
+    """Tests coercion when the point is inside the box."""
+    box = self.MakeBox(1, 2, 1, 2)
+
+    # x1 = x2
+    K = numpy.matrix([[1, -1]])
+    w = 0
+
+    assert_array_equal(polydrivetrain.CoerceGoal(box, K, w,
+                                                 numpy.matrix([[1.5], [1.5]])),
+                       numpy.matrix([[1.5], [1.5]]))
+
+  def test_coerce_outside_intersect(self):
+    """Tests coercion when the line intersects the box."""
+    box = self.MakeBox(1, 2, 1, 2)
+
+    # x1 = x2
+    K = numpy.matrix([[1, -1]])
+    w = 0
+
+    assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+                       numpy.matrix([[2.0], [2.0]]))
+
+  def test_coerce_outside_no_intersect(self):
+    """Tests coercion when the line does not intersect the box."""
+    box = self.MakeBox(3, 4, 1, 2)
+
+    # x1 = x2
+    K = numpy.matrix([[1, -1]])
+    w = 0
+
+    assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+                       numpy.matrix([[3.0], [2.0]]))
+
+  def test_coerce_middle_of_edge(self):
+    """Tests coercion when the line intersects the middle of an edge."""
+    box = self.MakeBox(0, 4, 1, 2)
+
+    # x1 = x2
+    K = numpy.matrix([[-1, 1]])
+    w = 0
+
+    assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+                       numpy.matrix([[2.0], [2.0]]))
+
+  def test_coerce_perpendicular_line(self):
+    """Tests coercion when the line does not intersect and is in quadrant 2."""
+    box = self.MakeBox(1, 2, 1, 2)
+
+    # x1 = -x2
+    K = numpy.matrix([[1, 1]])
+    w = 0
+
+    assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+                       numpy.matrix([[1.0], [1.0]]))
+
+
+if __name__ == '__main__':
+  unittest.main()
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index ff5e9d8..4c35419 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -223,15 +223,18 @@
   Eigen::Matrix<double, number_of_outputs, 1> U_ff;
   Eigen::Matrix<double, number_of_outputs, 1> Y;
 
-  ::std::vector<StateFeedbackController<number_of_states, number_of_inputs,
-                                        number_of_outputs> *> controllers_;
-
   const StateFeedbackController<
       number_of_states, number_of_inputs, number_of_outputs>
           &controller() const {
     return *controllers_[controller_index_];
   }
 
+  const StateFeedbackController<
+      number_of_states, number_of_inputs, number_of_outputs>
+          &controller(int index) const {
+    return *controllers_[index];
+  }
+
   void Reset() {
     X_hat.setZero();
     R.setZero();
@@ -324,6 +327,9 @@
   void controller_index() const { return controller_index_; }
 
  protected:
+  ::std::vector<StateFeedbackController<number_of_states, number_of_inputs,
+                                        number_of_outputs> *> controllers_;
+
   // these are accessible from non-templated subclasses
   static const int kNumStates = number_of_states;
   static const int kNumOutputs = number_of_outputs;
diff --git a/frc971/control_loops/update_polydrivetrain.sh b/frc971/control_loops/update_polydrivetrain.sh
new file mode 100755
index 0000000..71e5d09
--- /dev/null
+++ b/frc971/control_loops/update_polydrivetrain.sh
@@ -0,0 +1,6 @@
+#!/bin/bash
+#
+# Updates the drivetrain controller.
+
+./python/polydrivetrain.py drivetrain/polydrivetrain_motor_plant.h \
+    drivetrain/polydrivetrain_motor_plant.cc