diff --git a/y2014_bot3/control_loops/drivetrain/BUILD b/y2014_bot3/control_loops/drivetrain/BUILD
new file mode 100644
index 0000000..c1b1433
--- /dev/null
+++ b/y2014_bot3/control_loops/drivetrain/BUILD
@@ -0,0 +1,95 @@
+package(default_visibility = ['//visibility:public'])
+
+load('/aos/build/queues', 'queue_library')
+
+cc_binary(
+  name = 'replay_drivetrain_y2014_bot3',
+  srcs = [
+    'replay_drivetrain.cc',
+  ],
+  deps = [
+    ':drivetrain_queue',
+    '//aos/common/controls:replay_control_loop',
+    '//aos/linux_code:init',
+  ],
+)
+
+queue_library(
+  name = 'drivetrain_queue',
+  srcs = [
+    'drivetrain.q',
+  ],
+  deps = [
+    '//aos/common/controls:control_loop_queues',
+  ],
+)
+
+cc_library(
+  name = 'polydrivetrain_plants',
+  srcs = [
+    'polydrivetrain_dog_motor_plant.cc',
+    'drivetrain_dog_motor_plant.cc',
+    'polydrivetrain_cim_plant.cc',
+  ],
+  hdrs = [
+    'polydrivetrain_dog_motor_plant.h',
+    'drivetrain_dog_motor_plant.h',
+    'polydrivetrain_cim_plant.h',
+  ],
+  deps = [
+    '//frc971/control_loops:state_feedback_loop',
+  ],
+)
+
+cc_library(
+  name = 'drivetrain_lib',
+  hdrs = [
+    'drivetrain.h',
+  ],
+  srcs = [
+    'drivetrain.cc',
+  ],
+  deps = [
+    ':polydrivetrain_plants',
+    ':drivetrain_queue',
+    '//aos/common/controls:control_loop',
+    '//aos/common/controls:polytope',
+    '//frc971/control_loops:state_feedback_loop',
+    '//frc971/control_loops:coerce_goal',
+    '//frc971/queues:gyro',
+    '//aos/common/logging:queue_logging',
+    '//aos/common/logging:matrix_logging',
+    '//aos/common:math',
+    '//aos/common/util:log_interval',
+    '//frc971:shifter_hall_effect',
+  ],
+)
+
+cc_test(
+  name = 'drivetrain_lib_test',
+  srcs = [
+    'drivetrain_lib_test.cc',
+  ],
+  deps = [
+    '//aos/testing:googletest',
+    ':drivetrain_queue',
+    ':drivetrain_lib',
+    '//aos/common/controls:control_loop_test',
+    '//frc971/control_loops:state_feedback_loop',
+    '//frc971/queues:gyro',
+    '//aos/common:queues',
+    '//aos/common/network:team_number',
+  ],
+)
+
+cc_binary(
+  name = 'drivetrain_y2014_bot3',
+  srcs = [
+    'drivetrain_main.cc',
+  ],
+  deps = [
+    ':drivetrain_lib',
+    ':drivetrain_queue',
+    '//aos/linux_code:init',
+  ],
+)
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain.cc b/y2014_bot3/control_loops/drivetrain/drivetrain.cc
index 3a40836..047cb71 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain.cc
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain.cc
@@ -1,12 +1,9 @@
-#include "bot3/control_loops/drivetrain/drivetrain.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.h"
 
-#include <math.h>
 #include <stdio.h>
 #include <sched.h>
-
-#include <functional>
+#include <cmath>
 #include <memory>
-
 #include "Eigen/Dense"
 
 #include "aos/common/logging/logging.h"
@@ -15,21 +12,18 @@
 #include "aos/common/logging/queue_logging.h"
 #include "aos/common/logging/matrix_logging.h"
 
-#include "bot3/control_loops/drivetrain/drivetrain.q.h"
-#include "bot3/control_loops/drivetrain/drivetrain_constants.h"
-#include "bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "bot3/control_loops/drivetrain/polydrivetrain_cim_plant.h"
-#include "bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
-#include "bot3/shifter_hall_effect.h"
-#include "frc971/control_loops/coerce_goal.h"
 #include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/queues/other_sensors.q.h"
+#include "frc971/control_loops/coerce_goal.h"
+#include "y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/queues/gyro.q.h"
+#include "frc971/shifter_hall_effect.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
 
 using ::frc971::sensors::gyro_reading;
-using ::frc971::control_loops::DoCoerceGoal;
-using ::frc971::control_loops::CoerceGoal;
 
-namespace bot3 {
+namespace y2014_bot3 {
 namespace control_loops {
 
 class DrivetrainMotorsSS {
@@ -38,35 +32,38 @@
    public:
     LimitedDrivetrainLoop(StateFeedbackLoop<4, 2, 2> &&loop)
         : StateFeedbackLoop<4, 2, 2>(::std::move(loop)),
-        U_Poly_((Eigen::Matrix<double, 4, 2>() << 1, 0,
-                 -1, 0,
-                 0, 1,
-                 0, -1).finished(),
-                (Eigen::Matrix<double, 4, 1>() << 12.0, 12.0,
-                 12.0, 12.0).finished()) {
+          U_Poly_((Eigen::Matrix<double, 4, 2>() << 1, 0, -1, 0, 0, 1, 0, -1)
+                      .finished(),
+                  (Eigen::Matrix<double, 4, 1>() << 12.0, 12.0, 12.0, 12.0)
+                      .finished()) {
       ::aos::controls::HPolytope<0>::Init();
       T << 1, -1, 1, 1;
       T_inverse = T.inverse();
     }
 
-    bool output_was_capped() const {
-      return output_was_capped_;
-    }
+    bool output_was_capped() const { return output_was_capped_; }
 
    private:
     virtual void CapU() {
       const Eigen::Matrix<double, 4, 1> error = R() - X_hat();
 
       if (::std::abs(U(0, 0)) > 12.0 || ::std::abs(U(1, 0)) > 12.0) {
+        mutable_U() =
+            U() * 12.0 / ::std::max(::std::abs(U(0, 0)), ::std::abs(U(1, 0)));
+        LOG_MATRIX(DEBUG, "U is now", U());
+        // TODO(Austin): Figure out why the polytope stuff wasn't working and
+        // remove this hack.
         output_was_capped_ = true;
+        return;
+
         LOG_MATRIX(DEBUG, "U at start", U());
+        LOG_MATRIX(DEBUG, "R at start", R());
+        LOG_MATRIX(DEBUG, "Xhat at start", X_hat());
 
         Eigen::Matrix<double, 2, 2> position_K;
-        position_K << K(0, 0), K(0, 2),
-                   K(1, 0), K(1, 2);
+        position_K << K(0, 0), K(0, 2), K(1, 0), K(1, 2);
         Eigen::Matrix<double, 2, 2> velocity_K;
-        velocity_K << K(0, 1), K(0, 3),
-                   K(1, 1), K(1, 3);
+        velocity_K << K(0, 1), K(0, 3), K(1, 1), K(1, 3);
 
         Eigen::Matrix<double, 2, 1> position_error;
         position_error << error(0, 0), error(2, 0);
@@ -106,10 +103,11 @@
               standard.inverse() * W;
 
           bool is_inside_h;
-          const auto adjusted_pos_error_h =
-              DoCoerceGoal(pos_poly, LH, wh, drive_error, &is_inside_h);
+          const auto adjusted_pos_error_h = frc971::control_loops::DoCoerceGoal(
+              pos_poly, LH, wh, drive_error, &is_inside_h);
           const auto adjusted_pos_error_45 =
-              DoCoerceGoal(pos_poly, L45, w45, intersection, nullptr);
+              frc971::control_loops::DoCoerceGoal(pos_poly, L45, w45,
+                                                  intersection, nullptr);
           if (pos_poly.IsInside(intersection)) {
             adjusted_pos_error = adjusted_pos_error_h;
           } else {
@@ -136,12 +134,12 @@
 
     const ::aos::controls::HPolytope<2> U_Poly_;
     Eigen::Matrix<double, 2, 2> T, T_inverse;
-    bool output_was_capped_ = false;;
+    bool output_was_capped_ = false;
   };
 
   DrivetrainMotorsSS()
       : loop_(new LimitedDrivetrainLoop(
-            MakeDrivetrainLoop())),
+            ::y2014_bot3::control_loops::MakeDrivetrainLoop())),
         filtered_offset_(0.0),
         gyro_(0.0),
         left_goal_(0.0),
@@ -167,9 +165,7 @@
   }
   void SetPosition(double left, double right, double gyro) {
     // Decay the offset quickly because this gyro is great.
-    const double offset =
-        (right - left - gyro * kBot3TurnWidth) / 2.0;
-    // TODO(brians): filtered_offset_ = offset first time around.
+    const double offset = (right - left - gyro * kDrivetrainTurnWidth) / 2.0;
     filtered_offset_ = 0.25 * offset + 0.75 * filtered_offset_;
     gyro_ = gyro;
     SetRawPosition(left, right);
@@ -198,19 +194,13 @@
     return (loop_->X_hat(1, 0) + loop_->X_hat(3, 0)) / 2;
   }
 
-  double GetEstimatedLeftEncoder() const {
-    return loop_->X_hat(0, 0);
-  }
+  double GetEstimatedLeftEncoder() const { return loop_->X_hat(0, 0); }
 
-  double GetEstimatedRightEncoder() const {
-    return loop_->X_hat(2, 0);
-  }
+  double GetEstimatedRightEncoder() const { return loop_->X_hat(2, 0); }
 
-  bool OutputWasCapped() const {
-    return loop_->output_was_capped();
-  }
+  bool OutputWasCapped() const { return loop_->output_was_capped(); }
 
-  void SendMotors(Drivetrain::Output *output) const {
+  void SendMotors(DrivetrainQueue::Output *output) const {
     if (output) {
       output->left_voltage = loop_->U(0, 0);
       output->right_voltage = loop_->U(1, 0);
@@ -234,31 +224,26 @@
 
 class PolyDrivetrain {
  public:
-
-  enum Gear {
-    HIGH,
-    LOW,
-    SHIFTING_UP,
-    SHIFTING_DOWN
-  };
+  enum Gear { HIGH, LOW };
   // Stall Torque in N m
   static constexpr double kStallTorque = 2.42;
   // Stall Current in Amps
-  static constexpr double kStallCurrent = 133;
+  static constexpr double kStallCurrent = 133.0;
   // Free Speed in RPM. Used number from last year.
   static constexpr double kFreeSpeed = 4650.0;
   // Free Current in Amps
   static constexpr double kFreeCurrent = 2.7;
   // Moment of inertia of the drivetrain in kg m^2
   // Just borrowed from last year.
-  static constexpr double J = 6.4;
+  static constexpr double J = 10;
   // Mass of the robot, in kg.
   static constexpr double m = 68;
   // Radius of the robot, in meters (from last year).
-  static constexpr double rb = 0.617998644 / 2.0;
+  static constexpr double rb = 0.66675 / 2.0;
   static constexpr double kWheelRadius = 0.04445;
   // Resistance of the motor, divided by the number of motors.
-  static constexpr double kR = (12.0 / kStallCurrent / 4 + 0.03) / (0.93 * 0.93);
+  static constexpr double kR =
+      (12.0 / kStallCurrent / 4 + 0.03) / (0.93 * 0.93);
   // Motor velocity constant
   static constexpr double Kv =
       ((kFreeSpeed / 60.0 * 2.0 * M_PI) / (12.0 - kR * kFreeCurrent));
@@ -275,7 +260,7 @@
                  /*[*/ 12 /*]*/,
                  /*[*/ 12 /*]]*/).finished()),
         loop_(new StateFeedbackLoop<2, 2, 2>(
-            MakeVelocityDrivetrainLoop())),
+            ::y2014_bot3::control_loops::MakeVelocityDrivetrainLoop())),
         ttrust_(1.1),
         wheel_(0.0),
         throttle_(0.0),
@@ -285,84 +270,44 @@
         left_gear_(LOW),
         right_gear_(LOW),
         counter_(0) {
-
     last_position_.Zero();
     position_.Zero();
   }
-  static bool IsInGear(Gear gear) { return gear == LOW || gear == HIGH; }
 
-  static double MotorSpeed(const constants::ShifterHallEffect &hall_effect,
-                           double shifter_position, double velocity, Gear gear) {
-    // TODO(austin): G_high, G_low and kWheelRadius
-    const double avg_hall_effect =
-        (hall_effect.clear_high + hall_effect.clear_low) / 2.0;
-
-    const bool use_high =
-        kBot3SimpleShifting ? gear == HIGH : shifter_position > avg_hall_effect;
-    if (use_high) {
-      return velocity / kBot3HighGearRatio / kWheelRadius;
+  static double MotorSpeed(bool high_gear, double velocity) {
+    if (high_gear) {
+      return velocity / kDrivetrainHighGearRatio / kWheelRadius;
     } else {
-      return velocity / kBot3LowGearRatio / kWheelRadius;
+      return velocity / kDrivetrainLowGearRatio / kWheelRadius;
     }
   }
 
   void SetGoal(double wheel, double throttle, bool quickturn, bool highgear) {
-    const double kWheelNonLinearity = 0.3;
+    const double kWheelNonLinearity = 0.5;
     // Apply a sin function that's scaled to make it feel better.
     const double angular_range = M_PI_2 * kWheelNonLinearity;
+
+    quickturn_ = quickturn;
     wheel_ = sin(angular_range * wheel) / sin(angular_range);
     wheel_ = sin(angular_range * wheel_) / sin(angular_range);
-    quickturn_ = quickturn;
+    if (!quickturn_) {
+      wheel_ *= 1.5;
+    }
 
     static const double kThrottleDeadband = 0.05;
     if (::std::abs(throttle) < kThrottleDeadband) {
       throttle_ = 0;
     } else {
       throttle_ = copysign((::std::abs(throttle) - kThrottleDeadband) /
-                           (1.0 - kThrottleDeadband), throttle);
+                               (1.0 - kThrottleDeadband),
+                           throttle);
     }
 
-    // TODO(austin): Fix the upshift logic to include states.
-    Gear requested_gear;
-    requested_gear = highgear ? HIGH : LOW;
-
-    // Can be set to HIGH and LOW instead if we want to use simple shifting.
-    const Gear shift_up = kBot3SimpleShifting ? HIGH : SHIFTING_UP;
-    const Gear shift_down = kBot3SimpleShifting ? LOW : SHIFTING_DOWN;
-
-    if (left_gear_ != requested_gear) {
-      if (IsInGear(left_gear_)) {
-        if (requested_gear == HIGH) {
-          left_gear_ = shift_up;
-        } else {
-          left_gear_ = shift_down;
-        }
-      } else {
-        if (requested_gear == HIGH && left_gear_ == SHIFTING_DOWN) {
-          left_gear_ = SHIFTING_UP;
-        } else if (requested_gear == LOW && left_gear_ == SHIFTING_UP) {
-          left_gear_ = SHIFTING_DOWN;
-        }
-      }
-    }
-    if (right_gear_ != requested_gear) {
-      if (IsInGear(right_gear_)) {
-        if (requested_gear == HIGH) {
-          right_gear_ = shift_up;
-        } else {
-          right_gear_ = shift_down;
-        }
-      } else {
-        if (requested_gear == HIGH && right_gear_ == SHIFTING_DOWN) {
-          right_gear_ = SHIFTING_UP;
-        } else if (requested_gear == LOW && right_gear_ == SHIFTING_UP) {
-          right_gear_ = SHIFTING_DOWN;
-        }
-      }
-    }
+    Gear requested_gear = highgear ? HIGH : LOW;
+    left_gear_ = right_gear_ = requested_gear;
   }
 
-  void SetPosition(const Drivetrain::Position *position) {
+  void SetPosition(const DrivetrainQueue::Position *position) {
     if (position == NULL) {
       ++stale_count_;
     } else {
@@ -371,68 +316,6 @@
       position_time_delta_ = (stale_count_ + 1) * 0.01;
       stale_count_ = 0;
     }
-
-    if (position) {
-      GearLogging gear_logging;
-      // Switch to the correct controller.
-      const double left_middle_shifter_position =
-          (kBot3LeftDriveShifter.clear_high +
-          kBot3LeftDriveShifter.clear_low) / 2.0;
-      const double right_middle_shifter_position =
-          (kBot3RightDriveShifter.clear_high +
-          kBot3RightDriveShifter.clear_low) / 2.0;
-
-      if (position->left_shifter_position < left_middle_shifter_position ||
-          left_gear_ == LOW) {
-        if (position->right_shifter_position < right_middle_shifter_position ||
-            right_gear_ == LOW) {
-          gear_logging.left_loop_high = false;
-          gear_logging.right_loop_high = false;
-          loop_->set_controller_index(gear_logging.controller_index = 0);
-        } else {
-          gear_logging.left_loop_high = false;
-          gear_logging.right_loop_high = true;
-          loop_->set_controller_index(gear_logging.controller_index = 1);
-        }
-      } else {
-        if (position->right_shifter_position < right_middle_shifter_position ||
-            right_gear_ == LOW) {
-          gear_logging.left_loop_high = true;
-          gear_logging.right_loop_high = false;
-          loop_->set_controller_index(gear_logging.controller_index = 2);
-        } else {
-          gear_logging.left_loop_high = true;
-          gear_logging.right_loop_high = true;
-          loop_->set_controller_index(gear_logging.controller_index = 3);
-        }
-      }
-
-      // TODO(austin): Constants.
-      if (position->left_shifter_position >
-          kBot3LeftDriveShifter.clear_high &&
-          left_gear_ == SHIFTING_UP) {
-        left_gear_ = HIGH;
-      }
-      if (position->left_shifter_position <
-          kBot3LeftDriveShifter.clear_low &&
-          left_gear_ == SHIFTING_DOWN) {
-        left_gear_ = LOW;
-      }
-      if (position->right_shifter_position >
-          kBot3RightDriveShifter.clear_high &&
-          right_gear_ == SHIFTING_UP) {
-        right_gear_ = HIGH;
-      }
-      if (position->right_shifter_position <
-          kBot3RightDriveShifter.clear_low &&
-          right_gear_ == SHIFTING_DOWN) {
-        right_gear_ = LOW;
-      }
-
-      gear_logging.left_state = left_gear_;
-      gear_logging.right_state = right_gear_;
-      LOG_STRUCT(DEBUG, "state", gear_logging);
-    }
   }
 
   double FilterVelocity(double throttle) {
@@ -454,9 +337,10 @@
 
     const double adjusted_ff_voltage = ::aos::Clip(
         throttle * 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0);
-    return ((adjusted_ff_voltage +
-             ttrust_ * min_K_sum * (loop_->X_hat(0, 0) + loop_->X_hat(1, 0)) / 2.0) /
-            (ttrust_ * min_K_sum + min_FF_sum));
+    return (adjusted_ff_voltage +
+            ttrust_ * min_K_sum * (loop_->X_hat(0, 0) + loop_->X_hat(1, 0)) /
+                2.0) /
+           (ttrust_ * min_K_sum + min_FF_sum);
   }
 
   double MaxVelocity() {
@@ -473,11 +357,11 @@
     ::Eigen::Matrix<double, 1, 2> FF_sum = FF.colwise().sum();
     int min_FF_sum_index;
     const double min_FF_sum = FF_sum.minCoeff(&min_FF_sum_index);
-    //const double min_K_sum = loop_->K().col(min_FF_sum_index).sum();
+    // const double min_K_sum = loop_->K().col(min_FF_sum_index).sum();
     const double high_min_FF_sum = FF_high.col(0).sum();
 
-    const double adjusted_ff_voltage = ::aos::Clip(
-        12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0);
+    const double adjusted_ff_voltage =
+        ::aos::Clip(12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0);
     return adjusted_ff_voltage / min_FF_sum;
   }
 
@@ -485,47 +369,35 @@
     // TODO(austin): Observer for the current velocity instead of difference
     // calculations.
     ++counter_;
+
     const double current_left_velocity =
         (position_.left_encoder - last_position_.left_encoder) /
         position_time_delta_;
     const double current_right_velocity =
         (position_.right_encoder - last_position_.right_encoder) /
         position_time_delta_;
-    const double left_motor_speed =
-        MotorSpeed(kBot3LeftDriveShifter,
-                   position_.left_shifter_position,
-                   current_left_velocity,
-                   left_gear_);
-    const double right_motor_speed =
-        MotorSpeed(kBot3RightDriveShifter,
-                   position_.right_shifter_position,
-                   current_right_velocity,
-                   right_gear_);
+    const double left_motor_speed = MotorSpeed(left_gear_ == HIGH,
+        current_left_velocity);
+    const double right_motor_speed = MotorSpeed(right_gear_ == HIGH,
+        current_right_velocity);
 
     {
       CIMLogging logging;
 
       // Reset the CIM model to the current conditions to be ready for when we
       // shift.
-      if (IsInGear(left_gear_)) {
-        logging.left_in_gear = true;
-      } else {
-        logging.left_in_gear = false;
-      }
+      logging.left_in_gear = true;
       logging.left_motor_speed = left_motor_speed;
       logging.left_velocity = current_left_velocity;
-      if (IsInGear(right_gear_)) {
-        logging.right_in_gear = true;
-      } else {
-        logging.right_in_gear = false;
-      }
+
+      logging.right_in_gear = true;
       logging.right_motor_speed = right_motor_speed;
       logging.right_velocity = current_right_velocity;
 
       LOG_STRUCT(DEBUG, "currently", logging);
     }
 
-    if (IsInGear(left_gear_) && IsInGear(right_gear_)) {
+    {
       // FF * X = U (steady state)
       const Eigen::Matrix<double, 2, 2> FF =
           loop_->B().inverse() *
@@ -548,6 +420,7 @@
       }
       const double left_velocity = fvel - steering_velocity;
       const double right_velocity = fvel + steering_velocity;
+      LOG(DEBUG, "l=%f r=%f\n", left_velocity, right_velocity);
 
       // Integrate velocity to get the position.
       // This position is used to get integral control.
@@ -565,8 +438,8 @@
             U_Poly_.k() + U_Poly_.H() * loop_->K() * loop_->X_hat());
 
         // Limit R back inside the box.
-        loop_->mutable_R() =
-            CoerceGoal(R_poly, equality_k, equality_w, loop_->R());
+        loop_->mutable_R() = frc971::control_loops::CoerceGoal(
+            R_poly, equality_k, equality_w, loop_->R());
       }
 
       const Eigen::Matrix<double, 2, 1> FF_volts = FF * loop_->R();
@@ -578,35 +451,18 @@
       }
 
       // TODO(austin): Model this better.
-      // TODO(austin): Feed back?
+      // TODO(austin): Feedback?
       loop_->mutable_X_hat() =
           loop_->A() * loop_->X_hat() + loop_->B() * loop_->U();
-    } else {
-      // Any motor is not in gear.  Speed match.
-      ::Eigen::Matrix<double, 1, 1> R_left;
-      ::Eigen::Matrix<double, 1, 1> R_right;
-      R_left(0, 0) = left_motor_speed;
-      R_right(0, 0) = right_motor_speed;
-
-      const double wiggle =
-          (static_cast<double>((counter_ % 20) / 10) - 0.5) * 5.0;
-
-      loop_->mutable_U(0, 0) = ::aos::Clip(
-          (R_left / Kv)(0, 0) + (IsInGear(left_gear_) ? 0 : wiggle),
-          -12.0, 12.0);
-      loop_->mutable_U(1, 0) = ::aos::Clip(
-          (R_right / Kv)(0, 0) + (IsInGear(right_gear_) ? 0 : wiggle),
-          -12.0, 12.0);
-      loop_->mutable_U() *= 12.0 / position_.battery_voltage;
     }
   }
 
-  void SendMotors(Drivetrain::Output *output) {
+  void SendMotors(DrivetrainQueue::Output *output) {
     if (output != NULL) {
       output->left_voltage = loop_->U(0, 0);
       output->right_voltage = loop_->U(1, 0);
-      output->left_high = left_gear_ == HIGH || left_gear_ == SHIFTING_UP;
-      output->right_high = right_gear_ == HIGH || right_gear_ == SHIFTING_UP;
+      output->left_high = left_gear_ == HIGH;
+      output->right_high = right_gear_ == HIGH;
     }
   }
 
@@ -623,10 +479,11 @@
   double position_time_delta_;
   Gear left_gear_;
   Gear right_gear_;
-  Drivetrain::Position last_position_;
-  Drivetrain::Position position_;
+  DrivetrainQueue::Position last_position_;
+  DrivetrainQueue::Position position_;
   int counter_;
 };
+
 constexpr double PolyDrivetrain::kStallTorque;
 constexpr double PolyDrivetrain::kStallCurrent;
 constexpr double PolyDrivetrain::kFreeSpeed;
@@ -639,11 +496,10 @@
 constexpr double PolyDrivetrain::Kv;
 constexpr double PolyDrivetrain::Kt;
 
-
-void DrivetrainLoop::RunIteration(const Drivetrain::Goal *goal,
-                                  const Drivetrain::Position *position,
-                                  Drivetrain::Output *output,
-                                  Drivetrain::Status * status) {
+void DrivetrainLoop::RunIteration(const DrivetrainQueue::Goal *goal,
+                                  const DrivetrainQueue::Position *position,
+                                  DrivetrainQueue::Output *output,
+                                  DrivetrainQueue::Status *status) {
   // TODO(aschuh): These should be members of the class.
   static DrivetrainMotorsSS dt_closedloop;
   static PolyDrivetrain dt_openloop;
@@ -655,17 +511,22 @@
   }
   no_position_.Print();
 
-  double wheel = goal->steering;
-  double throttle = goal->throttle;
-  bool quickturn = goal->quickturn;
-  bool highgear = goal->highgear;
+  bool control_loop_driving = false;
+  if (goal) {
+    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;
+    control_loop_driving = goal->control_loop_driving;
+    double left_goal = goal->left_goal;
+    double right_goal = goal->right_goal;
 
-  dt_closedloop.SetGoal(left_goal, goal->left_velocity_goal, right_goal,
-                        goal->right_velocity_goal);
+    dt_closedloop.SetGoal(left_goal, goal->left_velocity_goal, right_goal,
+                          goal->right_velocity_goal);
+    dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
+  }
+
   if (!bad_pos) {
     const double left_encoder = position->left_encoder;
     const double right_encoder = position->right_encoder;
@@ -678,7 +539,6 @@
     }
   }
   dt_openloop.SetPosition(position);
-  dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
   dt_openloop.Update();
 
   if (control_loop_driving) {
@@ -699,15 +559,18 @@
     if (goal) {
       done = ((::std::abs(goal->left_goal -
                           dt_closedloop.GetEstimatedLeftEncoder()) <
-               kBot3DrivetrainDoneDistance) &&
+               kDrivetrainDoneDistance) &&
               (::std::abs(goal->right_goal -
                           dt_closedloop.GetEstimatedRightEncoder()) <
-               kBot3DrivetrainDoneDistance));
+               kDrivetrainDoneDistance));
     }
     status->is_done = done;
     status->robot_speed = dt_closedloop.GetEstimatedRobotSpeed();
     status->filtered_left_position = dt_closedloop.GetEstimatedLeftEncoder();
     status->filtered_right_position = dt_closedloop.GetEstimatedRightEncoder();
+
+    status->filtered_left_velocity = dt_closedloop.loop().X_hat(1, 0);
+    status->filtered_right_velocity = dt_closedloop.loop().X_hat(3, 0);
     status->output_was_capped = dt_closedloop.OutputWasCapped();
     status->uncapped_left_voltage = dt_closedloop.loop().U_uncapped(0, 0);
     status->uncapped_right_voltage = dt_closedloop.loop().U_uncapped(1, 0);
@@ -715,4 +578,4 @@
 }
 
 }  // namespace control_loops
-}  // namespace bot3
+}  // namespace y2014_bot3
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain.gyp b/y2014_bot3/control_loops/drivetrain/drivetrain.gyp
deleted file mode 100644
index d17cdf2..0000000
--- a/y2014_bot3/control_loops/drivetrain/drivetrain.gyp
+++ /dev/null
@@ -1,102 +0,0 @@
-{
-  'targets': [
-    {
-      'target_name': 'drivetrain_loop',
-      'type': 'static_library',
-      'sources': ['drivetrain.q'],
-      'variables': {
-        'header_path': 'bot3/control_loops/drivetrain',
-      },
-      'dependencies': [
-        '<(AOS)/common/controls/controls.gyp:control_loop_queues',
-      ],
-      'export_dependent_settings': [
-        '<(AOS)/common/controls/controls.gyp:control_loop_queues',
-      ],
-      'includes': ['../../../aos/build/queues.gypi'],
-    },
-    {
-      'target_name': 'polydrivetrain_plants',
-      'type': 'static_library',
-      'sources': [
-        'polydrivetrain_dog_motor_plant.cc',
-        'drivetrain_dog_motor_plant.cc',
-      ],
-      'dependencies': [
-        '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
-      ],
-      'export_dependent_settings': [
-        '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
-      ],
-    },
-    {
-      'target_name': 'drivetrain_constants',
-      'type': 'static_library',
-      'sources': [
-        #'drivetrain_constants.h'
-      ],
-      'dependencies': [
-        'polydrivetrain_plants',
-      ],
-      'export_dependent_settings': [
-        'polydrivetrain_plants',
-      ],
-    },
-    {
-      'target_name': 'drivetrain_lib',
-      'type': 'static_library',
-      'sources': [
-        'drivetrain.cc',
-        'polydrivetrain_cim_plant.cc',
-      ],
-      'dependencies': [
-        'drivetrain_loop',
-        'drivetrain_constants',
-        '<(AOS)/common/controls/controls.gyp:control_loop',
-        '<(AOS)/common/controls/controls.gyp:polytope',
-        '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
-        '<(DEPTH)/frc971/control_loops/control_loops.gyp:coerce_goal',
-        '<(DEPTH)/frc971/queues/queues.gyp:queues',
-        '<(AOS)/common/util/util.gyp:log_interval',
-        '<(AOS)/common/logging/logging.gyp:queue_logging',
-        '<(AOS)/common/logging/logging.gyp:matrix_logging',
-      ],
-      'export_dependent_settings': [
-        '<(AOS)/common/controls/controls.gyp:polytope',
-        '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
-        '<(DEPTH)/frc971/control_loops/control_loops.gyp:coerce_goal',
-        '<(AOS)/common/controls/controls.gyp:control_loop',
-        'drivetrain_loop',
-      ],
-    },
-    {
-      'target_name': 'drivetrain_lib_test',
-      'type': 'executable',
-      'sources': [
-        'drivetrain_lib_test.cc',
-      ],
-      'dependencies': [
-        '<(EXTERNALS):gtest',
-        'drivetrain_loop',
-        'drivetrain_lib',
-        '<(AOS)/common/controls/controls.gyp:control_loop_test',
-        '<(AOS)/common/network/network.gyp:team_number',
-        '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
-        '<(DEPTH)/frc971/queues/queues.gyp:queues',
-        '<(AOS)/common/common.gyp:queues',
-      ],
-    },
-    {
-      'target_name': 'drivetrain',
-      'type': 'executable',
-      'sources': [
-        'drivetrain_main.cc',
-      ],
-      'dependencies': [
-        '<(AOS)/linux_code/linux_code.gyp:init',
-        'drivetrain_lib',
-        'drivetrain_loop',
-      ],
-    },
-  ],
-}
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain.h b/y2014_bot3/control_loops/drivetrain/drivetrain.h
index b2bed8c..0565820 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain.h
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain.h
@@ -1,5 +1,5 @@
-#ifndef BOT3_CONTROL_LOOPS_DRIVETRAIN_H_
-#define BOT3_CONTROL_LOOPS_DRIVETRAIN_H_
+#ifndef Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_H_
+#define Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_H_
 
 #include "Eigen/Dense"
 
@@ -7,30 +7,46 @@
 #include "aos/common/controls/control_loop.h"
 #include "aos/common/controls/polytope.h"
 #include "aos/common/util/log_interval.h"
-#include "bot3/control_loops/drivetrain/drivetrain.q.h"
 
-namespace bot3 {
+#include "frc971/shifter_hall_effect.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.q.h"
+
+namespace y2014_bot3 {
 namespace control_loops {
 
+// Constants
+// TODO(comran): Get actual constants.
+constexpr double kDrivetrainTurnWidth = 0.63;
+constexpr double kDrivetrainDoneDistance = 0.02;
+constexpr double kDrivetrainEncoderRatio = 18.0 / 44.0;
+constexpr double kDrivetrainHighGearRatio =
+    kDrivetrainEncoderRatio * 18.0 / 60.0;
+constexpr double kDrivetrainLowGearRatio = kDrivetrainHighGearRatio;
+const ::frc971::constants::ShifterHallEffect kDrivetrainRightShifter{
+    555, 657, 660, 560, 0.2, 0.7};
+const ::frc971::constants::ShifterHallEffect kDrivetrainLeftShifter{
+    555, 660, 644, 552, 0.2, 0.7};
+// End constants
+
 class DrivetrainLoop
-    : public aos::controls::ControlLoop<control_loops::Drivetrain, true, false> {
+    : public aos::controls::ControlLoop<control_loops::DrivetrainQueue> {
  public:
   // Constructs a control loop which can take a Drivetrain or defaults to the
-  // drivetrain at bot3::control_loops::drivetrain
-  explicit DrivetrainLoop(
-      control_loops::Drivetrain *my_drivetrain = &control_loops::drivetrain)
-      : aos::controls::ControlLoop<control_loops::Drivetrain, true, false>(
-          my_drivetrain) {
+  // drivetrain at y2014_bot3::control_loops::drivetrain
+  explicit DrivetrainLoop(control_loops::DrivetrainQueue *my_drivetrain =
+                              &control_loops::drivetrain_queue)
+      : aos::controls::ControlLoop<control_loops::DrivetrainQueue>(
+            my_drivetrain) {
     ::aos::controls::HPolytope<0>::Init();
   }
 
  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);
+      const control_loops::DrivetrainQueue::Goal *goal,
+      const control_loops::DrivetrainQueue::Position *position,
+      control_loops::DrivetrainQueue::Output *output,
+      control_loops::DrivetrainQueue::Status *status);
 
   typedef ::aos::util::SimpleLogInterval SimpleLogInterval;
   SimpleLogInterval no_position_ = SimpleLogInterval(
@@ -38,6 +54,6 @@
 };
 
 }  // namespace control_loops
-}  // namespace bot3
+}  // namespace y2014_bot3
 
-#endif  // BOT3_CONTROL_LOOPS_DRIVETRAIN_H_
+#endif  // Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_H_
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain.q b/y2014_bot3/control_loops/drivetrain/drivetrain.q
index f4fe3d5..c4563f9 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain.q
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain.q
@@ -1,15 +1,7 @@
-package bot3.control_loops;
+package y2014_bot3.control_loops;
 
 import "aos/common/controls/control_loops.q";
 
-struct GearLogging {
-  int8_t controller_index;
-  bool left_loop_high;
-  bool right_loop_high;
-  int8_t left_state;
-  int8_t right_state;
-};
-
 struct CIMLogging {
   bool left_in_gear;
   bool right_in_gear;
@@ -19,7 +11,7 @@
   double right_velocity;
 };
 
-queue_group Drivetrain {
+queue_group DrivetrainQueue {
   implements aos.control_loops.ControlLoop;
 
   message Goal {
@@ -37,9 +29,6 @@
   message Position {
     double left_encoder;
     double right_encoder;
-    double left_shifter_position;
-    double right_shifter_position;
-    double battery_voltage;
   };
 
   message Output {
@@ -53,6 +42,8 @@
     double robot_speed;
     double filtered_left_position;
     double filtered_right_position;
+    double filtered_left_velocity;
+    double filtered_right_velocity;
 
     double uncapped_left_voltage;
     double uncapped_right_voltage;
@@ -67,4 +58,4 @@
   queue Status status;
 };
 
-queue_group Drivetrain drivetrain;
+queue_group DrivetrainQueue drivetrain_queue;
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc b/y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
index da59426..05f6a2b 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
@@ -1,17 +1,17 @@
-#include "bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
 
 #include <vector>
 
 #include "frc971/control_loops/state_feedback_loop.h"
 
-namespace bot3 {
+namespace y2014_bot3 {
 namespace control_loops {
 
 StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowLowPlantCoefficients() {
   Eigen::Matrix<double, 4, 4> A;
-  A << 1.0, 0.00718627304278, 0.0, 0.000575327102319, 0.0, 0.498791664199, 0.0, 0.0897184892451, 0.0, 0.000575327102319, 1.0, 0.00718627304278, 0.0, 0.0897184892451, 0.0, 0.498791664199;
+  A << 1.0, 0.00488249769672, 0.0, 2.55881088306e-05, 0.0, 0.953388055571, 0.0, 0.0100729449137, 0.0, 2.55881088306e-05, 1.0, 0.00488249769672, 0.0, 0.0100729449137, 0.0, 0.953388055571;
   Eigen::Matrix<double, 4, 2> B;
-  B << 0.000404687213064, -8.27470202964e-05, 0.0720868114296, -0.0129038552514, -8.27470202964e-05, 0.000404687213064, -0.0129038552514, 0.0720868114296;
+  B << 3.0031863249e-05, -6.5399448668e-06, 0.0119133285199, -0.00257449680311, -6.5399448668e-06, 3.0031863249e-05, -0.00257449680311, 0.0119133285199;
   Eigen::Matrix<double, 2, 4> C;
   C << 1, 0, 0, 0, 0, 0, 1, 0;
   Eigen::Matrix<double, 2, 2> D;
@@ -25,9 +25,9 @@
 
 StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowHighPlantCoefficients() {
   Eigen::Matrix<double, 4, 4> A;
-  A << 1.0, 0.00715806931946, 0.0, 8.27195198739e-05, 0.0, 0.491841728751, 0.0, 0.0145551608494, 0.0, 0.000706303878161, 1.0, 0.00959963582133, 0.0, 0.124279814134, 0.0, 0.921454874639;
+  A << 1.0, 0.00488249769672, 0.0, 2.55881088306e-05, 0.0, 0.953388055571, 0.0, 0.0100729449137, 0.0, 2.55881088306e-05, 1.0, 0.00488249769672, 0.0, 0.0100729449137, 0.0, 0.953388055571;
   Eigen::Matrix<double, 4, 2> B;
-  B << 0.000408743642973, -3.47646073582e-05, 0.0730863931411, -0.00611711060145, -0.000101584891631, 0.000168261415116, -0.0178746738354, 0.0330102308046;
+  B << 3.0031863249e-05, -6.5399448668e-06, 0.0119133285199, -0.00257449680311, -6.5399448668e-06, 3.0031863249e-05, -0.00257449680311, 0.0119133285199;
   Eigen::Matrix<double, 2, 4> C;
   C << 1, 0, 0, 0, 0, 0, 1, 0;
   Eigen::Matrix<double, 2, 2> D;
@@ -41,9 +41,9 @@
 
 StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighLowPlantCoefficients() {
   Eigen::Matrix<double, 4, 4> A;
-  A << 1.0, 0.00959963582133, 0.0, 0.000706303878161, 0.0, 0.921454874639, 0.0, 0.124279814134, 0.0, 8.27195198739e-05, 1.0, 0.00715806931946, 0.0, 0.0145551608494, 0.0, 0.491841728751;
+  A << 1.0, 0.00488249769672, 0.0, 2.55881088306e-05, 0.0, 0.953388055571, 0.0, 0.0100729449137, 0.0, 2.55881088306e-05, 1.0, 0.00488249769672, 0.0, 0.0100729449137, 0.0, 0.953388055571;
   Eigen::Matrix<double, 4, 2> B;
-  B << 0.000168261415116, -0.000101584891631, 0.0330102308046, -0.0178746738354, -3.47646073582e-05, 0.000408743642973, -0.00611711060145, 0.0730863931411;
+  B << 3.0031863249e-05, -6.5399448668e-06, 0.0119133285199, -0.00257449680311, -6.5399448668e-06, 3.0031863249e-05, -0.00257449680311, 0.0119133285199;
   Eigen::Matrix<double, 2, 4> C;
   C << 1, 0, 0, 0, 0, 0, 1, 0;
   Eigen::Matrix<double, 2, 2> D;
@@ -57,9 +57,9 @@
 
 StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighHighPlantCoefficients() {
   Eigen::Matrix<double, 4, 4> A;
-  A << 1.0, 0.00959512220091, 0.0, 0.000100752776744, 0.0, 0.920202836632, 0.0, 0.0195951135, 0.0, 0.000100752776744, 1.0, 0.00959512220091, 0.0, 0.0195951135, 0.0, 0.920202836632;
+  A << 1.0, 0.00488249769672, 0.0, 2.55881088306e-05, 0.0, 0.953388055571, 0.0, 0.0100729449137, 0.0, 2.55881088306e-05, 1.0, 0.00488249769672, 0.0, 0.0100729449137, 0.0, 0.953388055571;
   Eigen::Matrix<double, 4, 2> B;
-  B << 0.000170158358449, -4.23434605168e-05, 0.0335364259492, -0.00823525605573, -4.23434605168e-05, 0.000170158358449, -0.00823525605573, 0.0335364259492;
+  B << 3.0031863249e-05, -6.5399448668e-06, 0.0119133285199, -0.00257449680311, -6.5399448668e-06, 3.0031863249e-05, -0.00257449680311, 0.0119133285199;
   Eigen::Matrix<double, 2, 4> C;
   C << 1, 0, 0, 0, 0, 0, 1, 0;
   Eigen::Matrix<double, 2, 2> D;
@@ -73,53 +73,61 @@
 
 StateFeedbackController<4, 2, 2> MakeDrivetrainLowLowController() {
   Eigen::Matrix<double, 4, 2> L;
-  L << 0.798791664199, 0.0897184892451, 3.57844915691, 3.42875197165, 0.0897184892451, 0.798791664199, 3.42875197165, 3.57844915691;
+  L << 1.25338805557, 0.0100729449137, 74.065541605, 2.10150476024, 0.0100729449137, 1.25338805557, 2.10150476024, 74.065541605;
   Eigen::Matrix<double, 2, 4> K;
-  K << 118.526561433, 4.19900632703, 7.79020074229, 0.921948843301, 7.79020074228, 0.921948843301, 118.526561433, 4.19900632703;
-  return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainLowLowPlantCoefficients());
+  K << 158.010515913, 12.6294601573, 1.98944476188, 1.06520656374, 1.98944476188, 1.06520656374, 158.010515913, 12.6294601573;
+  Eigen::Matrix<double, 4, 4> A_inv;
+  A_inv << 1.0, -0.00512149525437, 0.0, 2.72716136841e-05, 0.0, 1.04900794038, 0.0, -0.0110832091253, 0.0, 2.72716136841e-05, 1.0, -0.00512149525437, 0.0, -0.0110832091253, 0.0, 1.04900794038;
+  return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainLowLowPlantCoefficients());
 }
 
 StateFeedbackController<4, 2, 2> MakeDrivetrainLowHighController() {
   Eigen::Matrix<double, 4, 2> L;
-  L << 0.780361183952, 0.0157698223449, 2.31127571816, 1.09059511674, 0.0157698223449, 1.23293541944, 3.02157734826, 34.5051463402;
+  L << 1.25338805557, 0.0100729449137, 74.065541605, 2.10150476024, 0.0100729449137, 1.25338805557, 2.10150476024, 74.065541605;
   Eigen::Matrix<double, 2, 4> K;
-  K << 116.74766142, 4.04674599343, -6.38397728262, -0.174173971494, 20.8480994015, 2.49474383494, 135.799605924, 11.0272286082;
-  return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainLowHighPlantCoefficients());
+  K << 158.010515913, 12.6294601573, 1.98944476188, 1.06520656374, 1.98944476188, 1.06520656374, 158.010515913, 12.6294601573;
+  Eigen::Matrix<double, 4, 4> A_inv;
+  A_inv << 1.0, -0.00512149525437, 0.0, 2.72716136841e-05, 0.0, 1.04900794038, 0.0, -0.0110832091253, 0.0, 2.72716136841e-05, 1.0, -0.00512149525437, 0.0, -0.0110832091253, 0.0, 1.04900794038;
+  return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainLowHighPlantCoefficients());
 }
 
 StateFeedbackController<4, 2, 2> MakeDrivetrainHighLowController() {
   Eigen::Matrix<double, 4, 2> L;
-  L << 1.26735275667, 0.0994717602348, 37.3766125148, 7.14618746927, 0.0994717602348, 0.745943846725, 2.20133516364, 1.9922178483;
+  L << 1.25338805557, 0.0100729449137, 74.065541605, 2.10150476024, 0.0100729449137, 1.25338805557, 2.10150476024, 74.065541605;
   Eigen::Matrix<double, 2, 4> K;
-  K << 135.799605924, 11.0272286082, 20.8480994015, 2.49474383494, -6.38397728263, -0.174173971495, 116.74766142, 4.04674599343;
-  return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainHighLowPlantCoefficients());
+  K << 158.010515913, 12.6294601573, 1.98944476188, 1.06520656374, 1.98944476188, 1.06520656374, 158.010515913, 12.6294601573;
+  Eigen::Matrix<double, 4, 4> A_inv;
+  A_inv << 1.0, -0.00512149525437, 0.0, 2.72716136841e-05, 0.0, 1.04900794038, 0.0, -0.0110832091253, 0.0, 2.72716136841e-05, 1.0, -0.00512149525437, 0.0, -0.0110832091253, 0.0, 1.04900794038;
+  return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainHighLowPlantCoefficients());
 }
 
 StateFeedbackController<4, 2, 2> MakeDrivetrainHighHighController() {
   Eigen::Matrix<double, 4, 2> L;
-  L << 1.22020283663, 0.0195951135, 33.6437810428, 1.9756574066, 0.0195951135, 1.22020283663, 1.9756574066, 33.6437810428;
+  L << 1.25338805557, 0.0100729449137, 74.065541605, 2.10150476024, 0.0100729449137, 1.25338805557, 2.10150476024, 74.065541605;
   Eigen::Matrix<double, 2, 4> K;
-  K << 136.125709167, 11.0402255404, 6.0764292977, 1.24911698877, 6.0764292977, 1.24911698877, 136.125709167, 11.0402255404;
-  return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainHighHighPlantCoefficients());
+  K << 158.010515913, 12.6294601573, 1.98944476188, 1.06520656374, 1.98944476188, 1.06520656374, 158.010515913, 12.6294601573;
+  Eigen::Matrix<double, 4, 4> A_inv;
+  A_inv << 1.0, -0.00512149525437, 0.0, 2.72716136841e-05, 0.0, 1.04900794038, 0.0, -0.0110832091253, 0.0, 2.72716136841e-05, 1.0, -0.00512149525437, 0.0, -0.0110832091253, 0.0, 1.04900794038;
+  return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainHighHighPlantCoefficients());
 }
 
 StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant() {
-  ::std::vector<StateFeedbackPlantCoefficients<4, 2, 2> *> plants(4);
-  plants[0] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainLowLowPlantCoefficients());
-  plants[1] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainLowHighPlantCoefficients());
-  plants[2] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainHighLowPlantCoefficients());
-  plants[3] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainHighHighPlantCoefficients());
-  return StateFeedbackPlant<4, 2, 2>(plants);
+  ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>> plants(4);
+  plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>(new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainLowLowPlantCoefficients()));
+  plants[1] = ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>(new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainLowHighPlantCoefficients()));
+  plants[2] = ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>(new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainHighLowPlantCoefficients()));
+  plants[3] = ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>(new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainHighHighPlantCoefficients()));
+  return StateFeedbackPlant<4, 2, 2>(&plants);
 }
 
 StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop() {
-  ::std::vector<StateFeedbackController<4, 2, 2> *> controllers(4);
-  controllers[0] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainLowLowController());
-  controllers[1] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainLowHighController());
-  controllers[2] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainHighLowController());
-  controllers[3] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainHighHighController());
-  return StateFeedbackLoop<4, 2, 2>(controllers);
+  ::std::vector< ::std::unique_ptr<StateFeedbackController<4, 2, 2>>> controllers(4);
+  controllers[0] = ::std::unique_ptr<StateFeedbackController<4, 2, 2>>(new StateFeedbackController<4, 2, 2>(MakeDrivetrainLowLowController()));
+  controllers[1] = ::std::unique_ptr<StateFeedbackController<4, 2, 2>>(new StateFeedbackController<4, 2, 2>(MakeDrivetrainLowHighController()));
+  controllers[2] = ::std::unique_ptr<StateFeedbackController<4, 2, 2>>(new StateFeedbackController<4, 2, 2>(MakeDrivetrainHighLowController()));
+  controllers[3] = ::std::unique_ptr<StateFeedbackController<4, 2, 2>>(new StateFeedbackController<4, 2, 2>(MakeDrivetrainHighHighController()));
+  return StateFeedbackLoop<4, 2, 2>(&controllers);
 }
 
 }  // namespace control_loops
-}  // namespace bot3
+}  // namespace y2014_bot3
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h b/y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h
index f5d5896..03322ed 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h
@@ -1,9 +1,9 @@
-#ifndef BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
-#define BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
+#ifndef Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
+#define Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
 
 #include "frc971/control_loops/state_feedback_loop.h"
 
-namespace bot3 {
+namespace y2014_bot3 {
 namespace control_loops {
 
 StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowLowPlantCoefficients();
@@ -27,6 +27,6 @@
 StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop();
 
 }  // namespace control_loops
-}  // namespace bot3
+}  // namespace y2014_bot3
 
-#endif  // BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
+#endif  // Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain_lib_test.cc b/y2014_bot3/control_loops/drivetrain/drivetrain_lib_test.cc
index 89e41c6..2c04831 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -8,15 +8,14 @@
 #include "aos/common/controls/polytope.h"
 #include "aos/common/controls/control_loop_test.h"
 
-#include "bot3/control_loops/drivetrain/drivetrain.q.h"
-#include "bot3/control_loops/drivetrain/drivetrain.h"
-#include "bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.h"
 #include "frc971/control_loops/state_feedback_loop.h"
 #include "frc971/control_loops/coerce_goal.h"
-#include "frc971/queues/other_sensors.q.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "frc971/queues/gyro.q.h"
 
-
-namespace bot3 {
+namespace y2014_bot3 {
 namespace control_loops {
 namespace testing {
 
@@ -49,11 +48,11 @@
   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") {
+        my_drivetrain_queue_(".y2014_bot3.control_loops.drivetrain",
+                       0x8a8dde77, ".y2014_bot3.control_loops.drivetrain.goal",
+                       ".y2014_bot3.control_loops.drivetrain.position",
+                       ".y2014_bot3.control_loops.drivetrain.output",
+                       ".y2014_bot3.control_loops.drivetrain.status") {
     Reinitialize();
   }
 
@@ -76,8 +75,8 @@
     const double left_encoder = GetLeftPosition();
     const double right_encoder = GetRightPosition();
 
-    ::aos::ScopedMessagePtr<control_loops::Drivetrain::Position> position =
-        my_drivetrain_loop_.position.MakeMessage();
+    ::aos::ScopedMessagePtr<control_loops::DrivetrainQueue::Position> position =
+        my_drivetrain_queue_.position.MakeMessage();
     position->left_encoder = left_encoder;
     position->right_encoder = right_encoder;
     position.Send();
@@ -87,15 +86,15 @@
   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_->mutable_U() << my_drivetrain_loop_.output->left_voltage,
-        my_drivetrain_loop_.output->right_voltage;
+    EXPECT_TRUE(my_drivetrain_queue_.output.FetchLatest());
+    drivetrain_plant_->mutable_U() << my_drivetrain_queue_.output->left_voltage,
+        my_drivetrain_queue_.output->right_voltage;
     drivetrain_plant_->Update();
   }
 
   ::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> drivetrain_plant_;
  private:
-  Drivetrain my_drivetrain_loop_;
+  DrivetrainQueue my_drivetrain_queue_;
   double last_left_position_;
   double last_right_position_;
 };
@@ -105,30 +104,30 @@
   // 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_;
+  DrivetrainQueue my_drivetrain_queue_;
 
   // Create a loop and simulation plant.
   DrivetrainLoop drivetrain_motor_;
   DrivetrainSimulation drivetrain_motor_plant_;
 
-  DrivetrainTest() : my_drivetrain_loop_(".frc971.control_loops.drivetrain",
+  DrivetrainTest() : my_drivetrain_queue_(".y2014_bot3.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_),
+                               ".y2014_bot3.control_loops.drivetrain.goal",
+                               ".y2014_bot3.control_loops.drivetrain.position",
+                               ".y2014_bot3.control_loops.drivetrain.output",
+                               ".y2014_bot3.control_loops.drivetrain.status"),
+                drivetrain_motor_(&my_drivetrain_queue_),
                 drivetrain_motor_plant_() {
     ::frc971::sensors::gyro_reading.Clear();
   }
 
   void VerifyNearGoal() {
-    my_drivetrain_loop_.goal.FetchLatest();
-    my_drivetrain_loop_.position.FetchLatest();
-    EXPECT_NEAR(my_drivetrain_loop_.goal->left_goal,
+    my_drivetrain_queue_.goal.FetchLatest();
+    my_drivetrain_queue_.position.FetchLatest();
+    EXPECT_NEAR(my_drivetrain_queue_.goal->left_goal,
                 drivetrain_motor_plant_.GetLeftPosition(),
                 1e-2);
-    EXPECT_NEAR(my_drivetrain_loop_.goal->right_goal,
+    EXPECT_NEAR(my_drivetrain_queue_.goal->right_goal,
                 drivetrain_motor_plant_.GetRightPosition(),
                 1e-2);
   }
@@ -140,7 +139,7 @@
 
 // Tests that the drivetrain converges on a goal.
 TEST_F(DrivetrainTest, ConvergesCorrectly) {
-  my_drivetrain_loop_.goal.MakeWithBuilder().control_loop_driving(true)
+  my_drivetrain_queue_.goal.MakeWithBuilder().control_loop_driving(true)
       .left_goal(-1.0)
       .right_goal(1.0).Send();
   for (int i = 0; i < 200; ++i) {
@@ -154,7 +153,7 @@
 
 // Tests that it survives disabling.
 TEST_F(DrivetrainTest, SurvivesDisabling) {
-  my_drivetrain_loop_.goal.MakeWithBuilder().control_loop_driving(true)
+  my_drivetrain_queue_.goal.MakeWithBuilder().control_loop_driving(true)
       .left_goal(-1.0)
       .right_goal(1.0).Send();
   for (int i = 0; i < 500; ++i) {
@@ -170,21 +169,24 @@
   VerifyNearGoal();
 }
 
-// Tests surviving bad positions.
-TEST_F(DrivetrainTest, SurvivesBadPosition) {
-  my_drivetrain_loop_.goal.MakeWithBuilder().control_loop_driving(true)
-      .left_goal(-1.0)
-      .right_goal(1.0).Send();
-  for (int i = 0; i < 500; ++i) {
-    if (i > 20 && i < 200) {
-    } else {
-      drivetrain_motor_plant_.SendPositionMessage();
-    }
+// Tests that never having a goal doesn't break.
+TEST_F(DrivetrainTest, NoGoalStart) {
+  for (int i = 0; i < 20; ++i) {
+    drivetrain_motor_plant_.SendPositionMessage();
+    drivetrain_motor_.Iterate();
+    drivetrain_motor_plant_.Simulate();
+  }
+}
+
+// Tests that never having a goal, but having driver's station messages, doesn't
+// break.
+TEST_F(DrivetrainTest, NoGoalWithRobotState) {
+  for (int i = 0; i < 20; ++i) {
+    drivetrain_motor_plant_.SendPositionMessage();
     drivetrain_motor_.Iterate();
     drivetrain_motor_plant_.Simulate();
     SimulateTimestep(true);
   }
-  VerifyNearGoal();
 }
 
 ::aos::controls::HPolytope<2> MakeBox(double x1_min, double x1_max,
@@ -291,4 +293,4 @@
 
 }  // namespace testing
 }  // namespace control_loops
-}  // namespace bot3
+}  // namespace y2014_bot3
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc b/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc
index f06acc0..588bc89 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc
@@ -1,10 +1,10 @@
-#include "bot3/control_loops/drivetrain/drivetrain.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.h"
 
 #include "aos/linux_code/init.h"
 
 int main() {
   ::aos::Init();
-  bot3::control_loops::DrivetrainLoop drivetrain;
+  y2014_bot3::control_loops::DrivetrainLoop drivetrain;
   drivetrain.Run();
   ::aos::Cleanup();
   return 0;
diff --git a/y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.cc b/y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.cc
index 1287483..75bae13 100644
--- a/y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.cc
+++ b/y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.cc
@@ -1,17 +1,17 @@
-#include "frc971/control_loops/drivetrain/polydrivetrain_cim_plant.h"
+#include "y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.h"
 
 #include <vector>
 
 #include "frc971/control_loops/state_feedback_loop.h"
 
-namespace frc971 {
+namespace y2014_bot3 {
 namespace control_loops {
 
 StateFeedbackPlantCoefficients<1, 1, 1> MakeCIMPlantCoefficients() {
   Eigen::Matrix<double, 1, 1> A;
-  A << 0.614537580221;
+  A << 0.783924473544;
   Eigen::Matrix<double, 1, 1> B;
-  B << 15.9657598852;
+  B << 8.94979586973;
   Eigen::Matrix<double, 1, 1> C;
   C << 1;
   Eigen::Matrix<double, 1, 1> D;
@@ -25,23 +25,25 @@
 
 StateFeedbackController<1, 1, 1> MakeCIMController() {
   Eigen::Matrix<double, 1, 1> L;
-  L << 0.604537580221;
+  L << 0.773924473544;
   Eigen::Matrix<double, 1, 1> K;
-  K << 0.0378646293422;
-  return StateFeedbackController<1, 1, 1>(L, K, MakeCIMPlantCoefficients());
+  K << 0.086473980503;
+  Eigen::Matrix<double, 1, 1> A_inv;
+  A_inv << 1.2756330919;
+  return StateFeedbackController<1, 1, 1>(L, K, A_inv, MakeCIMPlantCoefficients());
 }
 
 StateFeedbackPlant<1, 1, 1> MakeCIMPlant() {
-  ::std::vector<StateFeedbackPlantCoefficients<1, 1, 1> *> plants(1);
-  plants[0] = new StateFeedbackPlantCoefficients<1, 1, 1>(MakeCIMPlantCoefficients());
-  return StateFeedbackPlant<1, 1, 1>(plants);
+  ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<1, 1, 1>>> plants(1);
+  plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<1, 1, 1>>(new StateFeedbackPlantCoefficients<1, 1, 1>(MakeCIMPlantCoefficients()));
+  return StateFeedbackPlant<1, 1, 1>(&plants);
 }
 
 StateFeedbackLoop<1, 1, 1> MakeCIMLoop() {
-  ::std::vector<StateFeedbackController<1, 1, 1> *> controllers(1);
-  controllers[0] = new StateFeedbackController<1, 1, 1>(MakeCIMController());
-  return StateFeedbackLoop<1, 1, 1>(controllers);
+  ::std::vector< ::std::unique_ptr<StateFeedbackController<1, 1, 1>>> controllers(1);
+  controllers[0] = ::std::unique_ptr<StateFeedbackController<1, 1, 1>>(new StateFeedbackController<1, 1, 1>(MakeCIMController()));
+  return StateFeedbackLoop<1, 1, 1>(&controllers);
 }
 
 }  // namespace control_loops
-}  // namespace frc971
+}  // namespace y2014_bot3
diff --git a/y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.h b/y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.h
index 12b2c59..0def071 100644
--- a/y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.h
+++ b/y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.h
@@ -1,9 +1,9 @@
-#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CIM_PLANT_H_
-#define FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CIM_PLANT_H_
+#ifndef Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CIM_PLANT_H_
+#define Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CIM_PLANT_H_
 
 #include "frc971/control_loops/state_feedback_loop.h"
 
-namespace frc971 {
+namespace y2014_bot3 {
 namespace control_loops {
 
 StateFeedbackPlantCoefficients<1, 1, 1> MakeCIMPlantCoefficients();
@@ -15,6 +15,6 @@
 StateFeedbackLoop<1, 1, 1> MakeCIMLoop();
 
 }  // namespace control_loops
-}  // namespace frc971
+}  // namespace y2014_bot3
 
-#endif  // FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CIM_PLANT_H_
+#endif  // Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CIM_PLANT_H_
diff --git a/y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc b/y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
index 540741c..8fc4049 100644
--- a/y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
+++ b/y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
@@ -1,17 +1,17 @@
-#include "bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+#include "y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
 
 #include <vector>
 
 #include "frc971/control_loops/state_feedback_loop.h"
 
-namespace bot3 {
+namespace y2014_bot3 {
 namespace control_loops {
 
 StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowLowPlantCoefficients() {
   Eigen::Matrix<double, 2, 2> A;
-  A << 0.498791664199, 0.0897184892451, 0.0897184892451, 0.498791664199;
+  A << 0.953388055571, 0.0100729449137, 0.0100729449137, 0.953388055571;
   Eigen::Matrix<double, 2, 2> B;
-  B << 0.0720868114296, -0.0129038552514, -0.0129038552514, 0.0720868114296;
+  B << 0.0119133285199, -0.00257449680311, -0.00257449680311, 0.0119133285199;
   Eigen::Matrix<double, 2, 2> C;
   C << 1.0, 0.0, 0.0, 1.0;
   Eigen::Matrix<double, 2, 2> D;
@@ -25,9 +25,9 @@
 
 StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowHighPlantCoefficients() {
   Eigen::Matrix<double, 2, 2> A;
-  A << 0.491841728751, 0.0145551608494, 0.124279814134, 0.921454874639;
+  A << 0.953388055571, 0.0100729449137, 0.0100729449137, 0.953388055571;
   Eigen::Matrix<double, 2, 2> B;
-  B << 0.0730863931411, -0.00611711060145, -0.0178746738354, 0.0330102308046;
+  B << 0.0119133285199, -0.00257449680311, -0.00257449680311, 0.0119133285199;
   Eigen::Matrix<double, 2, 2> C;
   C << 1.0, 0.0, 0.0, 1.0;
   Eigen::Matrix<double, 2, 2> D;
@@ -41,9 +41,9 @@
 
 StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighLowPlantCoefficients() {
   Eigen::Matrix<double, 2, 2> A;
-  A << 0.921454874639, 0.124279814134, 0.0145551608494, 0.491841728751;
+  A << 0.953388055571, 0.0100729449137, 0.0100729449137, 0.953388055571;
   Eigen::Matrix<double, 2, 2> B;
-  B << 0.0330102308046, -0.0178746738354, -0.00611711060145, 0.0730863931411;
+  B << 0.0119133285199, -0.00257449680311, -0.00257449680311, 0.0119133285199;
   Eigen::Matrix<double, 2, 2> C;
   C << 1.0, 0.0, 0.0, 1.0;
   Eigen::Matrix<double, 2, 2> D;
@@ -57,9 +57,9 @@
 
 StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighHighPlantCoefficients() {
   Eigen::Matrix<double, 2, 2> A;
-  A << 0.920202836632, 0.0195951135, 0.0195951135, 0.920202836632;
+  A << 0.953388055571, 0.0100729449137, 0.0100729449137, 0.953388055571;
   Eigen::Matrix<double, 2, 2> B;
-  B << 0.0335364259492, -0.00823525605573, -0.00823525605573, 0.0335364259492;
+  B << 0.0119133285199, -0.00257449680311, -0.00257449680311, 0.0119133285199;
   Eigen::Matrix<double, 2, 2> C;
   C << 1.0, 0.0, 0.0, 1.0;
   Eigen::Matrix<double, 2, 2> D;
@@ -73,53 +73,61 @@
 
 StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowLowController() {
   Eigen::Matrix<double, 2, 2> L;
-  L << 0.478791664199, 0.0897184892451, 0.0897184892451, 0.478791664199;
+  L << 0.933388055571, 0.0100729449137, 0.0100729449137, 0.933388055571;
   Eigen::Matrix<double, 2, 2> K;
-  K << -1.22029287467, 1.0261517351, 1.0261517351, -1.22029287467;
-  return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowLowPlantCoefficients());
+  K << 31.3080614945, 7.61126069778, 7.61126069778, 31.3080614945;
+  Eigen::Matrix<double, 2, 2> A_inv;
+  A_inv << 1.04900794038, -0.0110832091253, -0.0110832091253, 1.04900794038;
+  return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainLowLowPlantCoefficients());
 }
 
 StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController() {
   Eigen::Matrix<double, 2, 2> L;
-  L << 0.450790078471, 0.12008012565, 0.12008012565, 0.922506524919;
+  L << 0.933388055571, 0.0100729449137, 0.0100729449137, 0.933388055571;
   Eigen::Matrix<double, 2, 2> K;
-  K << -1.22005303899, 1.06234079364, 3.10424257883, 10.3132835345;
-  return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowHighPlantCoefficients());
+  K << 31.3080614945, 7.61126069778, 7.61126069778, 31.3080614945;
+  Eigen::Matrix<double, 2, 2> A_inv;
+  A_inv << 1.04900794038, -0.0110832091253, -0.0110832091253, 1.04900794038;
+  return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainLowHighPlantCoefficients());
 }
 
 StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController() {
   Eigen::Matrix<double, 2, 2> L;
-  L << 0.918551885829, 0.0172871978996, 0.0172871978996, 0.454744717561;
+  L << 0.933388055571, 0.0100729449137, 0.0100729449137, 0.933388055571;
   Eigen::Matrix<double, 2, 2> K;
-  K << 10.3132835345, 3.10424257883, 1.06234079364, -1.22005303899;
-  return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighLowPlantCoefficients());
+  K << 31.3080614945, 7.61126069778, 7.61126069778, 31.3080614945;
+  Eigen::Matrix<double, 2, 2> A_inv;
+  A_inv << 1.04900794038, -0.0110832091253, -0.0110832091253, 1.04900794038;
+  return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainHighLowPlantCoefficients());
 }
 
 StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController() {
   Eigen::Matrix<double, 2, 2> L;
-  L << 0.900202836632, 0.0195951135, 0.0195951135, 0.900202836632;
+  L << 0.933388055571, 0.0100729449137, 0.0100729449137, 0.933388055571;
   Eigen::Matrix<double, 2, 2> K;
-  K << 10.3132878272, 3.11684016931, 3.11684016931, 10.3132878272;
-  return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighHighPlantCoefficients());
+  K << 31.3080614945, 7.61126069778, 7.61126069778, 31.3080614945;
+  Eigen::Matrix<double, 2, 2> A_inv;
+  A_inv << 1.04900794038, -0.0110832091253, -0.0110832091253, 1.04900794038;
+  return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainHighHighPlantCoefficients());
 }
 
 StateFeedbackPlant<2, 2, 2> MakeVelocityDrivetrainPlant() {
-  ::std::vector<StateFeedbackPlantCoefficients<2, 2, 2> *> plants(4);
-  plants[0] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowLowPlantCoefficients());
-  plants[1] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowHighPlantCoefficients());
-  plants[2] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighLowPlantCoefficients());
-  plants[3] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighHighPlantCoefficients());
-  return StateFeedbackPlant<2, 2, 2>(plants);
+  ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 2, 2>>> plants(4);
+  plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 2, 2>>(new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowLowPlantCoefficients()));
+  plants[1] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 2, 2>>(new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowHighPlantCoefficients()));
+  plants[2] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 2, 2>>(new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighLowPlantCoefficients()));
+  plants[3] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 2, 2>>(new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighHighPlantCoefficients()));
+  return StateFeedbackPlant<2, 2, 2>(&plants);
 }
 
 StateFeedbackLoop<2, 2, 2> MakeVelocityDrivetrainLoop() {
-  ::std::vector<StateFeedbackController<2, 2, 2> *> controllers(4);
-  controllers[0] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowLowController());
-  controllers[1] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowHighController());
-  controllers[2] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighLowController());
-  controllers[3] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighHighController());
-  return StateFeedbackLoop<2, 2, 2>(controllers);
+  ::std::vector< ::std::unique_ptr<StateFeedbackController<2, 2, 2>>> controllers(4);
+  controllers[0] = ::std::unique_ptr<StateFeedbackController<2, 2, 2>>(new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowLowController()));
+  controllers[1] = ::std::unique_ptr<StateFeedbackController<2, 2, 2>>(new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowHighController()));
+  controllers[2] = ::std::unique_ptr<StateFeedbackController<2, 2, 2>>(new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighLowController()));
+  controllers[3] = ::std::unique_ptr<StateFeedbackController<2, 2, 2>>(new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighHighController()));
+  return StateFeedbackLoop<2, 2, 2>(&controllers);
 }
 
 }  // namespace control_loops
-}  // namespace bot3
+}  // namespace y2014_bot3
diff --git a/y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h b/y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h
index 191d1aa..4442d35 100644
--- a/y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h
+++ b/y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h
@@ -1,9 +1,9 @@
-#ifndef BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_DOG_MOTOR_PLANT_H_
-#define BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_DOG_MOTOR_PLANT_H_
+#ifndef Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_DOG_MOTOR_PLANT_H_
+#define Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_DOG_MOTOR_PLANT_H_
 
 #include "frc971/control_loops/state_feedback_loop.h"
 
-namespace bot3 {
+namespace y2014_bot3 {
 namespace control_loops {
 
 StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowLowPlantCoefficients();
@@ -27,6 +27,6 @@
 StateFeedbackLoop<2, 2, 2> MakeVelocityDrivetrainLoop();
 
 }  // namespace control_loops
-}  // namespace bot3
+}  // namespace y2014_bot3
 
-#endif  // BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_DOG_MOTOR_PLANT_H_
+#endif  // Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_DOG_MOTOR_PLANT_H_
diff --git a/y2014_bot3/control_loops/drivetrain/replay_drivetrain.cc b/y2014_bot3/control_loops/drivetrain/replay_drivetrain.cc
new file mode 100644
index 0000000..4f83c9e
--- /dev/null
+++ b/y2014_bot3/control_loops/drivetrain/replay_drivetrain.cc
@@ -0,0 +1,24 @@
+#include "aos/common/controls/replay_control_loop.h"
+#include "aos/linux_code/init.h"
+
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.q.h"
+
+// Reads one or more log files and sends out all the queue messages (in the
+// correct order and at the correct time) to feed a "live" drivetrain process.
+
+int main(int argc, char **argv) {
+  if (argc <= 1) {
+    fprintf(stderr, "Need at least one file to replay!\n");
+    return EXIT_FAILURE;
+  }
+
+  ::aos::InitNRT();
+
+  ::aos::controls::ControlLoopReplayer<::y2014_bot3::control_loops::DrivetrainQueue>
+      replayer(&::y2014_bot3::control_loops::drivetrain_queue, "drivetrain");
+  for (int i = 1; i < argc; ++i) {
+    replayer.ProcessFile(argv[i]);
+  }
+
+  ::aos::Cleanup();
+}
diff --git a/y2014_bot3/control_loops/python/drivetrain.py b/y2014_bot3/control_loops/python/drivetrain.py
index bc0df04..c0c4595 100755
--- a/y2014_bot3/control_loops/python/drivetrain.py
+++ b/y2014_bot3/control_loops/python/drivetrain.py
@@ -56,30 +56,33 @@
     # Stall Torque in N m
     self.stall_torque = 2.42
     # Stall Current in Amps
-    self.stall_current = 133
+    self.stall_current = 133.0
     # 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 = 4.5
+    self.J = 10
     # Mass of the robot, in kg.
-    self.m = 60
+    self.m = 68
     # Radius of the robot, in meters (from last year).
-    self.rb = 0.7112 / 2.0
+    self.rb = 0.9603 / 2.0
     # Radius of the wheels, in meters.
-    self.r = .04445
+    self.r = 0.0508
     # Resistance of the motor, divided by the number of motors.
-    self.R = 12.0 / self.stall_current / 4
+    self.R = 12.0 / self.stall_current / 2
     # 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 = 14.0 / 60.0 * 17.0 / 50.0
-    self.G_high = 30.0 / 44.0 * 17.0 / 50.0
+    self.G_const = 18.0 / 44.0 * 18.0 / 60.0
+
+    self.G_low = self.G_const
+    self.G_high = self.G_const
+
     if left_low:
       self.Gl = self.G_low
     else:
@@ -88,8 +91,9 @@
       self.Gr = self.G_low
     else:
       self.Gr = self.G_high
+
     # Control loop time step
-    self.dt = 0.01
+    self.dt = 0.005
 
     # These describe the way that a given side of a robot will be influenced
     # by the other side. Units of 1 / kg.
@@ -223,13 +227,13 @@
   drivetrain_high_low = Drivetrain(name="DrivetrainHighLow", left_low=False, right_low=True)
   drivetrain_high_high = Drivetrain(name="DrivetrainHighHigh", left_low=False, right_low=False)
 
-  if len(argv) != 3:
+  if len(argv) != 5:
     print "Expected .h file name and .cc file name"
   else:
     dog_loop_writer = control_loop.ControlLoopWriter(
         "Drivetrain", [drivetrain_low_low, drivetrain_low_high,
                        drivetrain_high_low, drivetrain_high_high],
-        namespaces = ["bot3", "control_loops"])
+        namespaces=['y2014_bot3', 'control_loops'])
     if argv[1][-3:] == '.cc':
       dog_loop_writer.Write(argv[2], argv[1])
     else:
diff --git a/y2014_bot3/control_loops/python/polydrivetrain.py b/y2014_bot3/control_loops/python/polydrivetrain.py
index 49d390b..2ffbed6 100755
--- a/y2014_bot3/control_loops/python/polydrivetrain.py
+++ b/y2014_bot3/control_loops/python/polydrivetrain.py
@@ -102,7 +102,7 @@
     super(VelocityDrivetrainModel, self).__init__(name)
     self._drivetrain = drivetrain.Drivetrain(left_low=left_low,
                                              right_low=right_low)
-    self.dt = 0.01
+    self.dt = 0.005
     self.A_continuous = numpy.matrix(
         [[self._drivetrain.A_continuous[1, 1], self._drivetrain.A_continuous[1, 3]],
          [self._drivetrain.A_continuous[3, 1], self._drivetrain.A_continuous[3, 3]]])
@@ -168,7 +168,7 @@
         [[-12.0000000000],
          [-12.0000000000]])
 
-    self.dt = 0.01
+    self.dt = 0.005
 
     self.R = numpy.matrix(
         [[0.0],
@@ -177,7 +177,7 @@
     # ttrust is the comprimise between having full throttle negative inertia,
     # and having no throttle negative inertia.  A value of 0 is full throttle
     # inertia.  A value of 1 is no throttle negative inertia.
-    self.ttrust = 1.0
+    self.ttrust = 1.1
 
     self.left_gear = VelocityDrivetrain.LOW
     self.right_gear = VelocityDrivetrain.LOW
@@ -302,26 +302,30 @@
     # This is the same as sending the most torque down to the floor at the
     # wheel.
 
-    self.left_gear = self.ComputeGear(self.X[0, 0], should_print=True,
-                                      current_gear=self.left_gear,
-                                      gear_name="left")
-    self.right_gear = self.ComputeGear(self.X[1, 0], should_print=True,
-                                       current_gear=self.right_gear,
-                                       gear_name="right")
-    if self.IsInGear(self.left_gear):
-      self.left_cim.X[0, 0] = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
+    self.left_gear = self.right_gear = True
+    if False:
+      self.left_gear = self.ComputeGear(self.X[0, 0], should_print=True,
+                                        current_gear=self.left_gear,
+                                        gear_name="left")
+      self.right_gear = self.ComputeGear(self.X[1, 0], should_print=True,
+                                         current_gear=self.right_gear,
+                                         gear_name="right")
+      if self.IsInGear(self.left_gear):
+        self.left_cim.X[0, 0] = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
 
-    if self.IsInGear(self.right_gear):
-      self.right_cim.X[0, 0] = self.MotorRPM(self.right_shifter_position, self.X[0, 0])
+      if self.IsInGear(self.right_gear):
+        self.right_cim.X[0, 0] = self.MotorRPM(self.right_shifter_position, self.X[0, 0])
 
-    if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+    steering *= 2.3
+    if True or self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
       # Filter the throttle to provide a nicer response.
       fvel = self.FilterVelocity(throttle)
 
       # Constant radius means that angualar_velocity / linear_velocity = constant.
       # Compute the left and right velocities.
-      left_velocity = fvel - steering * numpy.abs(fvel)
-      right_velocity = fvel + steering * numpy.abs(fvel)
+      steering_velocity = numpy.abs(fvel) * steering
+      left_velocity = fvel - steering_velocity
+      right_velocity = fvel + steering_velocity
 
       # Write this constraint in the form of K * R = w
       # angular velocity / linear velocity = constant
@@ -379,7 +383,7 @@
 
     # TODO(austin): Model the robot as not accelerating when you shift...
     # This hack only works when you shift at the same time.
-    if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+    if True or self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
       self.X = self.CurrentDrivetrain().A * self.X + self.CurrentDrivetrain().B * self.U
 
     self.left_gear, self.left_shifter_position = self.SimShifter(
@@ -394,7 +398,7 @@
 def main(argv):
   vdrivetrain = VelocityDrivetrain()
 
-  if len(argv) != 5:
+  if len(argv) != 7:
     print "Expected .h file name and .cc file name"
   else:
     dog_loop_writer = control_loop.ControlLoopWriter(
@@ -402,7 +406,7 @@
                        vdrivetrain.drivetrain_low_high,
                        vdrivetrain.drivetrain_high_low,
                        vdrivetrain.drivetrain_high_high],
-        namespaces = ["bot3", "control_loops"])
+        namespaces=['y2014_bot3', 'control_loops'])
 
     if argv[1][-3:] == '.cc':
       dog_loop_writer.Write(argv[2], argv[1])
@@ -410,12 +414,13 @@
       dog_loop_writer.Write(argv[1], argv[2])
 
     cim_writer = control_loop.ControlLoopWriter(
-        "CIM", [drivetrain.CIM()])
+        "CIM", [drivetrain.CIM()],
+        namespaces=['y2014_bot3', 'control_loops'])
 
-    if argv[3][-3:] == '.cc':
-      cim_writer.Write(argv[4], argv[3])
+    if argv[5][-3:] == '.cc':
+      cim_writer.Write(argv[6], argv[5])
     else:
-      cim_writer.Write(argv[3], argv[4])
+      cim_writer.Write(argv[5], argv[6])
     return
 
   vl_plot = []
@@ -442,13 +447,13 @@
   else:
     print "Right is low"
 
-  for t in numpy.arange(0, 4.0, vdrivetrain.dt):
-    if t < 1.0:
-      vdrivetrain.Update(throttle=1.00, steering=0.0)
+  for t in numpy.arange(0, 1.7, vdrivetrain.dt):
+    if t < 0.5:
+      vdrivetrain.Update(throttle=0.00, steering=1.0)
     elif t < 1.2:
-      vdrivetrain.Update(throttle=1.00, steering=0.0)
+      vdrivetrain.Update(throttle=0.5, steering=1.0)
     else:
-      vdrivetrain.Update(throttle=1.00, steering=0.0)
+      vdrivetrain.Update(throttle=0.00, steering=1.0)
     t_plot.append(t)
     vl_plot.append(vdrivetrain.X[0, 0])
     vr_plot.append(vdrivetrain.X[1, 0])
@@ -475,10 +480,10 @@
     cim_velocity_plot.append(cim.X[0, 0])
     cim_voltage_plot.append(U[0, 0] * 10)
     cim_time.append(t)
-  #pylab.plot(cim_time, cim_velocity_plot, label='cim spinup')
-  #pylab.plot(cim_time, cim_voltage_plot, label='cim voltage')
-  #pylab.legend()
-  #pylab.show()
+  pylab.plot(cim_time, cim_velocity_plot, label='cim spinup')
+  pylab.plot(cim_time, cim_voltage_plot, label='cim voltage')
+  pylab.legend()
+  pylab.show()
 
   # TODO(austin):
   # Shifting compensation.
diff --git a/y2014_bot3/control_loops/rollers/BUILD b/y2014_bot3/control_loops/rollers/BUILD
new file mode 100644
index 0000000..e3526ac
--- /dev/null
+++ b/y2014_bot3/control_loops/rollers/BUILD
@@ -0,0 +1,40 @@
+package(default_visibility = ['//visibility:public'])
+
+load('/aos/build/queues', 'queue_library')
+
+queue_library(
+  name = 'rollers_queue',
+  srcs = [
+    'rollers.q',
+  ],
+  deps = [
+    '//aos/common/controls:control_loop_queues',
+    '//frc971/control_loops:queues',
+  ],
+)
+
+cc_library(
+  name = 'rollers_lib',
+  hdrs = [
+    'rollers.h',
+  ],
+  srcs = [
+    'rollers.cc',
+  ],
+  deps = [
+    ':rollers_queue',
+    '//aos/common/logging',
+    '//aos/common/controls:control_loop',
+  ],
+)
+
+cc_binary(
+  name = 'rollers',
+  srcs = [
+    'rollers_main.cc',
+  ],
+  deps = [
+    ':rollers_lib',
+    '//aos/linux_code:init',
+  ],
+)
diff --git a/y2014_bot3/control_loops/rollers/rollers.cc b/y2014_bot3/control_loops/rollers/rollers.cc
index 8777549..ef895b3 100644
--- a/y2014_bot3/control_loops/rollers/rollers.cc
+++ b/y2014_bot3/control_loops/rollers/rollers.cc
@@ -1,40 +1,45 @@
-#include "bot3/control_loops/rollers/rollers.h"
-#include "bot3/control_loops/rollers/rollers.q.h"
+#include "y2014_bot3/control_loops/rollers/rollers.h"
 
-namespace bot3 {
+#include "aos/common/logging/logging.h"
+
+namespace y2014_bot3 {
 namespace control_loops {
 
-void RollersLoop::RunIteration(const Rollers::Goal *goal,
-                               const Rollers::Position * /*position*/,
-                               Rollers::Output *output,
-                               Rollers::Status * /*status*/) {
-  constexpr double kBot3IntakeForwardVoltage = 12.0;
-  constexpr double kBot3IntakeBackwardVoltage = -12.0;
-  constexpr double kBot3LowGoalForwardVoltage = 6.0;
-  constexpr double kBot3LowGoalBackwardVoltage = -6.0;
+Rollers::Rollers(control_loops::RollersQueue *rollers)
+    : aos::controls::ControlLoop<control_loops::RollersQueue>(rollers) {}
+
+void Rollers::RunIteration(
+    const control_loops::RollersQueue::Goal *goal,
+    const control_loops::RollersQueue::Position * /*position*/,
+    control_loops::RollersQueue::Output *output,
+    control_loops::RollersQueue::Status * /*status*/) {
+  constexpr double k2014Bot3IntakeForwardVoltage = 12.0;
+  constexpr double k2014Bot3IntakeBackwardVoltage = -12.0;
+  constexpr double k2014Bot3LowGoalForwardVoltage = 6.0;
+  constexpr double k2014Bot3LowGoalBackwardVoltage = -6.0;
+
+  if (!output || !goal) {
+    return;
+  }
 
   const int intake = goal->intake;
   const int low_spit = goal->low_spit;
   const bool human_player = goal->human_player;
 
-  if (!output) {
-    return;
-  }
-
   output->Zero();
 
   switch (low_spit) {
     case 1:
       // Spit towards front
-      output->low_goal_voltage = kBot3LowGoalBackwardVoltage;
-      output->front_intake_voltage = kBot3IntakeBackwardVoltage;
-      output->back_intake_voltage = -kBot3IntakeForwardVoltage;
+      output->low_goal_voltage = k2014Bot3LowGoalBackwardVoltage;
+      output->front_intake_voltage = k2014Bot3IntakeBackwardVoltage;
+      output->back_intake_voltage = -k2014Bot3IntakeForwardVoltage;
       break;
     case -1:
       // Spit towards back
-      output->low_goal_voltage = kBot3LowGoalForwardVoltage;
-      output->back_intake_voltage = -kBot3IntakeBackwardVoltage;
-      output->front_intake_voltage = kBot3IntakeForwardVoltage;
+      output->low_goal_voltage = k2014Bot3LowGoalForwardVoltage;
+      output->back_intake_voltage = -k2014Bot3IntakeBackwardVoltage;
+      output->front_intake_voltage = k2014Bot3IntakeForwardVoltage;
       break;
     default:
       // Stationary
@@ -46,14 +51,14 @@
       // Front intake.
       output->front_extended = true;
       output->back_extended = false;
-      output->front_intake_voltage = kBot3IntakeForwardVoltage;
+      output->front_intake_voltage = k2014Bot3IntakeForwardVoltage;
       output->back_intake_voltage = 0.0;
       break;
     case -1:
       // Back intake.
       output->back_extended = true;
       output->front_extended = false;
-      output->back_intake_voltage = -kBot3IntakeForwardVoltage;
+      output->back_intake_voltage = -k2014Bot3IntakeForwardVoltage;
       output->front_intake_voltage = 0.0;
       break;
     default:
@@ -65,10 +70,10 @@
     // Intake for human player.
     output->front_extended = false;
     output->back_extended = false;
-    output->front_intake_voltage = kBot3IntakeForwardVoltage;
-    output->back_intake_voltage = -kBot3IntakeForwardVoltage;
+    output->front_intake_voltage = k2014Bot3IntakeForwardVoltage;
+    output->back_intake_voltage = -k2014Bot3IntakeForwardVoltage;
   }
 }
 
 }  //  namespace control_loops
-}  //  namespace bot3
+}  //  namespace y2014_bot3
diff --git a/y2014_bot3/control_loops/rollers/rollers.gyp b/y2014_bot3/control_loops/rollers/rollers.gyp
deleted file mode 100644
index e865ed8..0000000
--- a/y2014_bot3/control_loops/rollers/rollers.gyp
+++ /dev/null
@@ -1,45 +0,0 @@
-{
-  'targets': [
-    {
-      'target_name': 'rollers_loop',
-      'type': 'static_library',
-      'sources': ['rollers.q'],
-      'variables': {
-        'header_path': 'bot3/control_loops/rollers',
-      },
-      'dependencies': [
-        '<(AOS)/common/controls/controls.gyp:control_loop_queues',
-      ],
-      'export_dependent_settings': [
-        '<(AOS)/common/controls/controls.gyp:control_loop_queues',
-      ],
-      'includes': ['../../../aos/build/queues.gypi'],
-    },
-    {
-      'target_name': 'rollers_lib',
-      'type': 'static_library',
-      'sources': [
-        'rollers.cc',
-      ],
-      'dependencies': [
-        'rollers_loop',
-        '<(AOS)/common/controls/controls.gyp:control_loop',
-      ],
-      'export_dependent_settings': [
-        'rollers_loop',
-        '<(AOS)/common/controls/controls.gyp:control_loop',
-      ],
-    },
-    {
-      'target_name': 'rollers',
-      'type': 'executable',
-      'sources': [
-        'rollers_main.cc',
-      ],
-      'dependencies': [
-        'rollers_lib',
-        '<(AOS)/linux_code/linux_code.gyp:init'
-      ],
-    },
-  ],
-}
diff --git a/y2014_bot3/control_loops/rollers/rollers.h b/y2014_bot3/control_loops/rollers/rollers.h
index f67b372..f0cae13 100644
--- a/y2014_bot3/control_loops/rollers/rollers.h
+++ b/y2014_bot3/control_loops/rollers/rollers.h
@@ -1,33 +1,29 @@
-#ifndef BOT3_CONTROL_LOOPS_ROLLERS_H_
-#define BOT3_CONTROL_LOOPS_ROLLERS_H_
+#ifndef Y2014_BOT3_CONTROL_LOOPS_ROLLERS_H_
+#define Y2014_BOT3_CONTROL_LOOPS_ROLLERS_H_
 
 #include "aos/common/controls/control_loop.h"
-#include "bot3/control_loops/rollers/rollers.q.h"
 
-namespace bot3 {
+#include "y2014_bot3/control_loops/rollers/rollers.q.h"
+
+namespace y2014_bot3 {
 namespace control_loops {
 
-class RollersLoop
-  : public aos::controls::ControlLoop<control_loops::Rollers,
-      false, false, true> {
+class Rollers : public aos::controls::ControlLoop<control_loops::RollersQueue> {
  public:
   // Constructs a control loops which can take a rollers or defaults to the
-  // rollers at ::bot3::control_loops::rollers.
-  explicit RollersLoop(
-      control_loops::Rollers *my_rollers = &control_loops::rollers)
-      : aos::controls::ControlLoop<control_loops::Rollers, false, false, true>(
-          my_rollers) {}
+  // rollers at ::2014_bot3::control_loops::rollers.
+  explicit Rollers(control_loops::RollersQueue *rollers_queue =
+                       &control_loops::rollers_queue);
 
  protected:
   // Executes one cycle of the control loop.
-  virtual void RunIteration(
-      const control_loops::Rollers::Goal *goal,
-      const control_loops::Rollers::Position *position,
-      control_loops::Rollers::Output *output,
-      control_loops::Rollers::Status *status);
+  void RunIteration(const control_loops::RollersQueue::Goal *goal,
+                    const control_loops::RollersQueue::Position *position,
+                    control_loops::RollersQueue::Output *output,
+                    control_loops::RollersQueue::Status *status) override;
 };
 
 }  // namespace control_loops
-}  // namespace bot3
+}  // namespace y2014_bot3
 
-#endif  // BOT3_CONTROL_LOOPS_ROLLERS_H_
+#endif  // Y2014_BOT3_CONTROL_LOOPS_ROLLERS_H_
diff --git a/y2014_bot3/control_loops/rollers/rollers.q b/y2014_bot3/control_loops/rollers/rollers.q
index 5abb488..f218a41 100644
--- a/y2014_bot3/control_loops/rollers/rollers.q
+++ b/y2014_bot3/control_loops/rollers/rollers.q
@@ -1,8 +1,9 @@
-package bot3.control_loops;
+package y2014_bot3.control_loops;
 
 import "aos/common/controls/control_loops.q";
+import "frc971/control_loops/control_loops.q";
 
-queue_group Rollers {
+queue_group RollersQueue {
   implements aos.control_loops.ControlLoop;
 
   message Goal {
@@ -37,4 +38,4 @@
   queue Status status;
 };
 
-queue_group Rollers rollers;
+queue_group RollersQueue rollers_queue;
diff --git a/y2014_bot3/control_loops/rollers/rollers_main.cc b/y2014_bot3/control_loops/rollers/rollers_main.cc
index 75d2cc6..4700a6e 100644
--- a/y2014_bot3/control_loops/rollers/rollers_main.cc
+++ b/y2014_bot3/control_loops/rollers/rollers_main.cc
@@ -1,10 +1,10 @@
-#include "bot3/control_loops/rollers/rollers.h"
+#include "y2014_bot3/control_loops/rollers/rollers.h"
 
 #include "aos/linux_code/init.h"
 
 int main() {
   ::aos::Init();
-  bot3::control_loops::RollersLoop rollers;
+  ::y2014_bot3::control_loops::Rollers rollers;
   rollers.Run();
   ::aos::Cleanup();
   return 0;
diff --git a/y2014_bot3/control_loops/update_drivetrain.sh b/y2014_bot3/control_loops/update_drivetrain.sh
index 91ff838..b5757d3 100755
--- a/y2014_bot3/control_loops/update_drivetrain.sh
+++ b/y2014_bot3/control_loops/update_drivetrain.sh
@@ -1,9 +1,12 @@
 #!/bin/bash
 #
-# Updates the drivetrain controllers for the third robot.
+# Updates the drivetrain controllers.
 
 cd $(dirname $0)
 
-PYTHONPATH=$PYTHONPATH:../../frc971/control_loops/python \
+export PYTHONPATH=../../frc971/control_loops/python
+
 ./python/drivetrain.py drivetrain/drivetrain_dog_motor_plant.h \
-    drivetrain/drivetrain_dog_motor_plant.cc
+    drivetrain/drivetrain_dog_motor_plant.cc \
+    drivetrain/drivetrain_clutch_motor_plant.h \
+    drivetrain/drivetrain_clutch_motor_plant.cc
diff --git a/y2014_bot3/control_loops/update_polydrivetrain.sh b/y2014_bot3/control_loops/update_polydrivetrain.sh
index d8e2d7a..b1bb2b4 100755
--- a/y2014_bot3/control_loops/update_polydrivetrain.sh
+++ b/y2014_bot3/control_loops/update_polydrivetrain.sh
@@ -1,11 +1,14 @@
 #!/bin/bash
 #
-# Updates the polydrivetrain controllers and CIM models for the third robot.
+# Updates the polydrivetrain controllers and CIM models.
 
 cd $(dirname $0)
 
-PYTHONPATH=$PYTHONPATH:../../frc971/control_loops/python \
+export PYTHONPATH=../../frc971/control_loops/python
+
 ./python/polydrivetrain.py drivetrain/polydrivetrain_dog_motor_plant.h \
     drivetrain/polydrivetrain_dog_motor_plant.cc \
+    drivetrain/polydrivetrain_clutch_motor_plant.h \
+    drivetrain/polydrivetrain_clutch_motor_plant.cc \
     drivetrain/polydrivetrain_cim_plant.h \
     drivetrain/polydrivetrain_cim_plant.cc