diff --git a/frc971/control_loops/DriveTrain.cc b/frc971/control_loops/DriveTrain.cc
new file mode 100644
index 0000000..b627cc0
--- /dev/null
+++ b/frc971/control_loops/DriveTrain.cc
@@ -0,0 +1,298 @@
+#include "frc971/control_loops/DriveTrain.h"
+
+#include <stdio.h>
+#include <sched.h>
+#include <cmath>
+
+#include "aos/aos_core.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/queue.h"
+#include "frc971/control_loops/DriveTrain.mat"
+#include "frc971/control_loops/DriveTrain.q.h"
+#include "frc971/queues/GyroAngle.q.h"
+#include "frc971/queues/Piston.q.h"
+
+using frc971::sensors::gyro;
+
+namespace frc971 {
+namespace control_loops {
+
+// Width of the robot.
+const double width = 22.0 / 100.0 * 2.54;
+
+class DrivetrainMotorsSS : public MatrixClass {
+ public:
+  DrivetrainMotorsSS (void) {
+    MATRIX_INIT;
+    _offset = 0;
+    _integral_offset = 0;
+    _left_goal = 0.0;
+    _right_goal = 0.0;
+    _raw_left = 0.0;
+    _raw_right = 0.0;
+  }
+  void SetGoal(double left, double left_velocity, double right, double right_velocity) {
+    _left_goal = left;
+    _right_goal = right;
+    R << left + _integral_offset * width / 2.0, left_velocity, right - _integral_offset * width / 2.0, right_velocity;
+  }
+  void SetRawPosition(double left, double right) {
+    _raw_right = right;
+    _raw_left = left;
+    Y << left + _offset + _integral_offset, right - _offset + _integral_offset;
+  }
+  void SetPosition(double left, double right, double gyro, bool control_loop_driving) {
+    // Decay the offset quickly because this gyro is great.
+    _offset = (0.25) * (right - left - gyro * width) / 2.0 + 0.75 * _offset;
+    const double angle_error = (_right_goal - _left_goal) / width - (_raw_right - _offset - _raw_left - _offset) / width;
+    if (!control_loop_driving) {
+      _integral_offset = 0.0;
+    } else if (std::abs(angle_error) < M_PI / 10.0) {
+      _integral_offset -= angle_error * 0.010;
+    } else {
+      _integral_offset *= 0.97;
+    }
+    _gyro = gyro;
+    SetRawPosition(left, right);
+    LOG(DEBUG, "Left %f->%f Right %f->%f Gyro %f aerror %f ioff %f\n", left + _offset, _left_goal, right - _offset, _right_goal, gyro, angle_error, _integral_offset);
+  }
+  double UnDeadband(double value) {
+    const double positive_deadband_power = 0.15 * 12;
+    const double negative_deadband_power = 0.09 * 12;
+    if (value > 0) {
+      value += positive_deadband_power;
+    }
+    if (value < 0) {
+      value -= negative_deadband_power;
+    }
+    if (value > 12.0) {
+      value = 12.0;
+    }
+    if (value < -12.0) {
+      value = -12.0;
+    }
+    return value;
+  }
+
+  void SendMotors(Drivetrain::Output *status) {
+    status->left_voltage = UnDeadband(U[0]);
+    status->right_voltage = UnDeadband(U[1]);
+  }
+  void PrintMotors() const {
+    // LOG(DEBUG, "Left Power %f Right Power %f lg %f rg %f le %f re %f gyro %f\n", U[0], U[1], R[0], R[2], Y[0], Y[1], _gyro);
+    LOG(DEBUG, "lg %f rg %f le %f re %f gyro %f off %f\n", R[0], R[2], Y[0], Y[1], _gyro * 180.0 / M_PI, _offset);
+  }
+
+ private:
+  double _integral_offset;
+  double _offset;
+  double _gyro;
+  double _left_goal;
+  double _right_goal;
+  double _raw_left;
+  double _raw_right;
+};
+
+class DrivetrainMotorsOL {
+ public:
+  DrivetrainMotorsOL() {
+    _old_wheel = 0.0;
+    quick_stop_accumulator = 0.0;
+    _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;
+    _left_pwm = 0.0;
+    _right_pwm = 0.0;
+  }
+  void Update(void) {
+    double overPower;
+    float sensitivity = 1.7;
+    float angular_power;
+    float linear_power;
+    double wheel;
+
+    double neg_inertia = _wheel - _old_wheel;
+    _old_wheel = _wheel;
+
+    double wheelNonLinearity;
+    if (_highgear) {
+      wheelNonLinearity = 0.7;  // 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);
+    } else {
+      wheelNonLinearity = 0.4;  // 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);
+    }
+
+    double neg_inertia_scalar;
+    if (_highgear) {
+      neg_inertia_scalar = 20.0;  // used to be csvReader->NEG_INTERTIA_HIGH
+      sensitivity = 1.22; // used to be csvReader->SENSE_HIGH
+    } else {
+      if (wheel * neg_inertia > 0) {
+        neg_inertia_scalar = 16;  // used to be csvReader->NEG_INERTIA_LOW_MORE
+      } else {
+        if (fabs(wheel) > 0.65) {
+          neg_inertia_scalar = 16;  // used to be csvReader->NEG_INTERTIA_LOW_LESS_EXT
+        } else {
+          neg_inertia_scalar = 5;  // used to be csvReader->NEG_INTERTIA_LOW_LESS
+        }
+      }
+      sensitivity = 1.24;  // used to be csvReader->SENSE_LOW
+
+      if (fabs(_throttle) > 0.1) {  // used to be csvReader->SENSE_CUTTOFF
+        sensitivity = 1 - (1 - sensitivity) / fabs(_throttle);
+      }
+    }
+    double neg_inertia_power = neg_inertia * neg_inertia_scalar;
+    _neg_inertia_accumulator += neg_inertia_power;
+
+    wheel = wheel + _neg_inertia_accumulator;
+    if (_neg_inertia_accumulator > 1) {
+      _neg_inertia_accumulator -= 1;
+    } else if (_neg_inertia_accumulator < -1) {
+      _neg_inertia_accumulator += 1;
+    } else {
+      _neg_inertia_accumulator = 0;
+    }
+
+    linear_power = _throttle;
+
+    const double quickstop_scalar = 6;
+    if (_quickturn) {
+      double qt_angular_power = wheel;
+      const double alpha = 0.1;
+      if (fabs(linear_power) < 0.2) {
+        if (qt_angular_power > 1) qt_angular_power = 1.0;
+        if (qt_angular_power < -1) qt_angular_power = -1.0;
+      } else {
+        qt_angular_power = 0.0;
+      }
+      quick_stop_accumulator = (1 - alpha) * quick_stop_accumulator + alpha * qt_angular_power * quickstop_scalar;
+      overPower = 1.0;
+      if (_highgear) {
+        sensitivity = 1.0;
+      } else {
+        sensitivity = 1.0;
+      }
+      angular_power = wheel;
+    } else {
+      overPower = 0.0;
+      angular_power = fabs(_throttle) * wheel * sensitivity;
+      angular_power -= quick_stop_accumulator;
+      if (quick_stop_accumulator > 1) {
+        quick_stop_accumulator -= 1;
+      } else if (quick_stop_accumulator < -1) {
+        quick_stop_accumulator += 1;
+      } else {
+        quick_stop_accumulator = 0;
+      }
+    }
+
+    _right_pwm = _left_pwm = linear_power;
+    _left_pwm += angular_power;
+    _right_pwm -= angular_power;
+
+    if (_left_pwm > 1.0) {
+      _right_pwm -= overPower*(_left_pwm - 1.0);
+      _left_pwm = 1.0;
+    } else if (_right_pwm > 1.0) {
+      _left_pwm -= overPower*(_right_pwm - 1.0);
+      _right_pwm = 1.0;
+    } else if (_left_pwm < -1.0) {
+      _right_pwm += overPower*(-1.0 - _left_pwm);
+      _left_pwm = -1.0;
+    } else if (_right_pwm < -1.0) {
+      _left_pwm += overPower*(-1.0 - _right_pwm);
+      _right_pwm = -1.0;
+    }
+  }
+
+  void SendMotors(Drivetrain::Output *output) {
+    LOG(DEBUG, "left pwm: %f right pwm: %f wheel: %f throttle: %f, qa %f\n",
+        _left_pwm, _right_pwm, _wheel, _throttle, quick_stop_accumulator);
+    output->left_voltage = _left_pwm * 12.0;
+    output->right_voltage = _right_pwm * 12.0;
+    if (_highgear) {
+      shifters.MakeWithBuilder().set(false).Send();
+    } else {
+      shifters.MakeWithBuilder().set(true).Send();
+    }
+  }
+
+ private:
+  double _old_wheel;
+  double _wheel;
+  double _throttle;
+  bool _quickturn;
+  bool _highgear;
+  double _neg_inertia_accumulator;
+  double _left_pwm;
+  double _right_pwm;
+  double quick_stop_accumulator;
+};
+
+void DrivetrainLoop::RunIteration(const Drivetrain::Goal *goal,
+                                  const Drivetrain::Position *position,
+                                  Drivetrain::Output *output,
+                                  Drivetrain::Status * /*status*/) {
+  // TODO(aschuh): These should be members of the class.
+  static DrivetrainMotorsSS dt_closedloop;
+  static DrivetrainMotorsOL dt_openloop;
+
+  bool bad_pos = false;
+  if (position == NULL) {
+    LOG(WARNING, "no pos\n");
+    bad_pos = true;
+  }
+
+  double wheel = goal->steering;
+  double throttle = goal->throttle;
+  bool quickturn = goal->quickturn;
+  bool highgear = goal->highgear;
+
+  bool control_loop_driving = goal->control_loop_driving;
+  double left_goal = goal->left_goal;
+  double right_goal = goal->right_goal;
+
+  dt_closedloop.SetGoal(left_goal, 0.0, right_goal, 0.0);
+  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);
+    } else {
+      dt_closedloop.SetRawPosition(left_encoder, right_encoder);
+    }
+  }
+  dt_closedloop.Update(!bad_pos, bad_pos || (output == NULL));
+  dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
+  dt_openloop.Update();
+  if (control_loop_driving) {
+    dt_closedloop.SendMotors(output);
+  } else {
+    dt_openloop.SendMotors(output);
+  }
+}
+
+}  // namespace control_loops
+}  // namespace frc971
+
+AOS_RUN_LOOP(frc971::control_loops::DrivetrainLoop)
diff --git a/frc971/control_loops/DriveTrain.h b/frc971/control_loops/DriveTrain.h
new file mode 100644
index 0000000..61ea524
--- /dev/null
+++ b/frc971/control_loops/DriveTrain.h
@@ -0,0 +1,32 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_H_
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "frc971/control_loops/DriveTrain.q.h"
+
+namespace frc971 {
+namespace control_loops {
+
+class DrivetrainLoop
+    : public aos::control_loops::ControlLoop<control_loops::Drivetrain> {
+ public:
+  // Constructs a control loop which can take a Drivetrain or defaults to the
+  // drivetrain at frc971::control_loops::drivetrain
+  explicit DrivetrainLoop(
+      control_loops::Drivetrain *my_drivetrain = &control_loops::drivetrain)
+      : aos::control_loops::ControlLoop<control_loops::Drivetrain>(
+          my_drivetrain) {}
+
+ protected:
+  // Executes one cycle of the control loop.
+  virtual void RunIteration(
+      const control_loops::Drivetrain::Goal *goal,
+      const control_loops::Drivetrain::Position *position,
+      control_loops::Drivetrain::Output *output,
+      control_loops::Drivetrain::Status *status);
+};
+
+}  // namespace control_loops
+}  // namespace frc971
+
+#endif  // FRC971_CONTROL_LOOPS_DRIVETRAIN_H_
diff --git a/frc971/control_loops/DriveTrain.mat b/frc971/control_loops/DriveTrain.mat
new file mode 100644
index 0000000..fcc6f45
--- /dev/null
+++ b/frc971/control_loops/DriveTrain.mat
@@ -0,0 +1,12 @@
+#include "frc971/control_loops/StateFeedbackLoop.h"
+
+typedef StateFeedbackLoop<4, 2> MatrixClass;
+#define MATRIX_INIT A << 1.0000000000, 0.0095410093, 0.0000000000, -0.0000167223, 0.0000000000, 0.9096302600, 0.0000000000, -0.0032396985, 0.0000000000, -0.0000167223, 1.0000000000, 0.0095410093, 0.0000000000, -0.0032396985, 0.0000000000, 0.9096302600; \
+B << 0.0000628338, 0.0000022892, 0.0123712263, 0.0004435007, 0.0000022892, 0.0000628338, 0.0004435007, 0.0123712263; \
+C << 1.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000, 0.0000000000; \
+D << 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000; \
+L << 1.7496302600, -0.0032396985, 72.1296388532, -0.4369906587, -0.0032396985, 1.7496302600, -0.4369906586, 72.1296388532; \
+K << 242.8102455120, 19.7898401032, -8.7045950610, -0.9720464423, -8.7045950610, -0.9720464423, 242.8102455120, 19.7898401032; \
+U_max << 12.0000000000, 12.0000000000; \
+U_min << -12.0000000000, -12.0000000000; \
+
diff --git a/frc971/control_loops/DriveTrain.q b/frc971/control_loops/DriveTrain.q
new file mode 100644
index 0000000..f56c115
--- /dev/null
+++ b/frc971/control_loops/DriveTrain.q
@@ -0,0 +1,38 @@
+package frc971.control_loops;
+
+import "aos/common/control_loop/control_loops.q";
+
+queue_group Drivetrain {
+  implements aos.control_loops.ControlLoop;
+
+  message Goal {
+    float steering;
+    float throttle;
+    bool highgear;
+    bool quickturn;
+    bool control_loop_driving;
+    float left_goal;
+    float right_goal;
+  };
+
+  message Position {
+    double left_encoder;
+    double right_encoder;
+  };
+
+  message Output {
+    float left_voltage;
+    float right_voltage;
+  };
+
+  message Status {
+    bool is_done;
+  };
+
+  queue Goal goal;
+  queue Position position;
+  queue Output output;
+  queue Status status;
+};
+
+queue_group Drivetrain drivetrain;
diff --git a/frc971/control_loops/StateFeedbackLoop.h b/frc971/control_loops/StateFeedbackLoop.h
new file mode 100644
index 0000000..ad37f49
--- /dev/null
+++ b/frc971/control_loops/StateFeedbackLoop.h
@@ -0,0 +1,131 @@
+#ifndef FRC971_CONTROL_LOOPS_STATEFEEDBACKLOOP_H_
+#define FRC971_CONTROL_LOOPS_STATEFEEDBACKLOOP_H_
+
+// wikipedia article is <http://en.wikipedia.org/wiki/State_observer>
+
+#include "Eigen/Dense"
+
+template <int number_of_states, int number_of_outputs>
+class StateFeedbackPlant {
+ public:
+  Eigen::Matrix<double, number_of_states, 1> X;
+  Eigen::Matrix<double, number_of_outputs, 1> Y;
+  Eigen::Matrix<double, number_of_outputs, 1> U;
+  Eigen::Matrix<double, number_of_outputs, 1> U_max;
+  Eigen::Matrix<double, number_of_outputs, 1> U_min;
+  Eigen::Matrix<double, number_of_states, number_of_states> A;
+  Eigen::Matrix<double, number_of_states, number_of_outputs> B;
+  Eigen::Matrix<double, number_of_outputs, number_of_states> C;
+  Eigen::Matrix<double, number_of_outputs, number_of_outputs> D;
+  // TODO(aschuh): These following 2 lines are here because MATRIX_INIT
+  // assumes that you have a controller as well as a plant.
+  Eigen::Matrix<double, number_of_states, number_of_outputs> L;
+  Eigen::Matrix<double, number_of_outputs, number_of_states> K;
+
+  StateFeedbackPlant() {
+    X.setZero();
+    Y.setZero();
+    U.setZero();
+  }
+
+  virtual ~StateFeedbackPlant() {}
+
+  // If U is outside the hardware range, limit it before the plant tries to use
+  // it.
+  virtual void CapU() {
+    for (int i = 0; i < number_of_outputs; ++i) {
+      if (U[i] > U_max[i]) {
+        U[i] = U_max[i];
+      } else if (U[i] < U_min[i]) {
+        U[i] = U_min[i];
+      }
+    }
+  }
+  // Computes the new X and Y given the control input.
+  void Update() {
+    CapU();
+    X = A * X + B * U;
+    Y = C * X + D * U;
+  }
+
+ protected:
+  // these are accessible from non-templated subclasses
+  static const int number_of_states_var = number_of_states;
+  static const int number_of_outputs_var = number_of_outputs;
+};
+
+template <int number_of_states, int number_of_outputs>
+class StateFeedbackLoop {
+ public:
+  Eigen::Matrix<double, number_of_states, 1> X;
+  Eigen::Matrix<double, number_of_states, 1> X_hat;
+  Eigen::Matrix<double, number_of_outputs, 1> Y;
+  Eigen::Matrix<double, number_of_states, 1> R;
+  Eigen::Matrix<double, number_of_outputs, 1> U;
+  Eigen::Matrix<double, number_of_outputs, 1> U_max;
+  Eigen::Matrix<double, number_of_outputs, 1> U_min;
+  Eigen::Matrix<double, number_of_outputs, 1> U_ff;
+  Eigen::Matrix<double, number_of_states, number_of_states> A;
+  Eigen::Matrix<double, number_of_states, number_of_outputs> B;
+  // K in wikipedia article
+  Eigen::Matrix<double, number_of_outputs, number_of_states> C;
+  Eigen::Matrix<double, number_of_outputs, number_of_outputs> D;
+  // B in wikipedia article
+  Eigen::Matrix<double, number_of_states, number_of_outputs> L;
+  // C in wikipedia article
+  Eigen::Matrix<double, number_of_outputs, number_of_states> K;
+
+  StateFeedbackLoop() {
+    // You have to initialize all the matrices to 0 or else all their elements
+    // are undefined.
+    X.setZero();
+    X_hat.setZero();
+    Y.setZero();
+    R.setZero();
+    U.setZero();
+    U_ff.setZero();
+  }
+  virtual ~StateFeedbackLoop() {}
+
+  virtual void FeedForward() {
+    for (int i = 0; i < number_of_outputs; ++i) {
+      U_ff[i] = 0.0;
+    }
+  }
+  // If U is outside the hardware range, limit it before it
+  // gets added to the observer.
+  virtual void CapU() {
+    for (int i = 0; i < number_of_outputs; ++i) {
+      if (U[i] > U_max[i]) {
+        U[i] = U_max[i];
+      } else if (U[i] < U_min[i]) {
+        U[i] = U_min[i];
+      }
+    }
+  }
+  // update_observer is whether or not to use the values in Y.
+  // stop_motors is whether or not to output all 0s.
+  void Update(bool update_observer, bool stop_motors) {
+    if (stop_motors) {
+      for (int i = 0; i < number_of_outputs; ++i) {
+        U[i] = 0.0;
+      }
+    } else {
+      // new power = constant * (goal - current prediction)
+      U.noalias() = K * (R - X_hat);
+      CapU();
+    }
+
+    if (update_observer) {
+      X_hat = (A - L * C) * X_hat + L * Y + B * U;
+    } else {
+      X_hat = A * X_hat + B * U;
+    }
+  }
+
+ protected:
+  // these are accessible from non-templated subclasses
+  static const int number_of_states_var = number_of_states;
+  static const int number_of_outputs_var = number_of_outputs;
+};
+#endif  // FRC971_CONTROL_LOOPS_STATEFEEDBACKLOOP_H_
diff --git a/frc971/control_loops/control_loops.gyp b/frc971/control_loops/control_loops.gyp
new file mode 100644
index 0000000..a9ddb12
--- /dev/null
+++ b/frc971/control_loops/control_loops.gyp
@@ -0,0 +1,43 @@
+{
+  'variables': {
+    'loop_files': [
+      'DriveTrain.q',
+    ]
+  },
+  'targets': [
+    {
+      'target_name': 'control_loops',
+      'type': 'static_library',
+      'sources': ['<@(loop_files)'],
+      'variables': {
+        'header_path': 'frc971/control_loops',
+      },
+      'dependencies': [
+        '<(AOS)/build/aos.gyp:libaos',
+        '<(AOS)/common/common.gyp:control_loop_queues',
+        '<(AOS)/common/common.gyp:queues',
+      ],
+      'export_dependent_settings': [
+        '<(AOS)/build/aos.gyp:libaos',
+        '<(AOS)/common/common.gyp:control_loop_queues',
+        '<(AOS)/common/common.gyp:queues',
+      ],
+      'includes': ['../../aos/build/queues.gypi'],
+    },
+    {
+      'target_name': 'DriveTrain',
+      'type': 'executable',
+      'sources': [
+        'DriveTrain.cc',
+      ],
+      'dependencies': [
+        '<(AOS)/build/aos.gyp:logging',
+        '<(AOS)/common/common.gyp:controls',
+        '<(AOS)/build/aos.gyp:libaos',
+        'control_loops',
+        '<(DEPTH)/frc971/queues/queues.gyp:queues',
+        '<(EXTERNALS):eigen',
+      ],
+    },
+  ],
+}
diff --git a/frc971/control_loops/matlab/drivetrain_controller.m b/frc971/control_loops/matlab/drivetrain_controller.m
new file mode 100644
index 0000000..11a5a4b
--- /dev/null
+++ b/frc971/control_loops/matlab/drivetrain_controller.m
@@ -0,0 +1,231 @@
+close all;
+load 'drivetrain_spin_low'
+load 'drivetrain_strait_low'
+m = 68;
+rb = 0.617998644 / 2.0;
+J = 7;
+stall_current = 133.0;
+R = 12.0 / stall_current / 4 / 0.43;
+Km = (12.0 - R * 2.7) / (4650.0 / 60.0 * 2.0 * pi);
+Kt = 0.008;
+r = 0.04445; % 3.5 inches diameter
+G_low = 60.0 / 15.0 * 50.0 / 15.0;
+G_high = 45.0 / 30.0 * 50.0 / 15.0;
+dt = 0.01;
+
+G = G_low;
+
+msp = (1.0 / m + rb ^ 2.0 / J);
+msn = (1.0 / m - rb ^ 2.0 / J);
+tc = -Km * Kt * G ^ 2.0 / (R * r ^ 2.0);
+mp = G * Kt / (R * r);
+
+A = [0 1 0 0; 0 msp*tc 0 msn*tc; 0 0 0 1; 0 msn*tc 0 msp*tc];
+B = [0 0; msp * mp msn * mp; 0 0; msn * mp msp * mp];
+C = [1 0 0 0; 0 0 1 0];
+D = [0 0; 0 0];
+
+dm = c2d(ss(A, B, C, D), dt);
+
+hp = .8;
+lp = .85;
+K = place(dm.a, dm.b, [hp, hp, lp, lp]);
+
+hlp = 0.07;
+llp = 0.09;
+L = place(dm.a', dm.c', [hlp, hlp, llp, llp])';
+
+% Plot what we computed
+
+fd = fopen('/home/aschuh/frc971/2012/trunk/src/atom_code/control_loops/Drivetrain.mat', 'w');
+n = 1;
+sm = [];
+writeMatHeader(fd, size(dm.a, 1), size(dm.b, 2));
+writeMat(fd, dm.a, 'A');
+writeMat(fd, dm.b, 'B');
+writeMat(fd, dm.c, 'C');
+writeMat(fd, dm.d, 'D');
+writeMat(fd, L, 'L');
+writeMat(fd, K, 'K');
+writeMat(fd, [12; 12], 'U_max');
+writeMat(fd, [-12; -12], 'U_min');
+writeMatFooter(fd);
+fclose(fd);
+
+full_model = dss([dm.a (-dm.b * K); eye(4) (dm.a - dm.b * K - L * dm.c)], [0, 0; 0, 0; 0, 0; 0, 0; L], [C, [0, 0, 0, 0; 0, 0, 0, 0]], 0, eye(8), 0.01);
+
+n = 1;
+sm_strait = [];
+t = drivetrain_strait_low(1, 1) + dt * (n - 1);
+x = [drivetrain_strait_low(1, 2); 0; drivetrain_strait_low(1, 3); 0];
+while t < drivetrain_strait_low(end, 1)
+    sm_strait(n, 1) = t;
+    sm_strait(n, 2) = (x(1,1) + x(3,1)) / 2.0;
+    t = t + dt;
+    x = dm.a * x + dm.b * [drivetrain_strait_low(n, 4); drivetrain_strait_low(n, 5)];
+    n = n + 1;
+end
+
+figure;
+plot(drivetrain_strait_low(:, 1), (drivetrain_strait_low(:, 2) + drivetrain_strait_low(:, 3)) / 2.0, sm_strait(:, 1), sm_strait(:, 2));
+legend('actual', 'sim');
+
+n = 1;
+sm_spin = [];
+t = drivetrain_spin_low(1, 1) + dt * (n - 1);
+x = [drivetrain_spin_low(1, 2); 0; drivetrain_spin_low(1, 3); 0];
+while t < drivetrain_spin_low(end, 1)
+    sm_spin(n, 1) = t;
+    sm_spin(n, 2) = (x(1,1) - x(3,1)) / 2.0;
+    t = t + dt;
+    x = dm.a * x + dm.b * [drivetrain_spin_low(n, 4); drivetrain_spin_low(n, 5)];
+    n = n + 1;
+end
+
+figure;
+plot(drivetrain_spin_low(:, 1), (drivetrain_spin_low(:, 2) - drivetrain_spin_low(:, 3)) / 2.0, sm_spin(:, 1), sm_spin(:, 2));
+legend('actual', 'sim');
+
+%figure;
+%nyquist(full_model);
+
+
+%%
+t = 0;
+x = [0; 0; 0; 0;];
+while t < logging(end, 1)
+    sm(n, 1) = t;
+    sm(n, 2) = x(1,1);
+    sm(n, 3) = x(3,1);
+    t = t + dt;
+    x = dm.a * x + dm.b * [12.0; 12.0];
+    n = n + 1;
+end
+
+figure;
+plot(logging(:, 1), logging(:, 2), sm(:, 1), sm(:, 2));
+legend('actual', 'sim');
+
+%% Simulation of a small turn angle with a large distance to travel
+tf = 2;
+x = [0; 0; 0.1; 0;];
+r = [10; 0; 10; 0];
+
+smt = zeros(tf / dt, 8);
+t = 0;
+xhat = x;
+n = 1;
+% 1 means scale
+% 2 means just limit to 12 volts
+% 3 means preserve the difference in power
+captype = 1;
+while n <= size(smt, 1)
+    smt(n, 1) = t;
+    smt(n, 2) = x(1,1);
+    smt(n, 3) = x(3,1);
+    t = t + dt;
+    
+    u = K * (r - xhat);
+    smt(n, 4) = u(1,1);
+    smt(n, 5) = u(2,1);
+    
+    if captype == 1
+        if sum(abs(u) > 12.0)
+            % We have a problem!
+            % Check to see if it's a big steering power problem,
+            % or a big drive error.
+            turnPower = (u(1, 1) - u(2, 1));
+            drivePower = (u(1, 1) + u(2, 1));
+            scaleFactor = 12.0 / max(abs(u));
+            smt(n, 8) = 1.0 / scaleFactor;
+            % Only start scaling the turn power up if we are far out of
+            % range.
+            if abs(turnPower) < 0.5 * abs(drivePower)
+                % Turn power is swamped.
+                deltaTurn = turnPower / 2.0 / scaleFactor * 0.5;
+                u(1, 1) = u(1, 1) + deltaTurn;
+                u(2, 1) = u(2, 1) - deltaTurn;
+                scaleFactor = 12.0 / max(abs(u));
+            else
+                if 0.5 * abs(turnPower) > abs(drivePower)
+                    % Drive power is swamped.
+                    deltaDrive = drivePower / 2.0 / scaleFactor * 0.5;
+                    u(1, 1) = u(1, 1) + deltaDrive;
+                    u(2, 1) = u(2, 1) + deltaDrive;
+                    scaleFactor = 12.0 / max(abs(u));
+                end
+            end
+            u = u * scaleFactor;
+        end
+    else
+        if captype == 2
+            if u(1, 1) > 12.0
+                u(1, 1) = 12.0;
+            end
+            if u(1, 1) < -12.0
+                u(1, 1) = -12.0;
+            end
+            if u(2, 1) > 12.0
+                u(2, 1) = 12.0;
+            end
+            if u(2, 1) < -12.0
+                u(2, 1) = -12.0;
+            end
+        else
+            if captype == 3
+                if u(1, 1) > 12.0
+                    u(2, 1) = u(2, 1) - (u(1, 1) - 12.0);
+                else
+                    if u(1, 1) < -12.0
+                        u(2, 1) = u(2, 1) - (u(1, 1) + 12.0);
+                    end
+                end
+                if u(2, 1) > 12.0
+                    u(1, 1) = u(1, 1) - (u(2, 1) - 12.0);
+                else
+                    if u(2, 1) < -12.0
+                        u(1, 1) = u(1, 1) - (u(2, 1) + 12.0);
+                    end
+                end
+                if u(1, 1) > 12.0
+                    u(1, 1) = 12.0;
+                end
+                if u(1, 1) < -12.0
+                    u(1, 1) = -12.0;
+                end
+                if u(2, 1) > 12.0
+                    u(2, 1) = 12.0;
+                end
+                if u(2, 1) < -12.0
+                    u(2, 1) = -12.0;
+                end
+            end
+        end
+        
+    end
+    smt(n, 6) = u(1,1);
+    smt(n, 7) = u(2,1);
+    xhat = dm.a * xhat + dm.b * u + L * (dm.c * x - dm.c * xhat);
+    x = dm.a * x + dm.b * u;
+    
+    n = n + 1;
+end
+
+figure;
+subplot(6, 1, 1);
+plot(smt(:, 1), smt(:, 2) + smt(:, 3));
+legend('dist');
+subplot(6, 1, 2);
+plot(smt(:, 1), smt(:, 2) - smt(:, 3));
+legend('angle');
+subplot(3, 1, 2);
+plot(smt(:, 1), smt(:, 4), smt(:, 1), smt(:, 5));
+legend('lu', 'ru');
+subplot(3, 1, 3);
+plot(smt(:, 1), smt(:, 6), smt(:, 1), smt(:, 7));
+legend('lu_{real}', 'ru_{real}');
+
+%figure;
+%plot(smt(:, 1), smt(:, 8))
+%legend('Scale Factor');
+
diff --git a/frc971/control_loops/matlab/drivetrain_spin_fast.csv b/frc971/control_loops/matlab/drivetrain_spin_fast.csv
new file mode 100644
index 0000000..5768209
--- /dev/null
+++ b/frc971/control_loops/matlab/drivetrain_spin_fast.csv
@@ -0,0 +1,215 @@
+12038,8.037056,-7.571709,1.000000
+12039,8.037056,-7.571709,1.000000
+12040,8.037056,-7.571709,1.000000
+12041,8.037254,-7.572700,1.000000
+12042,8.044792,-7.577064,1.000000
+12043,8.053718,-7.583610,1.000000
+12044,8.066016,-7.593330,1.000000
+12045,8.078909,-7.602652,1.000000
+12046,8.094183,-7.613760,1.000000
+12047,8.109655,-7.624670,1.000000
+12048,8.126118,-7.637167,1.000000
+12049,8.141789,-7.650060,1.000000
+12050,8.156070,-7.662556,1.000000
+12051,8.174716,-7.677433,1.000000
+12052,8.190188,-7.690128,1.000000
+12053,8.208635,-7.704608,1.000000
+12054,8.225694,-7.717303,1.000000
+12055,8.242951,-7.729800,1.000000
+12056,8.261200,-7.745073,1.000000
+12057,8.276077,-7.758363,1.000000
+12058,8.289565,-7.771058,1.000000
+12059,8.308012,-7.787720,1.000000
+12060,8.327253,-7.805771,1.000000
+12061,8.345105,-7.819457,1.000000
+12062,8.360379,-7.830565,1.000000
+12063,8.377041,-7.844252,1.000000
+12064,8.396083,-7.860914,1.000000
+12065,8.413539,-7.877378,1.000000
+12066,8.430597,-7.895230,1.000000
+12067,8.445871,-7.910107,1.000000
+12068,8.460153,-7.923397,1.000000
+12069,8.478600,-7.938274,1.000000
+12070,8.497444,-7.954341,1.000000
+12071,8.516288,-7.970408,1.000000
+12072,8.535132,-7.987268,1.000000
+12073,8.549215,-8.002541,1.000000
+12074,8.563299,-8.018410,1.000000
+12075,8.581944,-8.037651,1.000000
+12076,8.605152,-8.057288,1.000000
+12077,8.623203,-8.070578,1.000000
+12078,8.639865,-8.087637,1.000000
+12079,8.658709,-8.105489,1.000000
+12080,8.676958,-8.122945,1.000000
+12087,8.815213,-8.259811,1.000000
+12088,8.835247,-8.279250,1.000000
+12089,8.852702,-8.298094,1.000000
+12090,8.876307,-8.320310,1.000000
+12091,8.897333,-8.338758,1.000000
+12092,8.922921,-8.360577,1.000000
+12093,8.948311,-8.383983,1.000000
+12094,8.968147,-8.403621,1.000000
+12095,8.988974,-8.426035,1.000000
+12096,9.009405,-8.447656,1.000000
+12097,9.035191,-8.474633,1.000000
+12098,9.055027,-8.494865,1.000000
+12099,9.076450,-8.516288,1.000000
+12100,9.101641,-8.540686,1.000000
+12101,9.121675,-8.560323,1.000000
+12102,9.144090,-8.583729,1.000000
+12103,9.171066,-8.609119,1.000000
+12104,9.195663,-8.630542,1.000000
+12105,9.218672,-8.653155,1.000000
+12106,9.240690,-8.671602,1.000000
+12107,9.265881,-8.693421,1.000000
+12108,9.290676,-8.717621,1.000000
+12109,9.315868,-8.741424,1.000000
+12110,9.343043,-8.767210,1.000000
+12111,9.365655,-8.788038,1.000000
+12112,9.392632,-8.813626,1.000000
+12113,9.417427,-8.836635,1.000000
+12114,9.441230,-8.857066,1.000000
+12115,9.468801,-8.881464,1.000000
+12116,9.499150,-8.906656,1.000000
+12117,9.521564,-8.928475,1.000000
+12118,9.551120,-8.953666,1.000000
+12119,9.574724,-8.974692,1.000000
+12120,9.604081,-9.003454,1.000000
+12121,9.631455,-9.030629,1.000000
+12122,9.656646,-9.054829,1.000000
+12123,9.686400,-9.084979,1.000000
+12124,9.711789,-9.111559,1.000000
+12125,9.737576,-9.136552,1.000000
+12126,9.761776,-9.158570,1.000000
+12127,9.788554,-9.182770,1.000000
+12128,9.815927,-9.208754,1.000000
+12129,9.845681,-9.235334,1.000000
+12130,9.872261,-9.259534,1.000000
+12131,9.896460,-9.282543,1.000000
+12132,9.924627,-9.308528,1.000000
+12133,9.954976,-9.336893,1.000000
+12134,9.982746,-9.362283,1.000000
+12135,10.007739,-9.385293,1.000000
+12136,10.038881,-9.414055,1.000000
+12137,10.065064,-9.437262,1.000000
+12138,10.097595,-9.464437,1.000000
+12139,10.123977,-9.488042,1.000000
+12140,10.155912,-9.517201,1.000000
+12141,10.180905,-9.539813,1.000000
+12142,10.213039,-9.568377,1.000000
+12143,10.238429,-9.592973,1.000000
+12144,10.270166,-9.622529,1.000000
+12145,10.296548,-9.646728,1.000000
+12146,10.326897,-9.673705,1.000000
+12147,10.356253,-9.700285,1.000000
+12148,10.388784,-9.728650,1.000000
+12149,10.419133,-9.755230,1.000000
+12150,10.449878,-9.783992,1.000000
+12151,10.481020,-9.814935,1.000000
+12152,10.508592,-9.843499,1.000000
+12153,10.536759,-9.875633,1.000000
+12154,10.568099,-9.908560,1.000000
+12155,10.594679,-9.934347,1.000000
+12156,10.624036,-9.961323,1.000000
+12157,10.653195,-9.988300,1.000000
+12158,10.682949,-10.015078,1.000000
+12159,10.716074,-10.045824,1.000000
+12160,10.745828,-10.074189,1.000000
+12161,10.773003,-10.098587,1.000000
+12162,10.804542,-10.127944,1.000000
+12163,10.831518,-10.152342,1.000000
+12164,10.865041,-10.183881,1.000000
+12165,10.893406,-10.211254,1.000000
+12166,10.924945,-10.240413,1.000000
+12167,10.960054,-10.269968,1.000000
+12168,10.989411,-10.294168,1.000000
+12169,11.020950,-10.322334,1.000000
+12170,11.057249,-10.352881,1.000000
+12171,11.086210,-10.379263,1.000000
+12172,11.116162,-10.407231,1.000000
+12173,11.144130,-10.434605,1.000000
+12174,11.176859,-10.466342,1.000000
+12175,11.212564,-10.498873,1.000000
+12176,11.240929,-10.524461,1.000000
+12177,11.271872,-10.553818,1.000000
+12178,11.306387,-10.587737,1.000000
+12179,11.337926,-10.620863,1.000000
+12180,11.364704,-10.651410,1.000000
+12186,11.552945,-10.839849,1.000000
+12187,11.580517,-10.865041,1.000000
+12188,11.612056,-10.895588,1.000000
+12189,11.642603,-10.925738,1.000000
+12190,11.676125,-10.957674,1.000000
+12191,11.707268,-10.987031,1.000000
+12192,11.736823,-11.013214,1.000000
+12193,11.773122,-11.045943,1.000000
+12194,11.802479,-11.072920,1.000000
+12195,11.834415,-11.103268,1.000000
+12196,11.868532,-11.136394,1.000000
+12197,11.900865,-11.167338,1.000000
+12198,11.931808,-11.197488,1.000000
+12199,11.964141,-11.227440,1.000000
+12200,11.997068,-11.256797,1.000000
+12201,12.026623,-11.283576,1.000000
+12202,12.063121,-11.316701,1.000000
+12203,12.092676,-11.344273,1.000000
+12204,12.128778,-11.377200,1.000000
+12205,12.158333,-11.404574,1.000000
+12206,12.193641,-11.437699,1.000000
+12207,12.223394,-11.465668,1.000000
+12208,12.259297,-11.499785,1.000000
+12209,12.292621,-11.532514,1.000000
+12210,12.321581,-11.563458,1.000000
+12211,12.357087,-11.601345,1.000000
+12212,12.389618,-11.637049,1.000000
+12213,12.418776,-11.669580,1.000000
+12214,12.454481,-11.707466,1.000000
+12215,12.482648,-11.737021,1.000000
+12216,12.517757,-11.769155,1.000000
+12217,12.547511,-11.796528,1.000000
+12218,12.584802,-11.830844,1.000000
+12219,12.614754,-11.859209,1.000000
+12220,12.647284,-11.890153,1.000000
+12221,12.679815,-11.920502,1.000000
+12222,12.715520,-11.954818,1.000000
+12223,12.749042,-11.986158,1.000000
+12224,12.781969,-12.017896,1.000000
+12225,12.811723,-12.045071,1.000000
+12226,12.844849,-12.075816,1.000000
+12227,12.878371,-12.106363,1.000000
+12228,12.913084,-12.137902,1.000000
+12229,12.947995,-12.169639,1.000000
+12230,12.982707,-12.200385,1.000000
+12231,13.019602,-12.233510,1.000000
+12232,13.051736,-12.264057,1.000000
+12233,13.086448,-12.296191,1.000000
+12234,13.117590,-12.326143,1.000000
+12235,13.156072,-12.363236,1.000000
+12236,13.186222,-12.393982,1.000000
+12237,13.219745,-12.428496,1.000000
+12238,13.255846,-12.467771,1.000000
+12239,13.285401,-12.499310,1.000000
+12240,13.320907,-12.536998,1.000000
+12241,13.353834,-12.570322,1.000000
+12242,13.387754,-12.601861,1.000000
+12243,13.419689,-12.629234,1.000000
+12244,13.457774,-12.663550,1.000000
+12245,13.492090,-12.695089,1.000000
+12246,13.522438,-12.723652,1.000000
+12247,13.558738,-12.758563,1.000000
+12248,13.591269,-12.790895,1.000000
+12249,13.621419,-12.819261,1.000000
+12250,13.659305,-12.854965,1.000000
+12251,13.691241,-12.883132,1.000000
+12252,13.726350,-12.914076,1.000000
+12253,13.760864,-12.944623,1.000000
+12254,13.798751,-12.979335,-1.000000
+12255,13.829694,-13.008295,-1.000000
+12256,13.866391,-13.044000,-1.000000
+12257,13.894557,-13.069985,-1.000000
+12258,13.918360,-13.087242,-1.000000
+12259,13.925898,-13.091209,-1.000000
+12260,13.924311,-13.085060,-1.000000
+12261,13.919551,-13.075340,-1.000000
+12262,13.915980,-13.064629,-1.000000
+12263,13.913005,-13.050942,-1.000000
\ No newline at end of file
diff --git a/frc971/control_loops/matlab/drivetrain_spin_low.csv b/frc971/control_loops/matlab/drivetrain_spin_low.csv
new file mode 100644
index 0000000..a96acbe
--- /dev/null
+++ b/frc971/control_loops/matlab/drivetrain_spin_low.csv
@@ -0,0 +1,280 @@
+120.220000,16.662837,-14.612612,12.000000,-12.000000
+120.230000,16.662837,-14.612612,12.000000,-12.000000
+120.240000,16.662837,-14.612612,12.000000,-12.000000
+120.250000,16.662837,-14.612612,12.000000,-12.000000
+120.260000,16.662837,-14.612612,12.000000,-12.000000
+120.270000,16.665217,-14.617571,12.000000,-12.000000
+120.280000,16.673152,-14.624117,12.000000,-12.000000
+120.290000,16.682474,-14.632051,12.000000,-12.000000
+120.300000,16.692194,-14.640977,12.000000,-12.000000
+120.310000,16.702112,-14.650895,12.000000,-12.000000
+120.320000,16.713220,-14.660020,12.000000,-12.000000
+120.330000,16.724526,-14.671326,12.000000,-12.000000
+120.340000,16.736626,-14.682632,12.000000,-12.000000
+120.350000,16.748527,-14.693939,12.000000,-12.000000
+120.360000,16.761024,-14.705245,12.000000,-12.000000
+120.370000,16.773719,-14.716552,12.000000,-12.000000
+120.380000,16.786017,-14.728255,12.000000,-12.000000
+120.390000,16.799109,-14.740156,12.000000,-12.000000
+120.400000,16.812002,-14.751661,12.000000,-12.000000
+120.410000,16.824895,-14.763364,12.000000,-12.000000
+120.420000,16.837193,-14.775861,12.000000,-12.000000
+120.430000,16.850087,-14.787960,12.000000,-12.000000
+120.440000,0.0,-0.0,12.000000,-12.000000
+120.450000,0.0,-0.0,12.000000,-12.000000
+120.460000,0.0,-0.0,12.000000,-12.000000
+120.470000,0.0,-0.0,12.000000,-12.000000
+120.480000,16.913759,-14.847864,12.000000,-12.000000
+120.490000,16.926653,-14.860361,12.000000,-12.000000
+120.500000,16.939546,-14.873453,12.000000,-12.000000
+120.510000,16.952439,-14.886147,12.000000,-12.000000
+120.520000,16.965134,-14.899041,12.000000,-12.000000
+120.530000,16.978821,-14.911537,12.000000,-12.000000
+120.540000,16.991714,-14.924232,12.000000,-12.000000
+120.550000,17.004409,-14.936729,12.000000,-12.000000
+120.560000,17.018294,-14.950019,12.000000,-12.000000
+120.570000,17.030592,-14.963110,12.000000,-12.000000
+120.580000,17.043882,-14.976400,12.000000,-12.000000
+120.590000,17.057569,-14.989690,12.000000,-12.000000
+120.600000,17.071850,-15.003178,12.000000,-12.000000
+120.610000,17.085537,-15.016270,12.000000,-12.000000
+120.620000,17.099422,-15.029758,12.000000,-12.000000
+120.630000,17.113109,-15.043048,12.000000,-12.000000
+120.640000,17.127192,-15.056735,12.000000,-12.000000
+120.650000,17.141672,-15.070818,12.000000,-12.000000
+120.660000,17.155756,-15.084505,12.000000,-12.000000
+120.670000,17.170434,-15.098192,12.000000,-12.000000
+120.680000,17.185113,-15.112275,12.000000,-12.000000
+120.690000,17.199990,-15.126359,12.000000,-12.000000
+120.700000,17.214073,-15.140442,12.000000,-12.000000
+120.710000,17.228553,-15.155120,12.000000,-12.000000
+120.720000,17.243430,-15.170196,12.000000,-12.000000
+120.730000,17.258307,-15.184279,12.000000,-12.000000
+120.740000,17.273183,-15.198561,12.000000,-12.000000
+120.750000,17.288259,-15.212842,12.000000,-12.000000
+120.760000,17.303731,-15.227521,12.000000,-12.000000
+120.770000,17.319202,-15.242398,12.000000,-12.000000
+120.780000,17.334674,-15.256878,12.000000,-12.000000
+120.790000,17.350345,-15.272151,12.000000,-12.000000
+120.800000,17.365618,-15.287425,12.000000,-12.000000
+120.810000,17.381288,-15.302500,12.000000,-12.000000
+120.820000,17.396959,-15.317972,12.000000,-12.000000
+120.830000,17.412431,-15.333444,12.000000,-12.000000
+120.840000,17.428101,-15.349313,12.000000,-12.000000
+120.850000,17.443176,-15.364388,12.000000,-12.000000
+120.860000,17.458846,-15.379265,12.000000,-12.000000
+120.870000,17.474318,-15.394340,12.000000,-12.000000
+120.880000,17.489988,-15.409812,12.000000,-12.000000
+120.890000,17.505064,-15.424887,12.000000,-12.000000
+120.900000,17.520932,-15.439962,12.000000,-12.000000
+120.910000,17.536404,-15.455236,12.000000,-12.000000
+120.920000,17.552074,-15.469914,12.000000,-12.000000
+120.930000,17.567745,-15.485188,12.000000,-12.000000
+120.940000,17.584208,-15.501255,12.000000,-12.000000
+120.950000,17.600275,-15.515735,12.000000,-12.000000
+120.960000,17.616541,-15.531207,12.000000,-12.000000
+120.970000,17.633004,-15.546282,12.000000,-12.000000
+120.980000,17.649865,-15.561555,12.000000,-12.000000
+120.990000,17.666527,-15.576234,12.000000,-12.000000
+121.000000,17.682792,-15.591111,12.000000,-12.000000
+121.010000,17.699256,-15.606384,12.000000,-12.000000
+121.020000,17.715521,-15.621658,12.000000,-12.000000
+121.030000,17.731786,-15.636534,12.000000,-12.000000
+121.040000,17.747853,-15.652006,12.000000,-12.000000
+121.050000,17.763722,-15.666883,12.000000,-12.000000
+121.060000,17.779789,-15.682355,12.000000,-12.000000
+121.070000,17.796253,-15.698025,12.000000,-12.000000
+121.080000,17.812716,-15.713696,12.000000,-12.000000
+121.090000,17.828585,-15.728771,12.000000,-12.000000
+121.100000,17.845247,-15.744441,12.000000,-12.000000
+121.110000,17.861512,-15.760310,12.000000,-12.000000
+121.120000,17.877778,-15.775980,12.000000,-12.000000
+121.130000,17.894241,-15.791650,12.000000,-12.000000
+121.140000,17.910507,-15.807519,12.000000,-12.000000
+121.150000,17.926970,-15.822991,12.000000,-12.000000
+121.160000,17.943037,-15.838066,12.000000,-12.000000
+121.170000,17.959104,-15.853736,12.000000,-12.000000
+121.180000,17.975568,-15.869605,12.000000,-12.000000
+121.190000,17.992032,-15.884680,12.000000,-12.000000
+121.200000,18.008694,-15.900747,12.000000,-12.000000
+121.210000,18.024364,-15.916814,12.000000,-12.000000
+121.220000,18.040828,-15.932286,12.000000,-12.000000
+121.230000,18.057291,-15.947559,12.000000,-12.000000
+121.240000,18.073755,-15.963825,12.000000,-12.000000
+121.250000,18.090615,-15.979495,12.000000,-12.000000
+121.260000,18.107476,-15.995959,12.000000,-12.000000
+121.270000,18.123939,-16.011232,12.000000,-12.000000
+121.280000,18.140998,-16.027299,12.000000,-12.000000
+121.290000,18.157462,-16.042573,12.000000,-12.000000
+121.300000,18.174719,-16.058838,12.000000,-12.000000
+121.310000,18.191183,-16.074707,12.000000,-12.000000
+121.320000,18.208242,-16.090575,12.000000,-12.000000
+121.330000,18.225102,-16.106444,12.000000,-12.000000
+121.340000,18.242161,-16.122511,12.000000,-12.000000
+121.350000,18.258823,-16.138181,12.000000,-12.000000
+121.360000,18.276080,-16.154248,12.000000,-12.000000
+121.370000,18.292742,-16.169918,12.000000,-12.000000
+121.380000,18.309801,-16.185588,12.000000,-12.000000
+121.390000,18.326463,-16.201655,12.000000,-12.000000
+121.400000,18.343521,-16.217524,12.000000,-12.000000
+121.410000,18.359985,-16.232996,12.000000,-12.000000
+121.420000,18.377242,-16.249063,12.000000,-12.000000
+121.430000,18.394301,-16.265130,12.000000,-12.000000
+121.440000,18.411558,-16.280800,12.000000,-12.000000
+121.450000,0.0,-0.0,12.000000,-12.000000
+121.460000,0.0,-0.0,12.000000,-12.000000
+121.470000,0.0,-0.0,12.000000,-12.000000
+121.480000,18.480587,-16.345068,12.000000,-12.000000
+121.490000,18.497447,-16.360738,12.000000,-12.000000
+121.500000,18.514506,-16.377202,12.000000,-12.000000
+121.510000,18.531564,-16.392475,12.000000,-12.000000
+121.520000,18.548623,-16.408542,12.000000,-12.000000
+121.530000,18.565880,-16.424609,12.000000,-12.000000
+121.540000,18.582542,-16.441073,12.000000,-12.000000
+121.550000,18.599601,-16.456743,12.000000,-12.000000
+121.560000,18.616858,-16.473009,12.000000,-12.000000
+121.570000,18.633520,-16.489274,12.000000,-12.000000
+121.580000,18.650381,-16.505738,12.000000,-12.000000
+121.590000,18.666646,-16.522003,12.000000,-12.000000
+121.600000,18.683705,-16.537673,12.000000,-12.000000
+121.610000,18.700764,-16.553542,12.000000,-12.000000
+121.620000,18.718219,-16.570204,12.000000,-12.000000
+121.630000,18.734683,-16.585676,12.000000,-12.000000
+121.640000,18.752138,-16.602140,12.000000,-12.000000
+121.650000,18.768602,-16.618008,12.000000,-12.000000
+121.660000,18.785661,-16.634075,12.000000,-12.000000
+121.670000,18.802521,-16.649745,12.000000,-12.000000
+121.680000,18.819977,-16.666011,12.000000,-12.000000
+121.690000,18.836440,-16.681879,12.000000,-12.000000
+121.700000,18.853301,-16.697748,12.000000,-12.000000
+121.710000,18.870161,-16.713617,12.000000,-12.000000
+121.720000,18.887815,-16.729882,12.000000,-12.000000
+121.730000,18.904675,-16.745949,12.000000,-12.000000
+121.740000,18.921932,-16.762214,12.000000,-12.000000
+121.750000,18.938594,-16.777686,12.000000,-12.000000
+121.760000,18.955653,-16.793555,12.000000,-12.000000
+121.770000,18.972910,-16.809423,12.000000,-12.000000
+121.780000,18.990564,-16.825689,12.000000,-12.000000
+121.790000,19.007623,-16.841359,12.000000,-12.000000
+121.800000,19.025078,-16.857426,12.000000,-12.000000
+121.810000,19.041740,-16.873096,12.000000,-12.000000
+121.820000,19.058998,-16.889163,12.000000,-12.000000
+121.830000,19.076056,-16.905032,12.000000,-12.000000
+121.840000,19.094107,-16.921694,12.000000,-12.000000
+121.850000,19.110372,-16.937364,12.000000,-12.000000
+121.860000,19.128423,-16.953828,12.000000,-12.000000
+121.870000,19.144886,-16.969300,12.000000,-12.000000
+121.880000,19.162540,-16.986160,12.000000,-12.000000
+121.890000,19.179004,-17.001433,12.000000,-12.000000
+121.900000,19.196459,-17.017897,12.000000,-12.000000
+121.910000,19.213716,-17.033964,12.000000,-12.000000
+121.920000,19.231172,-17.050428,12.000000,-12.000000
+121.930000,19.248231,-17.066098,12.000000,-12.000000
+121.940000,19.265885,-17.082562,12.000000,-12.000000
+121.950000,19.283340,-17.099025,12.000000,-12.000000
+121.960000,19.301192,-17.115886,12.000000,-12.000000
+121.970000,19.318251,-17.131754,12.000000,-12.000000
+121.980000,19.335905,-17.148218,12.000000,-12.000000
+121.990000,19.352765,-17.164285,12.000000,-12.000000
+122.000000,19.370022,-17.180947,12.000000,-12.000000
+122.010000,19.387081,-17.197411,12.000000,-12.000000
+122.020000,19.404933,-17.214073,12.000000,-12.000000
+122.030000,19.421199,-17.229942,12.000000,-12.000000
+122.040000,19.439448,-17.247000,12.000000,-12.000000
+122.050000,19.455514,-17.262472,12.000000,-12.000000
+122.060000,19.472772,-17.279333,12.000000,-12.000000
+122.070000,19.489632,-17.295400,12.000000,-12.000000
+122.080000,19.507087,-17.311665,12.000000,-12.000000
+122.090000,19.523948,-17.327930,12.000000,-12.000000
+122.100000,19.541403,-17.343799,12.000000,-12.000000
+122.110000,19.558462,-17.360064,12.000000,-12.000000
+122.120000,19.576116,-17.376925,12.000000,-12.000000
+122.130000,19.592976,-17.392992,12.000000,-12.000000
+122.140000,19.610432,-17.409654,12.000000,-12.000000
+122.150000,19.627491,-17.425125,12.000000,-12.000000
+122.160000,19.644946,-17.441788,12.000000,-12.000000
+122.170000,19.661608,-17.457458,12.000000,-12.000000
+122.180000,19.679262,-17.473525,12.000000,-12.000000
+122.190000,19.696321,-17.489195,12.000000,-12.000000
+122.200000,19.713379,-17.505262,12.000000,-12.000000
+122.210000,19.730438,-17.520932,12.000000,-12.000000
+122.220000,19.748687,-17.537198,12.000000,-12.000000
+122.230000,19.765349,-17.552868,12.000000,-12.000000
+122.240000,19.783003,-17.569728,12.000000,-12.000000
+122.250000,19.799665,-17.585398,12.000000,-12.000000
+122.260000,19.817517,-17.601862,12.000000,-12.000000
+122.270000,19.834378,-17.617136,12.000000,-12.000000
+122.280000,19.851635,-17.633401,12.000000,-12.000000
+122.290000,19.869090,-17.649270,12.000000,-12.000000
+122.300000,19.886347,-17.665535,12.000000,-12.000000
+122.310000,19.903604,-17.681404,12.000000,-12.000000
+122.320000,19.921258,-17.698066,12.000000,-12.000000
+122.330000,19.938119,-17.713934,12.000000,-12.000000
+122.340000,19.955177,-17.729803,12.000000,-12.000000
+122.350000,19.972435,-17.746068,12.000000,-12.000000
+122.360000,19.989692,-17.762532,12.000000,-12.000000
+122.370000,20.005957,-17.778400,12.000000,-12.000000
+122.380000,20.023611,-17.794864,12.000000,-12.000000
+122.390000,20.040075,-17.810534,12.000000,-12.000000
+122.400000,20.057530,-17.826998,12.000000,-12.000000
+122.410000,20.074589,-17.842867,12.000000,-12.000000
+122.420000,20.092243,-17.859925,12.000000,-12.000000
+122.430000,20.109103,-17.875199,12.000000,-12.000000
+122.440000,20.126360,-17.891663,12.000000,-12.000000
+122.450000,0.0,-0.0,12.000000,-12.000000
+122.460000,0.0,-0.0,12.000000,-12.000000
+122.470000,0.0,-0.0,12.000000,-12.000000
+122.480000,20.195587,-17.957319,12.000000,-12.000000
+122.490000,20.212249,-17.973188,12.000000,-12.000000
+122.500000,20.229506,-17.989651,12.000000,-12.000000
+122.510000,20.246565,-18.005520,12.000000,-12.000000
+122.520000,20.263822,-18.022182,12.000000,-12.000000
+122.530000,20.281079,-18.038051,12.000000,-12.000000
+122.540000,20.298733,-18.054911,12.000000,-12.000000
+122.550000,20.314998,-18.070185,12.000000,-12.000000
+122.560000,20.333049,-18.087243,12.000000,-12.000000
+122.570000,20.350108,-18.102914,12.000000,-12.000000
+122.580000,20.367761,-18.119576,12.000000,-12.000000
+122.590000,20.385018,-18.135643,12.000000,-12.000000
+122.600000,20.402276,-18.152106,12.000000,-12.000000
+122.610000,20.419929,-18.167777,12.000000,-12.000000
+122.620000,20.437782,-18.184835,12.000000,-12.000000
+122.630000,20.455039,-18.200704,12.000000,-12.000000
+122.640000,20.472296,-18.216969,12.000000,-12.000000
+122.650000,20.489355,-18.233036,12.000000,-12.000000
+122.660000,20.507604,-18.249698,12.000000,-12.000000
+122.670000,20.524861,-18.265170,12.000000,-12.000000
+122.680000,20.542118,-18.281634,12.000000,-12.000000
+122.690000,20.559177,-18.297106,12.000000,-12.000000
+122.700000,20.576235,-18.313371,12.000000,-12.000000
+122.710000,20.593492,-18.329240,12.000000,-12.000000
+122.720000,20.610948,-18.346100,12.000000,-12.000000
+122.730000,20.628205,-18.361770,12.000000,-12.000000
+122.740000,20.646454,-18.378631,12.000000,-12.000000
+122.750000,20.662918,-18.393904,12.000000,-12.000000
+122.760000,20.681166,-18.410765,12.000000,-12.000000
+122.770000,20.697829,-18.426435,12.000000,-12.000000
+122.780000,20.715681,-18.442700,12.000000,-12.000000
+122.790000,20.732739,-18.458767,12.000000,-12.000000
+122.800000,20.749997,-18.474636,12.000000,-12.000000
+122.810000,20.767254,-18.491100,12.000000,-12.000000
+122.820000,20.784709,-18.507762,12.000000,-12.000000
+122.830000,20.802958,-18.525217,12.000000,-12.000000
+122.840000,20.820017,-18.541681,12.000000,-12.000000
+122.850000,20.835290,-18.555962,12.000000,-12.000000
+122.860000,20.852547,-18.572228,12.000000,-12.000000
+122.870000,0.0,-0.0,12.000000,-12.000000
+122.880000,0.0,-0.0,12.000000,-12.000000
+122.890000,0.0,-0.0,12.000000,-12.000000
+122.900000,0.0,-0.0,12.000000,-12.000000
+122.910000,20.938833,-18.652959,12.000000,-12.000000
+122.920000,20.956289,-18.669026,12.000000,-12.000000
+122.930000,20.973744,-18.685292,12.000000,-12.000000
+122.940000,20.991596,-18.702350,12.000000,-12.000000
+122.950000,21.008258,-18.718021,12.000000,-12.000000
+122.960000,21.025912,-18.734683,12.000000,-12.000000
+122.970000,21.042772,-18.750948,12.000000,-12.000000
+122.980000,21.061021,-18.768205,12.000000,-12.000000
+122.990000,21.077882,-18.783875,12.000000,-12.000000
+123.000000,21.094941,-18.800141,12.000000,-12.000000
+123.010000,21.111999,-18.816406,12.000000,-12.000000
diff --git a/frc971/control_loops/matlab/drivetrain_spin_low.mat b/frc971/control_loops/matlab/drivetrain_spin_low.mat
new file mode 100644
index 0000000..457083f
--- /dev/null
+++ b/frc971/control_loops/matlab/drivetrain_spin_low.mat
Binary files differ
diff --git a/frc971/control_loops/matlab/drivetrain_strait_low.csv b/frc971/control_loops/matlab/drivetrain_strait_low.csv
new file mode 100644
index 0000000..6966f5b
--- /dev/null
+++ b/frc971/control_loops/matlab/drivetrain_strait_low.csv
@@ -0,0 +1,103 @@
+11.590000,15.318369,-16.398228,12.000000,12.000000
+11.600000,15.318369,-16.398228,12.000000,12.000000
+11.610000,15.318369,-16.398228,12.000000,12.000000
+11.620000,15.318369,-16.398228,12.000000,12.000000
+11.630000,15.318369,-16.398228,12.000000,12.000000
+11.640000,15.321146,-16.395649,12.000000,12.000000
+11.650000,15.329278,-16.388707,12.000000,12.000000
+11.660000,15.338403,-16.380772,12.000000,12.000000
+11.670000,15.347924,-16.372045,12.000000,12.000000
+11.680000,15.358635,-16.362325,12.000000,12.000000
+11.690000,15.369545,-16.351812,12.000000,12.000000
+11.700000,15.381050,-16.341101,12.000000,12.000000
+11.710000,15.392555,-16.329993,12.000000,12.000000
+11.720000,15.404258,-16.319083,12.000000,12.000000
+11.730000,15.415961,-16.308173,12.000000,12.000000
+11.740000,15.428061,-16.294685,12.000000,12.000000
+11.750000,15.440954,-16.283379,12.000000,12.000000
+11.760000,15.453847,-16.268502,12.000000,12.000000
+11.770000,15.466939,-16.255014,12.000000,12.000000
+11.780000,15.479038,-16.241327,12.000000,12.000000
+11.790000,15.491138,-16.228235,12.000000,12.000000
+11.800000,15.503635,-16.216136,12.000000,12.000000
+11.810000,15.515933,-16.204234,12.000000,12.000000
+11.820000,15.528628,-16.192134,12.000000,12.000000
+11.830000,15.541521,-16.179043,12.000000,12.000000
+11.840000,15.554811,-16.165951,12.000000,12.000000
+11.850000,15.568299,-16.152859,12.000000,12.000000
+11.860000,15.581589,-16.139569,12.000000,12.000000
+11.870000,15.595673,-16.126081,12.000000,12.000000
+11.880000,15.610153,-16.112791,12.000000,12.000000
+11.890000,15.624435,-16.098708,12.000000,12.000000
+11.900000,15.638716,-16.084228,12.000000,12.000000
+11.910000,15.653395,-16.069946,12.000000,12.000000
+11.920000,15.668272,-16.055466,12.000000,12.000000
+11.930000,15.683347,-16.040787,12.000000,12.000000
+11.940000,15.698620,-16.026307,12.000000,12.000000
+11.950000,15.714092,-16.011430,12.000000,12.000000
+11.960000,15.728969,-15.996554,12.000000,12.000000
+11.970000,15.744243,-15.981478,12.000000,12.000000
+11.980000,15.759715,-15.966205,12.000000,12.000000
+11.990000,15.774988,-15.950931,12.000000,12.000000
+12.000000,15.789865,-15.935459,12.000000,12.000000
+12.010000,15.805535,-15.920186,12.000000,12.000000
+12.020000,15.821205,-15.904516,12.000000,12.000000
+12.030000,15.836876,-15.888845,12.000000,12.000000
+12.040000,15.852744,-15.873175,12.000000,12.000000
+12.050000,15.868613,-15.857703,12.000000,12.000000
+12.060000,15.884680,-15.841835,12.000000,12.000000
+12.070000,15.900747,-15.826164,12.000000,12.000000
+12.080000,15.916615,-15.810296,12.000000,12.000000
+12.090000,15.932881,-15.794427,12.000000,12.000000
+12.100000,15.949146,-15.778162,12.000000,12.000000
+12.110000,15.965213,-15.762293,12.000000,12.000000
+12.120000,15.981677,-15.746226,12.000000,12.000000
+12.130000,0.0,-0.0,12.000000,12.000000
+12.140000,0.0,-0.0,12.000000,12.000000
+12.150000,0.0,-0.0,12.000000,12.000000
+12.160000,16.048127,-15.680173,12.000000,12.000000
+12.170000,16.064194,-15.663709,12.000000,12.000000
+12.180000,16.081649,-15.647047,12.000000,12.000000
+12.190000,16.098113,-15.630385,12.000000,12.000000
+12.200000,16.114973,-15.613525,12.000000,12.000000
+12.210000,16.131834,-15.597061,12.000000,12.000000
+12.220000,16.148892,-15.580399,12.000000,12.000000
+12.230000,16.165951,-15.563340,12.000000,12.000000
+12.240000,16.183010,-15.546877,12.000000,12.000000
+12.250000,16.200069,-15.530016,12.000000,12.000000
+12.260000,16.217326,-15.512958,12.000000,12.000000
+12.270000,16.234583,-15.495700,12.000000,12.000000
+12.280000,16.251840,-15.478840,12.000000,12.000000
+12.290000,16.268899,-15.461980,12.000000,12.000000
+12.300000,16.286156,-15.445119,12.000000,12.000000
+12.310000,16.303215,-15.428061,12.000000,12.000000
+12.320000,16.320273,-15.411002,12.000000,12.000000
+12.330000,16.337729,-15.393943,12.000000,12.000000
+12.340000,16.354589,-15.376884,12.000000,12.000000
+12.350000,16.371648,-15.359825,12.000000,12.000000
+12.360000,16.389103,-15.342767,12.000000,12.000000
+12.370000,16.406162,-15.325708,12.000000,12.000000
+12.380000,16.423221,-15.308649,12.000000,12.000000
+12.390000,16.440676,-15.291590,12.000000,12.000000
+12.400000,16.457934,-15.274333,12.000000,12.000000
+12.410000,16.475191,-15.257671,12.000000,12.000000
+12.420000,16.492646,-15.240216,12.000000,12.000000
+12.430000,16.509903,-15.222959,12.000000,12.000000
+12.440000,16.527557,-15.205900,12.000000,12.000000
+12.450000,16.544616,-15.189040,12.000000,12.000000
+12.460000,16.561873,-15.171782,12.000000,12.000000
+12.470000,16.579130,-15.154922,12.000000,12.000000
+12.480000,16.596982,-15.137665,12.000000,12.000000
+12.490000,16.614239,-15.120209,12.000000,12.000000
+12.500000,16.631100,-15.103151,12.000000,12.000000
+12.510000,16.648159,-15.085894,12.000000,12.000000
+12.520000,16.666011,-15.068636,12.000000,12.000000
+12.530000,16.683069,-15.051379,12.000000,12.000000
+12.540000,16.700327,-15.034321,12.000000,12.000000
+12.550000,16.717782,-15.017262,12.000000,12.000000
+12.560000,16.734841,-15.000203,12.000000,12.000000
+12.570000,16.752495,-14.982946,12.000000,12.000000
+12.580000,16.769752,-14.965887,12.000000,12.000000
+12.590000,16.787009,-14.948828,12.000000,12.000000
+12.600000,16.804464,-14.931175,12.000000,12.000000
+12.610000,16.821920,-14.914314,12.000000,12.000000
diff --git a/frc971/control_loops/matlab/drivetrain_strait_low.mat b/frc971/control_loops/matlab/drivetrain_strait_low.mat
new file mode 100644
index 0000000..e6a4973
--- /dev/null
+++ b/frc971/control_loops/matlab/drivetrain_strait_low.mat
Binary files differ
diff --git a/frc971/control_loops/matlab/drivetrain_strait_low_wgaps.csv b/frc971/control_loops/matlab/drivetrain_strait_low_wgaps.csv
new file mode 100644
index 0000000..0091dd2
--- /dev/null
+++ b/frc971/control_loops/matlab/drivetrain_strait_low_wgaps.csv
@@ -0,0 +1,185 @@
+31783,-2.373349,-1.651328,1.000000
+31784,-2.373349,-1.651328,1.000000
+31785,-2.373349,-1.651328,1.000000
+31786,-2.373349,-1.651328,1.000000
+31787,-2.370771,-1.651328,1.000000
+31788,-2.364622,-1.649344,1.000000
+31789,-2.356489,-1.639624,1.000000
+31790,-2.348158,-1.632880,1.000000
+31791,-2.336257,-1.622367,1.000000
+31792,-2.325744,-1.612449,1.000000
+31793,-2.312652,-1.599755,1.000000
+31794,-2.301941,-1.589440,1.000000
+31795,-2.288651,-1.577142,1.000000
+31796,-2.278138,-1.567224,1.000000
+31797,-2.263856,-1.553934,1.000000
+31798,-2.251558,-1.542231,1.000000
+31799,-2.236681,-1.526957,1.000000
+31800,-2.223589,-1.513072,1.000000
+31801,-2.211093,-1.499386,1.000000
+31802,-2.198200,-1.486294,1.000000
+31803,-2.185505,-1.473004,1.000000
+31804,-2.173603,-1.462491,1.000000
+31805,-2.160115,-1.450193,1.000000
+31806,-2.145238,-1.436506,1.000000
+31807,-2.131353,-1.423613,1.000000
+31808,-2.117666,-1.410521,1.000000
+31809,-2.103980,-1.397231,1.000000
+31810,-2.089500,-1.383148,1.000000
+31811,-2.076011,-1.369660,1.000000
+31812,-2.063316,-1.357163,1.000000
+31813,-2.047646,-1.341493,1.000000
+31814,-2.034356,-1.328600,1.000000
+31815,-2.017496,-1.312533,1.000000
+31816,-2.002619,-1.297854,1.000000
+31817,-1.987544,-1.282977,1.000000
+31818,-1.974055,-1.269886,1.000000
+31819,-1.957195,-1.253819,1.000000
+31820,-1.941525,-1.238347,1.000000
+31821,-1.927441,-1.224859,1.000000
+31822,-1.910383,-1.208197,1.000000
+31823,-1.896299,-1.194311,1.000000
+31824,-1.879042,-1.177253,1.000000
+31825,-1.862578,-1.161384,1.000000
+31826,-1.846908,-1.145515,1.000000
+31827,-1.829254,-1.128060,1.000000
+31828,-1.814378,-1.114175,1.000000
+31829,-1.798906,-1.098306,1.000000
+31830,-1.782442,-1.081644,1.000000
+31831,-1.765978,-1.065776,1.000000
+31832,-1.751697,-1.051097,1.000000
+31833,-1.735233,-1.035229,1.000000
+31834,-1.718571,-1.019162,1.000000
+31835,-1.700124,-1.000913,1.000000
+31836,-1.683461,-0.984846,1.000000
+31837,-1.666799,-0.968382,1.000000
+31838,-1.650137,-0.952117,1.000000
+31839,-1.633277,-0.935851,1.000000
+31840,-1.616020,-0.919189,1.000000
+31841,-1.599358,-0.902527,1.000000
+31842,-1.582696,-0.885469,1.000000
+31843,-1.565835,-0.868608,1.000000
+31844,-1.548975,-0.851748,1.000000
+31845,-1.533305,-0.836474,1.000000
+31846,-1.516643,-0.820209,1.000000
+31847,-1.497997,-0.801762,1.000000
+31848,-1.481137,-0.784901,1.000000
+31849,-1.463880,-0.767843,1.000000
+31850,-1.446424,-0.750784,1.000000
+31851,-1.430952,-0.735709,1.000000
+31852,-1.411711,-0.717261,1.000000
+31853,-1.396240,-0.701789,1.000000
+31854,-1.377197,-0.682747,1.000000
+31855,-1.359543,-0.665688,1.000000
+31856,-1.342286,-0.648630,1.000000
+31857,-1.325029,-0.631174,1.000000
+31864,-1.204626,-0.512358,1.000000
+31865,-1.187171,-0.495299,1.000000
+31866,-1.169517,-0.478240,1.000000
+31867,-1.152260,-0.460785,1.000000
+31868,-1.134606,-0.443726,1.000000
+31869,-1.117150,-0.426667,1.000000
+31870,-1.097116,-0.407228,1.000000
+31871,-1.081446,-0.391756,1.000000
+31872,-1.062205,-0.373111,1.000000
+31873,-1.044551,-0.355457,1.000000
+31874,-1.027096,-0.337803,1.000000
+31875,-1.009045,-0.320546,1.000000
+31876,-0.993574,-0.304876,1.000000
+31877,-0.974134,-0.286032,1.000000
+31878,-0.958861,-0.270560,1.000000
+31879,-0.939422,-0.250724,1.000000
+31880,-0.921371,-0.233467,1.000000
+31881,-0.903916,-0.216408,1.000000
+31882,-0.886659,-0.199349,1.000000
+31883,-0.870592,-0.183679,1.000000
+31884,-0.850954,-0.164439,1.000000
+31885,-0.834887,-0.148768,1.000000
+31886,-0.815448,-0.129726,1.000000
+31887,-0.797795,-0.112667,1.000000
+31888,-0.779942,-0.095013,1.000000
+31889,-0.762090,-0.077955,1.000000
+31890,-0.746420,-0.061689,1.000000
+31891,-0.727179,-0.042647,1.000000
+31892,-0.709525,-0.025390,1.000000
+31893,-0.693657,-0.009720,1.000000
+31894,-0.674416,0.009323,1.000000
+31895,-0.658349,0.025390,1.000000
+31896,-0.640695,0.042449,1.000000
+31897,-0.621455,0.061491,1.000000
+31898,-0.603999,0.078748,1.000000
+31899,-0.586147,0.096402,1.000000
+31900,-0.567898,0.113659,1.000000
+31901,-0.550443,0.130718,1.000000
+31902,-0.532590,0.148570,1.000000
+31903,-0.514936,0.165827,1.000000
+31904,-0.497283,0.183282,1.000000
+31905,-0.481216,0.199151,1.000000
+31906,-0.463562,0.216607,1.000000
+31907,-0.445710,0.234062,1.000000
+31908,-0.428056,0.251518,1.000000
+31909,-0.408815,0.270560,1.000000
+31910,-0.390765,0.288412,1.000000
+31911,-0.373706,0.305471,1.000000
+31912,-0.357441,0.320943,1.000000
+31913,-0.337803,0.340580,1.000000
+31914,-0.321934,0.356052,1.000000
+31915,-0.302495,0.375491,1.000000
+31916,-0.284643,0.392550,1.000000
+31917,-0.266791,0.410005,1.000000
+31918,-0.248939,0.427659,1.000000
+31919,-0.231285,0.444916,1.000000
+31920,-0.213234,0.462570,1.000000
+31921,-0.195581,0.479827,1.000000
+31922,-0.178125,0.497084,1.000000
+31923,-0.160273,0.514738,1.000000
+31924,-0.142818,0.532194,1.000000
+31925,-0.126949,0.548261,1.000000
+31926,-0.109097,0.566113,1.000000
+31927,-0.089459,0.585552,1.000000
+31928,-0.071607,0.602809,1.000000
+31929,-0.053557,0.620463,1.000000
+31930,-0.035308,0.638315,1.000000
+31931,-0.017852,0.655572,1.000000
+31932,0.000198,0.673424,1.000000
+31933,0.016265,0.688896,1.000000
+31934,0.036101,0.708732,1.000000
+31935,0.054152,0.726187,1.000000
+31936,0.071607,0.743841,1.000000
+31937,0.089658,0.761693,1.000000
+31938,0.107113,0.779149,1.000000
+31939,0.124965,0.796803,1.000000
+31940,0.141429,0.812671,1.000000
+31941,0.158884,0.830325,1.000000
+31942,0.177927,0.849368,1.000000
+31943,0.195581,0.867418,1.000000
+31944,0.213433,0.884874,1.000000
+31945,0.229500,0.900742,1.000000
+31946,0.249336,0.919784,1.000000
+31947,0.266989,0.937240,1.000000
+31948,0.285040,0.954695,1.000000
+31949,0.301107,0.970564,1.000000
+31950,0.320943,0.989805,1.000000
+31951,0.338200,1.007260,1.000000
+31952,0.355854,1.024517,1.000000
+31953,0.372119,1.040584,1.000000
+31954,0.391360,1.059627,1.000000
+31955,0.409609,1.077479,1.000000
+31956,0.426866,1.094934,1.000000
+31957,0.444718,1.112191,1.000000
+31964,0.566906,1.232793,1.000000
+31965,0.584362,1.250248,-1.000000
+31966,0.602412,1.267902,-1.000000
+31967,0.620066,1.285556,-1.000000
+31968,0.637720,1.302615,-1.000000
+31969,0.656762,1.317690,-1.000000
+31970,0.664895,1.321062,-1.000000
+31971,0.667077,1.320070,-1.000000
+31972,0.669655,1.316698,-1.000000
+31973,0.666482,1.313524,-1.000000
+31974,0.660928,1.307177,-1.000000
+31975,0.652002,1.297656,-1.000000
+31976,0.641687,1.287143,-1.000000
+31977,0.630777,1.276630,-1.000000
+31978,0.619273,1.268101,-1.000000
+31979,0.608760,1.256992,-1.000000
\ No newline at end of file
diff --git a/frc971/control_loops/matlab/drivetrain_strait_low_wgaps.mat b/frc971/control_loops/matlab/drivetrain_strait_low_wgaps.mat
new file mode 100644
index 0000000..e9cd690
--- /dev/null
+++ b/frc971/control_loops/matlab/drivetrain_strait_low_wgaps.mat
Binary files differ
diff --git a/frc971/control_loops/matlab/writeMat.m b/frc971/control_loops/matlab/writeMat.m
new file mode 100644
index 0000000..b1541f5
--- /dev/null
+++ b/frc971/control_loops/matlab/writeMat.m
@@ -0,0 +1,16 @@
+function writeMat(fd, matrix, name)
+    %fprintf(fd, '%s = init_matrix(%d, %d);\n', name, size(matrix, 1), size(matrix, 2));
+    fprintf(fd, '%s << ', name);
+    first_loop = 1;
+    for i=1:size(matrix, 1)
+        for j=1:size(matrix, 2)
+            if first_loop
+                first_loop = 0;
+            else
+                fprintf(fd, ', ');
+            end
+            fprintf(fd, '%.10f', matrix(i, j));
+        end
+    end
+    fprintf(fd, '; \\\n');
+end
\ No newline at end of file
diff --git a/frc971/control_loops/matlab/writeMatFlash.m b/frc971/control_loops/matlab/writeMatFlash.m
new file mode 100644
index 0000000..10104d0
--- /dev/null
+++ b/frc971/control_loops/matlab/writeMatFlash.m
@@ -0,0 +1,16 @@
+function writeMatFlash(fd, matrix, name)
+    %fprintf(fd, '%s = init_matrix(%d, %d);\n', name, size(matrix, 1), size(matrix, 2));
+    fprintf(fd, 'flash_matrix(%s, ', name);
+    first_loop = 1;
+    for i=1:size(matrix, 1)
+        for j=1:size(matrix, 2)
+            if first_loop
+                first_loop = 0;
+            else
+                fprintf(fd, ', ');
+            end
+            fprintf(fd, '%.10f', matrix(i, j));
+        end
+    end
+    fprintf(fd, ');\n');
+end
diff --git a/frc971/control_loops/matlab/writeMatFooter.m b/frc971/control_loops/matlab/writeMatFooter.m
new file mode 100644
index 0000000..b23664e
--- /dev/null
+++ b/frc971/control_loops/matlab/writeMatFooter.m
@@ -0,0 +1,3 @@
+function writeMatFooter(fd)
+    fprintf(fd, '\n');
+end
\ No newline at end of file
diff --git a/frc971/control_loops/matlab/writeMatFooterFlash.m b/frc971/control_loops/matlab/writeMatFooterFlash.m
new file mode 100644
index 0000000..5326a94
--- /dev/null
+++ b/frc971/control_loops/matlab/writeMatFooterFlash.m
@@ -0,0 +1,3 @@
+function writeMatFooterFlash(fd)
+    fprintf(fd, '\n');
+end
diff --git a/frc971/control_loops/matlab/writeMatHeader.m b/frc971/control_loops/matlab/writeMatHeader.m
new file mode 100644
index 0000000..5c100f3
--- /dev/null
+++ b/frc971/control_loops/matlab/writeMatHeader.m
@@ -0,0 +1,4 @@
+function writeMatHeader(fd, number_of_states, number_of_outputs)
+    fprintf(fd, 'typedef StateFeedbackLoop<%d, %d> MatrixClass;\n', number_of_states, number_of_outputs);
+    fprintf(fd, '#define MATRIX_INIT ');
+end
\ No newline at end of file
diff --git a/frc971/control_loops/matlab/writeMatHeaderFlash.m b/frc971/control_loops/matlab/writeMatHeaderFlash.m
new file mode 100644
index 0000000..88a6cc6
--- /dev/null
+++ b/frc971/control_loops/matlab/writeMatHeaderFlash.m
@@ -0,0 +1,4 @@
+function writeMatHeaderFlash(fd, number_of_states, number_of_outputs)
+    fprintf(fd, 'typedef StateFeedbackLoop<%d, %d> MatrixClass;\n', number_of_states, number_of_outputs);
+    fprintf(fd, '#define MATRIX_INIT ');
+end
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()