diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 092f66e..c92b45e 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -26,43 +26,44 @@
 namespace control_loops {
 
 // Width of the robot.
-const double width = 22.0 / 100.0 * 2.54;
+const double width = 25.0 / 100.0 * 2.54;
 
 class DrivetrainMotorsSS {
  public:
   DrivetrainMotorsSS()
       : loop_(new StateFeedbackLoop<4, 2, 2>(
-            constants::GetValues().make_drivetrain_loop())) {
-    _offset = 0;
-    _integral_offset = 0;
-    _left_goal = 0.0;
-    _right_goal = 0.0;
-    _raw_left = 0.0;
-    _raw_right = 0.0;
-    _control_loop_driving = false;
+            constants::GetValues().make_drivetrain_loop())),
+        filtered_offset_(0.0),
+        gyro_(0.0),
+        left_goal_(0.0),
+        right_goal_(0.0),
+        raw_left_(0.0),
+        raw_right_(0.0),
+        control_loop_driving_(false) {
+    // High gear on both.
+    loop_->set_controller_index(3);
   }
-  void SetGoal(double left, double left_velocity, double right, double right_velocity) {
-    _left_goal = left;
-    _right_goal = right;
+
+  void SetGoal(double left, double left_velocity, double right,
+               double right_velocity) {
+    left_goal_ = left;
+    right_goal_ = right;
     loop_->R << left, left_velocity, right, right_velocity;
   }
   void SetRawPosition(double left, double right) {
-    _raw_right = right;
-    _raw_left = left;
+    raw_right_ = right;
+    raw_left_ = left;
     Eigen::Matrix<double, 2, 1> Y;
-    Y << left, right;
+    Y << left + filtered_offset_, right - filtered_offset_;
     loop_->Correct(Y);
   }
   void SetPosition(
       double left, double right, double gyro, bool control_loop_driving) {
     // Decay the offset quickly because this gyro is great.
-    _offset = (0.25) * (right - left - gyro * width) / 2.0 + 0.75 * _offset;
-    //const double angle_error = (_right_goal - _left_goal) / width - (_raw_right - _offset - _raw_left - _offset) / width;
-    // TODO(aschuh): Add in the gyro.
-    _integral_offset = 0.0;
-    _offset = 0.0;
-    _gyro = gyro;
-    _control_loop_driving = control_loop_driving;
+    const double offset = (right - left - gyro * width) / 2.0;
+    filtered_offset_ = 0.25 * offset + 0.75 * filtered_offset_;
+    gyro_ = gyro;
+    control_loop_driving_ = control_loop_driving;
     SetRawPosition(left, right);
   }
 
@@ -71,7 +72,7 @@
   }
 
   void Update(bool stop_motors) {
-    if (_control_loop_driving) {
+    if (control_loop_driving_) {
       loop_->Update(stop_motors);
     } else {
       if (stop_motors) {
@@ -102,20 +103,21 @@
     if (output) {
       output->left_voltage = loop_->U(0, 0);
       output->right_voltage = loop_->U(1, 0);
+      output->left_high = true;
+      output->right_high = true;
     }
   }
 
  private:
   ::std::unique_ptr<StateFeedbackLoop<4, 2, 2>> loop_;
 
-  double _integral_offset;
-  double _offset;
-  double _gyro;
-  double _left_goal;
-  double _right_goal;
-  double _raw_left;
-  double _raw_right;
-  bool _control_loop_driving;
+  double filtered_offset_;
+  double gyro_;
+  double left_goal_;
+  double right_goal_;
+  double raw_left_;
+  double raw_right_;
+  bool control_loop_driving_;
 };
 
 class PolyDrivetrain {
diff --git a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
index 231eefb..b254fd7 100644
--- a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
@@ -7,7 +7,7 @@
 namespace frc971 {
 namespace control_loops {
 
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainPlantCoefficients() {
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowLowPlantCoefficients() {
   Eigen::Matrix<double, 4, 4> A;
   A << 1.0, 0.00860955515291, 0.0, 0.000228184998733, 0.0, 0.735841675858, 0.0, 0.0410810558113, 0.0, 0.000228184998733, 1.0, 0.00860955515291, 0.0, 0.0410810558113, 0.0, 0.735841675858;
   Eigen::Matrix<double, 4, 2> B;
@@ -23,23 +23,101 @@
   return StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
 }
 
-StateFeedbackController<4, 2, 2> MakeDrivetrainController() {
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowHighPlantCoefficients() {
+  Eigen::Matrix<double, 4, 4> A;
+  A << 1.0, 0.00860667098456, 0.0, 7.04111872002e-05, 0.0, 0.735048848179, 0.0, 0.0131811893199, 0.0, 0.000245343870066, 1.0, 0.00957169266049, 0.0, 0.045929121897, 0.0, 0.915703853642;
+  Eigen::Matrix<double, 4, 2> B;
+  B << 0.000272809358971, -2.57343985847e-05, 0.0518765869984, -0.00481755802263, -4.80375440247e-05, 0.00015654091672, -0.00899277497558, 0.0308091755839;
+  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 StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighLowPlantCoefficients() {
+  Eigen::Matrix<double, 4, 4> A;
+  A << 1.0, 0.00957169266049, 0.0, 0.000245343870066, 0.0, 0.915703853642, 0.0, 0.045929121897, 0.0, 7.04111872002e-05, 1.0, 0.00860667098456, 0.0, 0.0131811893199, 0.0, 0.735048848179;
+  Eigen::Matrix<double, 4, 2> B;
+  B << 0.00015654091672, -4.80375440247e-05, 0.0308091755839, -0.00899277497558, -2.57343985847e-05, 0.000272809358971, -0.00481755802263, 0.0518765869984;
+  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 StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighHighPlantCoefficients() {
+  Eigen::Matrix<double, 4, 4> A;
+  A << 1.0, 0.00957076892085, 0.0, 7.56192087769e-05, 0.0, 0.915439806567, 0.0, 0.0146814193986, 0.0, 7.56192087769e-05, 1.0, 0.00957076892085, 0.0, 0.0146814193986, 0.0, 0.915439806567;
+  Eigen::Matrix<double, 4, 2> B;
+  B << 0.000156878531877, -2.76378646165e-05, 0.0309056814511, -0.00536587314624, -2.76378646165e-05, 0.000156878531877, -0.00536587314624, 0.0309056814511;
+  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 StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<4, 2, 2> MakeDrivetrainLowLowController() {
   Eigen::Matrix<double, 4, 2> L;
-  L << 1.57584167586, 0.0410810558113, 50.0130674801, 4.93325200717, 0.0410810558113, 1.57584167586, 4.93325200717, 50.0130674801;
+  L << 1.46584167586, 0.0410810558113, 41.9788073691, 4.62131736499, 0.0410810558113, 1.46584167586, 4.62131736499, 41.9788073691;
   Eigen::Matrix<double, 2, 4> K;
-  K << 128.210620632, 6.93828382074, 5.11036686771, 0.729493080206, 5.1103668677, 0.729493080206, 128.210620632, 6.93828382074;
-  return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainPlantCoefficients());
+  K << 128.210620632, 6.93828382074, 5.1103668677, 0.729493080206, 5.1103668677, 0.729493080206, 128.210620632, 6.93828382074;
+  return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainLowLowPlantCoefficients());
+}
+
+StateFeedbackController<4, 2, 2> MakeDrivetrainLowHighController() {
+  Eigen::Matrix<double, 4, 2> L;
+  L << 1.45856045663, 0.0145216082167, 41.3651523316, 1.69047524315, 0.0145216082167, 1.65219224519, 3.02153581754, 64.1990849762;
+  Eigen::Matrix<double, 2, 4> K;
+  K << 127.841025245, 6.90618982868, -2.11442482189, 0.171361719101, 11.257083857, 1.47190974842, 138.457761234, 11.0770574926;
+  return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainLowHighPlantCoefficients());
+}
+
+StateFeedbackController<4, 2, 2> MakeDrivetrainHighLowController() {
+  Eigen::Matrix<double, 4, 2> L;
+  L << 1.64511269416, 0.045918446918, 63.7288738736, 5.55411342044, 0.045918446918, 1.46564000767, 3.81771765442, 41.8737588606;
+  Eigen::Matrix<double, 2, 4> K;
+  K << 138.457761234, 11.0770574926, 11.257083857, 1.47190974842, -2.11442482191, 0.171361719101, 127.841025245, 6.90618982869;
+  return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainHighLowPlantCoefficients());
+}
+
+StateFeedbackController<4, 2, 2> MakeDrivetrainHighHighController() {
+  Eigen::Matrix<double, 4, 2> L;
+  L << 1.64543980657, 0.0146814193986, 63.6243330534, 1.89166687195, 0.0146814193986, 1.64543980657, 1.89166687195, 63.6243330534;
+  Eigen::Matrix<double, 2, 4> K;
+  K << 138.52410152, 11.0779399816, 3.96842371774, 0.882728086516, 3.96842371774, 0.882728086517, 138.52410152, 11.0779399816;
+  return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainHighHighPlantCoefficients());
 }
 
 StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant() {
-  ::std::vector<StateFeedbackPlantCoefficients<4, 2, 2> *> plants(1);
-  plants[0] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainPlantCoefficients());
+  ::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);
 }
 
 StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop() {
-  ::std::vector<StateFeedbackController<4, 2, 2> *> controllers(1);
-  controllers[0] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainController());
+  ::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);
 }
 
diff --git a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h
index 3829e9a..366f95d 100644
--- a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h
+++ b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h
@@ -6,9 +6,21 @@
 namespace frc971 {
 namespace control_loops {
 
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainPlantCoefficients();
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowLowPlantCoefficients();
 
-StateFeedbackController<4, 2, 2> MakeDrivetrainController();
+StateFeedbackController<4, 2, 2> MakeDrivetrainLowLowController();
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowHighPlantCoefficients();
+
+StateFeedbackController<4, 2, 2> MakeDrivetrainLowHighController();
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighLowPlantCoefficients();
+
+StateFeedbackController<4, 2, 2> MakeDrivetrainHighLowController();
+
+StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighHighPlantCoefficients();
+
+StateFeedbackController<4, 2, 2> MakeDrivetrainHighHighController();
 
 StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant();