diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index c0c5983..24e7dd8 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -99,14 +99,14 @@
       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;
+    //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;
     SetRawPosition(left, right);
-    LOG(DEBUG, "Left %f->%f Right %f->%f Gyro %f aerror %f ioff %f\n", left + _offset, _left_goal, right - _offset, _right_goal, gyro, angle_error, _integral_offset);
+    //LOG(DEBUG, "Left %f->%f Right %f->%f Gyro %f aerror %f ioff %f\n", left + _offset, _left_goal, right - _offset, _right_goal, gyro, angle_error, _integral_offset);
   }
 
   void Update(bool update_observer, bool stop_motors) {
@@ -193,7 +193,8 @@
         stale_count_(0),
         position_time_delta_(0.01),
         left_gear_(LOW),
-        right_gear_(LOW) {
+        right_gear_(LOW),
+        counter_(0) {
 
     last_position_.Zero();
     position_.Zero();
@@ -202,7 +203,7 @@
 
   static double MotorSpeed(double shifter_position, double velocity) {
     // TODO(austin): G_high, G_low and kWheelRadius
-    if (shifter_position > 0.5) {
+    if (shifter_position > 0.57) {
       return velocity / G_high / kWheelRadius;
     } else {
       return velocity / G_low / kWheelRadius;
@@ -210,7 +211,7 @@
   }
 
   void SetGoal(double wheel, double throttle, bool quickturn, bool highgear) {
-    const double kWheelNonLinearity = 0.4;
+    const double kWheelNonLinearity = 0.3;
     // Apply a sin function that's scaled to make it feel better.
     const double angular_range = M_PI_2 * kWheelNonLinearity;
     wheel_ = sin(angular_range * wheel) / sin(angular_range);
@@ -252,20 +253,52 @@
 
     if (position) {
       // Switch to the correct controller.
-      // TODO(austin): Un-hard code 0.5
-      if (position->left_shifter_position < 0.5) {
-        if (position->right_shifter_position < 0.5) {
+      // TODO(austin): Un-hard code 0.57
+      if (position->left_shifter_position < 0.57) {
+        if (position->right_shifter_position < 0.57) {
+          LOG(DEBUG, "Loop Left low, Right low\n");
           loop_->set_controller_index(0);
         } else {
+          LOG(DEBUG, "Loop Left low, Right high\n");
           loop_->set_controller_index(1);
         }
       } else {
-        if (position->right_shifter_position < 0.5) {
+        if (position->right_shifter_position < 0.57) {
+          LOG(DEBUG, "Loop Left high, Right low\n");
           loop_->set_controller_index(2);
         } else {
+          LOG(DEBUG, "Loop Left high, Right high\n");
           loop_->set_controller_index(3);
         }
       }
+      switch (left_gear_) {
+        case LOW:
+          LOG(DEBUG, "Left is in low\n");
+          break;
+        case HIGH:
+          LOG(DEBUG, "Left is in high\n");
+          break;
+        case SHIFTING_UP:
+          LOG(DEBUG, "Left is shifting up\n");
+          break;
+        case SHIFTING_DOWN:
+          LOG(DEBUG, "Left is shifting down\n");
+          break;
+      }
+      switch (right_gear_) {
+        case LOW:
+          LOG(DEBUG, "Right is in low\n");
+          break;
+        case HIGH:
+          LOG(DEBUG, "Right is in high\n");
+          break;
+        case SHIFTING_UP:
+          LOG(DEBUG, "Right is shifting up\n");
+          break;
+        case SHIFTING_DOWN:
+          LOG(DEBUG, "Right is shifting down\n");
+          break;
+      }
       // TODO(austin): Constants.
       if (position->left_shifter_position > 0.9 && left_gear_ == SHIFTING_UP) {
         left_gear_ = HIGH;
@@ -332,11 +365,12 @@
   void Update() {
     // TODO(austin): Observer for the current velocity instead of difference
     // calculations.
+    ++counter_;
     const double current_left_velocity =
-        (position_.left_encoder - last_position_.left_encoder) * 100.0 /
+        (position_.left_encoder - last_position_.left_encoder) /
         position_time_delta_;
     const double current_right_velocity =
-        (position_.right_encoder - last_position_.right_encoder) * 100.0 /
+        (position_.right_encoder - last_position_.right_encoder) /
         position_time_delta_;
     const double left_motor_speed =
         MotorSpeed(position_.left_shifter_position, current_left_velocity);
@@ -346,10 +380,16 @@
     // Reset the CIM model to the current conditions to be ready for when we shift.
     if (IsInGear(left_gear_)) {
       left_cim_->X_hat(0, 0) = left_motor_speed;
+      LOG(DEBUG, "Setting left CIM to %f at robot speed %f\n", left_motor_speed,
+          current_left_velocity);
     }
     if (IsInGear(right_gear_)) {
-      right_cim_->X_hat(1, 0) = right_motor_speed;
+      right_cim_->X_hat(0, 0) = right_motor_speed;
+      LOG(DEBUG, "Setting right CIM to %f at robot speed %f\n",
+          right_motor_speed, current_right_velocity);
     }
+    LOG(DEBUG, "robot speed l=%f r=%f\n", current_left_velocity,
+        current_right_velocity);
 
     if (IsInGear(left_gear_) && IsInGear(right_gear_)) {
       // FF * X = U (steady state)
@@ -401,36 +441,40 @@
       for (int i = 0; i < 2; i++) {
         loop_->U[i] = ::aos::Clip(U_ideal[i], -12, 12);
       }
+
+      // TODO(austin): Model this better.
+      // TODO(austin): Feed back?
+      loop_->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;
       R_left(0, 0) = left_motor_speed;
+      const double wiggle = (static_cast<double>((counter_ % 4) / 2) - 0.5) * 3.5;
 
-      // TODO(austin): Use battery volts here at some point.
-      loop_->U(0, 0) = ::aos::Clip(
-          (left_cim_->K() * (R_left - left_cim_->X_hat) + R_left / Kv)(0, 0), -12, 12);
-      right_cim_->X_hat = right_cim_->A() * right_cim_->X_hat + right_cim_->B() * loop_->U(0, 0);
+      loop_->U(0, 0) =
+          ::aos::Clip((R_left / Kv)(0, 0) + wiggle, -position_.battery_voltage,
+                      position_.battery_voltage);
+      right_cim_->X_hat = right_cim_->A() * right_cim_->X_hat +
+                          right_cim_->B() * loop_->U(0, 0);
 
       ::Eigen::Matrix<double, 1, 1> R_right;
       R_right(0, 0) = right_motor_speed;
-      loop_->U(1, 0) = ::aos::Clip(
-          (right_cim_->K() * (R_right - right_cim_->X_hat) + R_right / Kv)(0, 0), -12,
-          12);
-      right_cim_->X_hat = right_cim_->A() * right_cim_->X_hat + right_cim_->B() * loop_->U(1, 0);
-    }
-
-    if (IsInGear(left_gear_) && IsInGear(right_gear_)) {
-      // TODO(austin): Model this better.
-      // TODO(austin): Feed back?
-      loop_->X_hat = loop_->A() * loop_->X_hat + loop_->B() * loop_->U;
+      loop_->U(1, 0) =
+          ::aos::Clip((R_right / Kv)(0, 0) + wiggle, -position_.battery_voltage,
+                      position_.battery_voltage);
+      right_cim_->X_hat = right_cim_->A() * right_cim_->X_hat +
+                          right_cim_->B() * loop_->U(1, 0);
+      loop_->U *= 12.0 / position_.battery_voltage;
     }
   }
 
   void SendMotors(Drivetrain::Output *output) {
     LOG(DEBUG, "left pwm: %f right pwm: %f wheel: %f throttle: %f\n",
         loop_->U(0, 0), loop_->U(1, 0), wheel_, throttle_);
-    output->left_voltage = loop_->U(0, 0);
-    output->right_voltage = loop_->U(1, 0);
+    if (output != NULL) {
+      output->left_voltage = loop_->U(0, 0);
+      output->right_voltage = loop_->U(1, 0);
+    }
     // Go in high gear if anything wants to be in high gear.
     // TODO(austin): Seperate these.
     if (left_gear_ == HIGH || left_gear_ == SHIFTING_UP ||
@@ -458,6 +502,7 @@
   Gear right_gear_;
   Drivetrain::Position last_position_;
   Drivetrain::Position position_;
+  int counter_;
 };
 
 
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 0f128ad..1dcc947 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -22,6 +22,7 @@
     double right_encoder;
     double left_shifter_position;
     double right_shifter_position;
+    double battery_voltage;
   };
 
   message Output {
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.cc b/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.cc
index 1cd17ea..4b1a1ba 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.cc
@@ -9,9 +9,9 @@
 
 StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowLowPlantCoefficients() {
   Eigen::Matrix<double, 2, 2> A;
-  A << 0.899177606502, 0.000686937184856, 0.000686937184856, 0.899177606502;
+  A << 0.8793262727, 0.0205382709865, 0.0205382709865, 0.8793262727;
   Eigen::Matrix<double, 2, 2> B;
-  B << 0.0186835844877, -0.000127297602107, -0.000127297602107, 0.0186835844877;
+  B << 0.0223622719242, -0.00380598503859, -0.00380598503859, 0.0223622719242;
   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.89917740051, 0.000149762601927, 0.000716637450627, 0.978035563679;
+  A << 0.879138263008, 0.00451814985125, 0.0216200529991, 0.97348145244;
   Eigen::Matrix<double, 2, 2> B;
-  B << 0.0186836226605, -6.07092175404e-05, -0.00013280141337, 0.0089037164526;
+  B << 0.0223971123485, -0.00183152094495, -0.00400645206708, 0.0107498150538;
   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.978035563679, 0.000716637450627, 0.000149762601927, 0.89917740051;
+  A << 0.97348145244, 0.0216200529991, 0.00451814985125, 0.879138263008;
   Eigen::Matrix<double, 2, 2> B;
-  B << 0.0089037164526, -0.00013280141337, -6.07092175404e-05, 0.0186836226605;
+  B << 0.0107498150538, -0.00400645206708, -0.00183152094495, 0.0223971123485;
   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.978035518136, 0.000156145735499, 0.000156145735499, 0.978035518136;
+  A << 0.973439382316, 0.00475228155545, 0.00475228155545, 0.973439382316;
   Eigen::Matrix<double, 2, 2> B;
-  B << 0.0089037349145, -6.32967463335e-05, -6.32967463335e-05, 0.0089037349145;
+  B << 0.0107668690064, -0.00192643083821, -0.00192643083821, 0.0107668690064;
   Eigen::Matrix<double, 2, 2> C;
   C << 1.0, 0.0, 0.0, 1.0;
   Eigen::Matrix<double, 2, 2> D;
@@ -73,33 +73,33 @@
 
 StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowLowController() {
   Eigen::Matrix<double, 2, 2> L;
-  L << 0.879177606502, 0.000686937184856, 0.000686937184856, 0.879177606502;
+  L << 0.8593262727, 0.0205382709865, 0.0205382709865, 0.8593262727;
   Eigen::Matrix<double, 2, 2> K;
-  K << 16.0138530269, 0.145874699657, 0.145874699657, 16.0138530269;
+  K << 13.0245570096, 3.13517071687, 3.13517071687, 13.0245570096;
   return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowLowPlantCoefficients());
 }
 
 StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController() {
   Eigen::Matrix<double, 2, 2> L;
-  L << 0.879178111554, 0.000716636558747, 0.000716636558747, 0.958034852635;
+  L << 0.859606856614, 0.0216072038404, 0.0216072038404, 0.953012858834;
   Eigen::Matrix<double, 2, 2> K;
-  K << 16.0138530273, 0.145983330189, 0.319338534789, 42.460353773;
+  K << 13.0245575441, 3.13849145977, 6.86545006825, 35.9127730204;
   return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowHighPlantCoefficients());
 }
 
 StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController() {
   Eigen::Matrix<double, 2, 2> L;
-  L << 0.958040379369, 0.000149803514919, 0.000149803514919, 0.87917258482;
+  L << 0.956890244313, 0.00522697730081, 0.00522697730081, 0.855729471134;
   Eigen::Matrix<double, 2, 2> K;
-  K << 42.460353773, 0.319338534789, 0.145983330189, 16.0138530273;
+  K << 35.9127730204, 6.86545006825, 3.13849145977, 13.0245575441;
   return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighLowPlantCoefficients());
 }
 
 StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController() {
   Eigen::Matrix<double, 2, 2> L;
-  L << 0.958035518136, 0.000156145735499, 0.000156145735499, 0.958035518136;
+  L << 0.953439382316, 0.00475228155545, 0.00475228155545, 0.953439382316;
   Eigen::Matrix<double, 2, 2> K;
-  K << 42.460353773, 0.319388212341, 0.319388212341, 42.460353773;
+  K << 35.9127730463, 6.86696893904, 6.86696893904, 35.9127730463;
   return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighHighPlantCoefficients());
 }
 
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index 27bebbc..c6a3f05 100755
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -62,7 +62,7 @@
     self.free_current = 2.7
     # Moment of inertia of the drivetrain in kg m^2
     # Just borrowed from last year.
-    self.J = 6.4
+    self.J = 4.5
     # Mass of the robot, in kg.
     self.m = 68
     # Radius of the robot, in meters (from last year).
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index da9d414..f28cf42 100755
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -209,9 +209,9 @@
 
   def SimShifter(self, gear, shifter_position):
     if gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.SHIFTING_UP:
-      shifter_position = min(shifter_position + 0.1, 1.0)
+      shifter_position = min(shifter_position + 0.5, 1.0)
     else:
-      shifter_position = max(shifter_position - 0.1, 0.0)
+      shifter_position = max(shifter_position - 0.5, 0.0)
 
     if shifter_position == 1.0:
       gear = VelocityDrivetrain.HIGH
@@ -441,11 +441,11 @@
 
   for t in numpy.arange(0, 4.0, vdrivetrain.dt):
     if t < 1.0:
-      vdrivetrain.Update(throttle=0.60, steering=0.0)
+      vdrivetrain.Update(throttle=1.00, steering=0.0)
     elif t < 1.2:
-      vdrivetrain.Update(throttle=0.60, steering=0.0)
+      vdrivetrain.Update(throttle=1.00, steering=0.0)
     else:
-      vdrivetrain.Update(throttle=0.60, steering=0.0)
+      vdrivetrain.Update(throttle=1.00, steering=0.0)
     t_plot.append(t)
     vl_plot.append(vdrivetrain.X[0, 0])
     vr_plot.append(vdrivetrain.X[1, 0])