diff --git a/frc971/atom_code/atom_code.gyp b/frc971/atom_code/atom_code.gyp
index e9c787e..df39aca 100644
--- a/frc971/atom_code/atom_code.gyp
+++ b/frc971/atom_code/atom_code.gyp
@@ -5,7 +5,8 @@
       'type': 'none',
       'dependencies': [
         '<(AOS)/build/aos_all.gyp:Atom',
-        '../control_loops/control_loops.gyp:DriveTrain',
+        '../control_loops/drivetrain/drivetrain.gyp:drivetrain',
+        '../control_loops/drivetrain/drivetrain.gyp:drivetrain_lib_test',
         '../control_loops/wrist/wrist.gyp:wrist',
         '../control_loops/wrist/wrist.gyp:wrist_lib_test',
         '../control_loops/index/index.gyp:index',
diff --git a/frc971/control_loops/DriveTrain.mat b/frc971/control_loops/DriveTrain.mat
deleted file mode 100644
index fcc6f45..0000000
--- a/frc971/control_loops/DriveTrain.mat
+++ /dev/null
@@ -1,12 +0,0 @@
-#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/StateFeedbackLoop.h b/frc971/control_loops/StateFeedbackLoop.h
deleted file mode 100644
index ad37f49..0000000
--- a/frc971/control_loops/StateFeedbackLoop.h
+++ /dev/null
@@ -1,131 +0,0 @@
-#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
index 18df792..ea09d11 100644
--- a/frc971/control_loops/control_loops.gyp
+++ b/frc971/control_loops/control_loops.gyp
@@ -1,9 +1,4 @@
 {
-  'variables': {
-    'loop_files': [
-      'DriveTrain.q',
-    ]
-  },
   'targets': [
     {
       'target_name': 'state_feedback_loop',
@@ -19,37 +14,5 @@
         '<(EXTERNALS):eigen',
       ],
     },
-    {
-      'target_name': 'control_loops',
-      'type': 'static_library',
-      'sources': ['<@(loop_files)'],
-      'variables': {
-        'header_path': 'frc971/control_loops',
-      },
-      'dependencies': [
-        '<(AOS)/common/common.gyp:control_loop_queues',
-        '<(AOS)/common/common.gyp:queues',
-      ],
-      'export_dependent_settings': [
-        '<(AOS)/common/common.gyp:control_loop_queues',
-        '<(AOS)/common/common.gyp:queues',
-      ],
-      'includes': ['../../aos/build/queues.gypi'],
-    },
-    {
-      'target_name': 'DriveTrain',
-      'type': 'executable',
-      'sources': [
-        'DriveTrain.cc',
-      ],
-      'dependencies': [
-        '<(AOS)/build/aos.gyp:logging',
-        '<(AOS)/common/common.gyp:controls',
-        'control_loops',
-        '<(DEPTH)/frc971/queues/queues.gyp:queues',
-        '<(AOS)/atom_code/atom_code.gyp:init',
-        'state_feedback_loop',
-      ],
-    },
   ],
 }
diff --git a/frc971/control_loops/DriveTrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
similarity index 95%
rename from frc971/control_loops/DriveTrain.cc
rename to frc971/control_loops/drivetrain/drivetrain.cc
index bcae82d..af04086 100644
--- a/frc971/control_loops/DriveTrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -1,4 +1,4 @@
-#include "frc971/control_loops/DriveTrain.h"
+#include "frc971/control_loops/drivetrain/drivetrain.h"
 
 #include <stdio.h>
 #include <sched.h>
@@ -7,8 +7,9 @@
 #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/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/drivetrain/drivetrain_motor_plant.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "frc971/queues/GyroAngle.q.h"
 #include "frc971/queues/Piston.q.h"
 
@@ -20,10 +21,10 @@
 // Width of the robot.
 const double width = 22.0 / 100.0 * 2.54;
 
-class DrivetrainMotorsSS : public MatrixClass {
+class DrivetrainMotorsSS : public StateFeedbackLoop<4, 2, 2> {
  public:
-  DrivetrainMotorsSS (void) {
-    MATRIX_INIT;
+  DrivetrainMotorsSS (void)
+      : StateFeedbackLoop(MakeDrivetrainLoop()) {
     _offset = 0;
     _integral_offset = 0;
     _left_goal = 0.0;
@@ -298,5 +299,3 @@
 
 }  // namespace control_loops
 }  // namespace frc971
-
-AOS_RUN_LOOP(frc971::control_loops::DrivetrainLoop)
diff --git a/frc971/control_loops/drivetrain/drivetrain.gyp b/frc971/control_loops/drivetrain/drivetrain.gyp
new file mode 100644
index 0000000..84c4a3a
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain.gyp
@@ -0,0 +1,70 @@
+{
+  'targets': [
+    {
+      'target_name': 'drivetrain_loop',
+      'type': 'static_library',
+      'sources': ['drivetrain.q'],
+      'variables': {
+        'header_path': 'frc971/control_loops/drivetrain',
+      },
+      '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_lib',
+      'type': 'static_library',
+      'sources': [
+        'drivetrain.cc',
+        'drivetrain_motor_plant.cc',
+      ],
+      'dependencies': [
+        '<(AOS)/build/aos.gyp:libaos',
+        'drivetrain_loop',
+        '<(AOS)/common/common.gyp:controls',
+        '<(DEPTH)/frc971/frc971.gyp:common',
+        '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+        '<(DEPTH)/frc971/queues/queues.gyp:queues',
+      ],
+      'export_dependent_settings': [
+        '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+        '<(AOS)/common/common.gyp:controls',
+        'drivetrain_loop',
+      ],
+    },
+    {
+      'target_name': 'drivetrain_lib_test',
+      'type': 'executable',
+      'sources': [
+        'drivetrain_lib_test.cc',
+      ],
+      'dependencies': [
+        '<(EXTERNALS):gtest',
+        'drivetrain_loop',
+        'drivetrain_lib',
+        '<(AOS)/common/common.gyp:queue_testutils',
+        '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+      ],
+    },
+    {
+      'target_name': 'drivetrain',
+      'type': 'executable',
+      'sources': [
+        'drivetrain_main.cc',
+      ],
+      'dependencies': [
+        '<(AOS)/atom_code/atom_code.gyp:init',
+        'drivetrain_lib',
+        'drivetrain_loop',
+      ],
+    },
+  ],
+}
diff --git a/frc971/control_loops/DriveTrain.h b/frc971/control_loops/drivetrain/drivetrain.h
similarity index 94%
rename from frc971/control_loops/DriveTrain.h
rename to frc971/control_loops/drivetrain/drivetrain.h
index 61ea524..7b07247 100644
--- a/frc971/control_loops/DriveTrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -2,7 +2,7 @@
 #define FRC971_CONTROL_LOOPS_DRIVETRAIN_H_
 
 #include "aos/common/control_loop/ControlLoop.h"
-#include "frc971/control_loops/DriveTrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
 
 namespace frc971 {
 namespace control_loops {
diff --git a/frc971/control_loops/DriveTrain.q b/frc971/control_loops/drivetrain/drivetrain.q
similarity index 100%
rename from frc971/control_loops/DriveTrain.q
rename to frc971/control_loops/drivetrain/drivetrain.q
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
new file mode 100644
index 0000000..934ba9d
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -0,0 +1,149 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/drivetrain/drivetrain_motor_plant.h"
+
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+
+// Class which simulates the drivetrain and sends out queue messages containing the
+// position.
+class DrivetrainSimulation {
+ public:
+  // Constructs a motor simulation.
+  DrivetrainSimulation()
+      : drivetrain_plant_(
+            new StateFeedbackPlant<4, 2, 2>(MakeDrivetrainPlant())),
+        my_drivetrain_loop_(".frc971.control_loops.drivetrain",
+                       0x8a8dde77, ".frc971.control_loops.drivetrain.goal",
+                       ".frc971.control_loops.drivetrain.position",
+                       ".frc971.control_loops.drivetrain.output",
+                       ".frc971.control_loops.drivetrain.status") {
+    Reinitialize();
+  }
+
+  // Resets the plant.
+  void Reinitialize() {
+    drivetrain_plant_->X(0, 0) = 0.0;
+    drivetrain_plant_->X(1, 0) = 0.0;
+    drivetrain_plant_->Y = drivetrain_plant_->C * drivetrain_plant_->X;
+    last_left_position_ = drivetrain_plant_->Y(0, 0);
+    last_right_position_ = drivetrain_plant_->Y(1, 0);
+  }
+
+  // Returns the position of the drivetrain.
+  double GetLeftPosition() const {
+    return drivetrain_plant_->Y(0, 0);
+  }
+  double GetRightPosition() const {
+    return drivetrain_plant_->Y(1, 0);
+  }
+
+  // Sends out the position queue messages.
+  void SendPositionMessage() {
+    const double left_encoder = GetLeftPosition();
+    const double right_encoder = GetRightPosition();
+
+    ::aos::ScopedMessagePtr<control_loops::Drivetrain::Position> position =
+        my_drivetrain_loop_.position.MakeMessage();
+    position->left_encoder = left_encoder;
+    position->right_encoder = right_encoder;
+    position.Send();
+  }
+
+  // Simulates the drivetrain moving for one timestep.
+  void Simulate() {
+    last_left_position_ = drivetrain_plant_->Y(0, 0);
+    last_right_position_ = drivetrain_plant_->Y(1, 0);
+    EXPECT_TRUE(my_drivetrain_loop_.output.FetchLatest());
+    drivetrain_plant_->U << my_drivetrain_loop_.output->left_voltage,
+                            my_drivetrain_loop_.output->right_voltage;
+    drivetrain_plant_->Update();
+  }
+
+  ::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> drivetrain_plant_;
+ private:
+  Drivetrain my_drivetrain_loop_;
+  double last_left_position_;
+  double last_right_position_;
+};
+
+class DrivetrainTest : public ::testing::Test {
+ protected:
+  ::aos::common::testing::GlobalCoreInstance my_core;
+
+  // Create a new instance of the test queue so that it invalidates the queue
+  // that it points to.  Otherwise, we will have a pointer to shared memory that
+  // is no longer valid.
+  Drivetrain my_drivetrain_loop_;
+
+  // Create a loop and simulation plant.
+  DrivetrainLoop drivetrain_motor_;
+  DrivetrainSimulation drivetrain_motor_plant_;
+
+  DrivetrainTest() : my_drivetrain_loop_(".frc971.control_loops.drivetrain",
+                               0x8a8dde77,
+                               ".frc971.control_loops.drivetrain.goal",
+                               ".frc971.control_loops.drivetrain.position",
+                               ".frc971.control_loops.drivetrain.output",
+                               ".frc971.control_loops.drivetrain.status"),
+                drivetrain_motor_(&my_drivetrain_loop_),
+                drivetrain_motor_plant_() {
+    // Flush the robot state queue so we can use clean shared memory for this
+    // test.
+    ::aos::robot_state.Clear();
+    SendDSPacket(true);
+  }
+
+  void SendDSPacket(bool enabled) {
+    ::aos::robot_state.MakeWithBuilder().enabled(enabled)
+                                        .autonomous(false)
+                                        .team_id(971).Send();
+    ::aos::robot_state.FetchLatest();
+  }
+
+  void VerifyNearGoal() {
+    my_drivetrain_loop_.goal.FetchLatest();
+    my_drivetrain_loop_.position.FetchLatest();
+    EXPECT_NEAR(my_drivetrain_loop_.goal->left_goal,
+                drivetrain_motor_plant_.GetLeftPosition(),
+                1e-2);
+    EXPECT_NEAR(my_drivetrain_loop_.goal->right_goal,
+                drivetrain_motor_plant_.GetRightPosition(),
+                1e-2);
+  }
+
+  virtual ~DrivetrainTest() {
+    ::aos::robot_state.Clear();
+  }
+};
+
+// Tests that the drivetrain converges on a goal.
+TEST_F(DrivetrainTest, ConvergesCorrectly) {
+  my_drivetrain_loop_.goal.MakeWithBuilder().control_loop_driving(true)
+      .left_goal(-1.0)
+      .right_goal(1.0).Send();
+  for (int i = 0; i < 200; ++i) {
+    drivetrain_motor_plant_.SendPositionMessage();
+    drivetrain_motor_.Iterate();
+    drivetrain_motor_plant_.Simulate();
+    SendDSPacket(true);
+  }
+  VerifyNearGoal();
+}
+
+}  // namespace testing
+}  // namespace control_loops
+}  // namespace frc971
diff --git a/frc971/control_loops/drivetrain/drivetrain_main.cc b/frc971/control_loops/drivetrain/drivetrain_main.cc
new file mode 100644
index 0000000..a6efbcd
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_main.cc
@@ -0,0 +1,11 @@
+#include "frc971/control_loops/drivetrain/drivetrain.h"
+
+#include "aos/aos_core.h"
+
+int main() {
+  ::aos::Init();
+  frc971::control_loops::DrivetrainLoop drivetrain;
+  drivetrain.Run();
+  ::aos::Cleanup();
+  return 0;
+}
diff --git a/frc971/control_loops/drivetrain/drivetrain_motor_plant.cc b/frc971/control_loops/drivetrain/drivetrain_motor_plant.cc
new file mode 100644
index 0000000..d4edba8
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_motor_plant.cc
@@ -0,0 +1,34 @@
+#include "frc971/control_loops/drivetrain/drivetrain_motor_plant.h"
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+
+StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant() {
+  Eigen::Matrix<double, 4, 4> A;
+  A << 1.0, 0.0081841122497, 0.0, -5.9473473594e-05, 0.0, 0.660289401132, 0.0, -0.0103071702002, 0.0, -5.9473473594e-05, 1.0, 0.0081841122497, 0.0, -0.0103071702002, 0.0, 0.660289401132;
+  Eigen::Matrix<double, 4, 2> B;
+  B << 0.000333031510271, 1.09073596255e-05, 0.0623024929693, 0.00189032194188, 1.09073596255e-05, 0.000333031510271, 0.00189032194188, 0.0623024929693;
+  Eigen::Matrix<double, 2, 4> C;
+  C << 1, 0, 0, 0, 0, 0, 1, 0;
+  Eigen::Matrix<double, 2, 2> D;
+  D << 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 StateFeedbackPlant<4, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop() {
+  Eigen::Matrix<double, 4, 2> L;
+  L << 1.50028940113, -0.0103071702002, 41.1373728147, -1.1627040905, -0.0103071702002, 1.50028940113, -1.1627040905, 41.1373728147;
+  Eigen::Matrix<double, 2, 4> K;
+  K << 48.196534943, -0.087263189034, -1.46233261597, -0.163410937407, -1.46233261597, -0.163410937407, 48.196534943, -0.087263189034;
+  return StateFeedbackLoop<4, 2, 2>(L, K, MakeDrivetrainPlant());
+}
+
+}  // namespace frc971
+}  // namespace control_loops
diff --git a/frc971/control_loops/drivetrain/drivetrain_motor_plant.h b/frc971/control_loops/drivetrain/drivetrain_motor_plant.h
new file mode 100644
index 0000000..af43b46
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_motor_plant.h
@@ -0,0 +1,16 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant();
+
+StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop();
+
+}  // namespace frc971
+}  // namespace control_loops
+
+#endif  // FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/matlab/drivetrain_controller.m b/frc971/control_loops/matlab/drivetrain_controller.m
index 11a5a4b..b51af99 100644
--- a/frc971/control_loops/matlab/drivetrain_controller.m
+++ b/frc971/control_loops/matlab/drivetrain_controller.m
@@ -1,12 +1,18 @@
 close all;
 load 'drivetrain_spin_low'
 load 'drivetrain_strait_low'
+% Max power amps of CIM or maybe half the mass of the robot in lbs or the whole robot in kg.
 m = 68;
+% Must be in meters. Maybe width of robot (distance of center wheels from center).
 rb = 0.617998644 / 2.0;
+% Moment of Inertia
 J = 7;
 stall_current = 133.0;
+% Resistance of the motor, divided by the number of motors.
 R = 12.0 / stall_current / 4 / 0.43;
+% Motor Constant
 Km = (12.0 - R * 2.7) / (4650.0 / 60.0 * 2.0 * pi);
+% Torque Constant
 Kt = 0.008;
 r = 0.04445; % 3.5 inches diameter
 G_low = 60.0 / 15.0 * 50.0 / 15.0;
@@ -15,6 +21,9 @@
 
 G = G_low;
 
+% must refer to how each side of the robot affects the other? Units of 1 / kg
+% I think that if the center of mass is in the center of the robot, then
+% msp will evaluate to 2/(mass of robot) and msn will evaluate to 0.
 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);
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
new file mode 100755
index 0000000..e5a8217
--- /dev/null
+++ b/frc971/control_loops/python/drivetrain.py
@@ -0,0 +1,161 @@
+#!/usr/bin/python
+
+import control_loop
+import numpy
+import sys
+from matplotlib import pylab
+
+class Drivetrain(control_loop.ControlLoop):
+  def __init__(self):
+    super(Drivetrain, self).__init__("Drivetrain")
+    # Stall Torque in N m
+    self.stall_torque = 2.42
+    # Stall Current in Amps
+    self.stall_current = 133
+    # Free Speed in RPM. Used number from last year.
+    self.free_speed = 4650.0
+    # Free Current in Amps
+    self.free_current = 2.7
+    # Moment of inertia of the drivetrain in kg m^2
+    # Just borrowed from last year.
+    self.J = 7.0
+    # Mass of the robot, in kg.
+    self.m = 68
+    # Radius of the robot, in meters (from last year).
+    self.rb = 0.617998644 / 2.0
+    # Radius of the wheels, in meters.
+    self.r = .04445
+    # Resistance of the motor, divided by the number of motors.
+    self.R = 12.0 / self.stall_current / 6
+    # Motor velocity constant
+    self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+               (12.0 - self.R * self.free_current))
+    # Torque constant
+    self.Kt = self.stall_torque / self.stall_current
+    # Gear ratios
+    self.G_low = 16.0 / 60.0 * 19.0 / 50.0
+    self.G_high = 28.0 / 48.0 * 19.0 / 50.0
+    self.G = self.G_low
+    # Control loop time step
+    self.dt = 0.01
+
+    # These describe the way that a given side of a robot will be influenced
+    # by the other side. Units of 1 / kg.
+    self.msp = 1.0 / self.m + self.rb * self.rb / self.J
+    self.msn = 1.0 / self.m - self.rb * self.rb / self.J
+    # The calculations which we will need for A and B.
+    self.tc = -self.Kt / self.Kv / (self.G * self.G * self.R * self.r * self.r)
+    self.mp = self.Kt / (self.G * self.R * self.r)
+
+    # State feedback matrices
+    # X will be of the format
+    # [[position1], [velocity1], [position2], velocity2]]
+    self.A_continuous = numpy.matrix(
+        [[0, 1, 0, 0],
+         [0, self.msp * self.tc, 0, self.msn * self.tc],
+         [0, 0, 0, 1],
+         [0, self.msn * self.tc, 0, self.msp * self.tc]])
+    self.B_continuous = numpy.matrix(
+        [[0, 0],
+         [self.msp * self.mp, self.msn * self.mp],
+         [0, 0],
+         [self.msn * self.mp, self.msp * self.mp]])
+    self.C = numpy.matrix([[1, 0, 0, 0],
+                           [0, 0, 1, 0]])
+    self.D = numpy.matrix([[0, 0],
+                           [0, 0]])
+
+    self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
+                              self.dt, self.C)
+
+    # Poles from last year.
+    self.hp = 0.8
+    self.lp = 0.85
+    self.PlaceControllerPoles([self.hp, self.hp, self.lp, self.lp])
+
+    print self.K
+
+    self.hlp = 0.07
+    self.llp = 0.09
+    self.PlaceObserverPoles([self.hlp, self.hlp, self.llp, self.llp])
+
+    self.U_max = numpy.matrix([[12.0], [12.0]])
+    self.U_min = numpy.matrix([[-12.0], [-12.0]])
+
+def main(argv):
+  # Simulate the response of the system to a step input.
+  drivetrain = Drivetrain()
+  simulated_left = []
+  simulated_right = []
+  for _ in xrange(100):
+    drivetrain.Update(numpy.matrix([[12.0], [12.0]]))
+    simulated_left.append(drivetrain.X[0, 0])
+    simulated_right.append(drivetrain.X[2, 0])
+
+  pylab.plot(range(100), simulated_left)
+  pylab.plot(range(100), simulated_right)
+  pylab.show()
+
+  # Simulate forwards motion.
+  drivetrain = Drivetrain()
+  close_loop_left = []
+  close_loop_right = []
+  R = numpy.matrix([[1.0], [0.0], [1.0], [0.0]])
+  for _ in xrange(100):
+    U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
+                   drivetrain.U_min, drivetrain.U_max)
+    drivetrain.UpdateObserver(U)
+    drivetrain.Update(U)
+    close_loop_left.append(drivetrain.X[0, 0])
+    close_loop_right.append(drivetrain.X[2, 0])
+
+  pylab.plot(range(100), close_loop_left)
+  pylab.plot(range(100), close_loop_right)
+  pylab.show()
+
+  # Try turning in place
+  drivetrain = Drivetrain()
+  close_loop_left = []
+  close_loop_right = []
+  R = numpy.matrix([[-1.0], [0.0], [1.0], [0.0]])
+  for _ in xrange(100):
+    U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
+                   drivetrain.U_min, drivetrain.U_max)
+    drivetrain.UpdateObserver(U)
+    drivetrain.Update(U)
+    close_loop_left.append(drivetrain.X[0, 0])
+    close_loop_right.append(drivetrain.X[2, 0])
+
+  pylab.plot(range(100), close_loop_left)
+  pylab.plot(range(100), close_loop_right)
+  pylab.show()
+
+  # Try turning just one side.
+  drivetrain = Drivetrain()
+  close_loop_left = []
+  close_loop_right = []
+  R = numpy.matrix([[0.0], [0.0], [1.0], [0.0]])
+  for _ in xrange(100):
+    U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
+                   drivetrain.U_min, drivetrain.U_max)
+    drivetrain.UpdateObserver(U)
+    drivetrain.Update(U)
+    close_loop_left.append(drivetrain.X[0, 0])
+    close_loop_right.append(drivetrain.X[2, 0])
+
+  pylab.plot(range(100), close_loop_left)
+  pylab.plot(range(100), close_loop_right)
+  pylab.show()
+
+  # Write the generated constants out to a file.
+  if len(argv) != 3:
+    print "Expected .h file name and .cc file name"
+  else:
+    if argv[1][-3:] == '.cc':
+      print '.cc file is second'
+    else:
+      drivetrain.DumpHeaderFile(argv[1])
+      drivetrain.DumpCppFile(argv[2], argv[1])
+
+if __name__ == '__main__':
+  sys.exit(main(sys.argv))
diff --git a/frc971/input/JoystickReader.cc b/frc971/input/JoystickReader.cc
index e0cfbac..6951760 100644
--- a/frc971/input/JoystickReader.cc
+++ b/frc971/input/JoystickReader.cc
@@ -8,7 +8,7 @@
 #include "aos/atom_code/input/JoystickInput.h"
 
 #include "frc971/input/AutoMode.q.h"
-#include "frc971/control_loops/DriveTrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "frc971/queues/GyroAngle.q.h"
 #include "frc971/queues/Piston.q.h"
 
diff --git a/frc971/input/SensorReader.cc b/frc971/input/SensorReader.cc
index fd14df4..e66dea0 100644
--- a/frc971/input/SensorReader.cc
+++ b/frc971/input/SensorReader.cc
@@ -6,7 +6,7 @@
 #include "aos/common/inttypes.h"
 #include "aos/common/input/SensorInput.h"
 
-#include "frc971/control_loops/DriveTrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "frc971/queues/sensor_values.h"
 
 #define M_PI 3.14159265358979323846
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index c62365b..a7086fb 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -24,7 +24,7 @@
         '<(AOS)/atom_code/input/input.gyp:joystick',
         '<(AOS)/common/network/network.gyp:socket',
         'actions',
-        '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+        '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
         '<(DEPTH)/frc971/queues/queues.gyp:queues',
         '<(AOS)/atom_code/atom_code.gyp:init',
       ],
@@ -37,7 +37,7 @@
       ],
       'dependencies': [
         '<(AOS)/build/aos.gyp:libaos',
-        '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+        '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
         '<(DEPTH)/frc971/queues/queues.gyp:queues',
         '<(AOS)/common/network/network.gyp:socket',
       ],
@@ -57,7 +57,6 @@
       ],
       'dependencies': [
         '<(AOS)/build/aos.gyp:libaos',
-        '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
         '<(AOS)/crio/shared_libs/shared_libs.gyp:interrupt_notifier',
       ],
     },
@@ -81,7 +80,6 @@
       ],
       'dependencies': [
         '<(AOS)/build/aos.gyp:libaos',
-        '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
         '<(DEPTH)/frc971/queues/queues.gyp:queues',
         'actions',
 # TODO(brians) this shouldn't need to be here
diff --git a/frc971/output/AtomMotorWriter.cc b/frc971/output/AtomMotorWriter.cc
index 243ac81..add20d4 100644
--- a/frc971/output/AtomMotorWriter.cc
+++ b/frc971/output/AtomMotorWriter.cc
@@ -9,7 +9,7 @@
 #include "aos/atom_code/output/MotorOutput.h"
 
 #include "frc971/queues/Piston.q.h"
-#include "frc971/control_loops/DriveTrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "frc971/constants.h"
 
 using ::frc971::control_loops::drivetrain;
diff --git a/frc971/output/output.gyp b/frc971/output/output.gyp
index 780eadf..fc2def4 100644
--- a/frc971/output/output.gyp
+++ b/frc971/output/output.gyp
@@ -41,7 +41,7 @@
       'dependencies': [
         '<(AOS)/build/aos.gyp:libaos',
         '<(AOS)/common/common.gyp:controls',
-        '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+        '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
         '<(DEPTH)/frc971/queues/queues.gyp:queues',
         '<(AOS)/common/network/network.gyp:socket',
       ],