diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index bcfc6a1..b348bf8 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -241,21 +241,21 @@
   // 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 kWheelRadius = 0.04445;
+  static constexpr double rb = 0.9603 / 2.0;
+  static constexpr double kWheelRadius = 0.0515938;
   // 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 / 2 + 0.03) / (0.93 * 0.93);
   // Motor velocity constant
   static constexpr double Kv =
       ((kFreeSpeed / 60.0 * 2.0 * M_PI) / (12.0 - kR * kFreeCurrent));
@@ -322,11 +322,13 @@
   }
 
   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;
+
     wheel_ = sin(angular_range * wheel) / sin(angular_range);
     wheel_ = sin(angular_range * wheel_) / sin(angular_range);
+    wheel_ *= 2.3;
     quickturn_ = quickturn;
 
     static const double kThrottleDeadband = 0.05;
@@ -481,9 +483,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() {
@@ -580,6 +583,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.
diff --git a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
index a925a89..b04c5af 100644
--- a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
@@ -9,9 +9,9 @@
 
 StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowLowPlantCoefficients() {
   Eigen::Matrix<double, 4, 4> A;
-  A << 1.0, 0.00489267131849, 0.0, 1.91843505092e-05, 0.0, 0.957387962253, 0.0, 0.00756271754847, 0.0, 1.91843505092e-05, 1.0, 0.00489267131849, 0.0, 0.00756271754847, 0.0, 0.957387962253;
+  A << 1.0, 0.00494348612824, 0.0, 1.24048455404e-05, 0.0, 0.977484092703, 0.0, 0.00492433284048, 0.0, 1.24048455404e-05, 1.0, 0.00494348612824, 0.0, 0.00492433284048, 0.0, 0.977484092703;
   Eigen::Matrix<double, 4, 2> B;
-  B << 3.95240483493e-05, -7.06468379563e-06, 0.0156919866758, -0.0027849891551, -7.06468379563e-06, 3.95240483493e-05, -0.0027849891551, 0.0156919866758;
+  B << 2.09180756177e-05, -4.59153636043e-06, 0.00833405032795, -0.00182269526971, -4.59153636043e-06, 2.09180756177e-05, -0.00182269526971, 0.00833405032795;
   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.00489267131849, 0.0, 1.91843505092e-05, 0.0, 0.957387962253, 0.0, 0.00756271754847, 0.0, 1.91843505092e-05, 1.0, 0.00489267131849, 0.0, 0.00756271754847, 0.0, 0.957387962253;
+  A << 1.0, 0.00494348612824, 0.0, 1.24048455404e-05, 0.0, 0.977484092703, 0.0, 0.00492433284048, 0.0, 1.24048455404e-05, 1.0, 0.00494348612824, 0.0, 0.00492433284048, 0.0, 0.977484092703;
   Eigen::Matrix<double, 4, 2> B;
-  B << 3.95240483493e-05, -7.06468379563e-06, 0.0156919866758, -0.0027849891551, -7.06468379563e-06, 3.95240483493e-05, -0.0027849891551, 0.0156919866758;
+  B << 2.09180756177e-05, -4.59153636043e-06, 0.00833405032795, -0.00182269526971, -4.59153636043e-06, 2.09180756177e-05, -0.00182269526971, 0.00833405032795;
   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.00489267131849, 0.0, 1.91843505092e-05, 0.0, 0.957387962253, 0.0, 0.00756271754847, 0.0, 1.91843505092e-05, 1.0, 0.00489267131849, 0.0, 0.00756271754847, 0.0, 0.957387962253;
+  A << 1.0, 0.00494348612824, 0.0, 1.24048455404e-05, 0.0, 0.977484092703, 0.0, 0.00492433284048, 0.0, 1.24048455404e-05, 1.0, 0.00494348612824, 0.0, 0.00492433284048, 0.0, 0.977484092703;
   Eigen::Matrix<double, 4, 2> B;
-  B << 3.95240483493e-05, -7.06468379563e-06, 0.0156919866758, -0.0027849891551, -7.06468379563e-06, 3.95240483493e-05, -0.0027849891551, 0.0156919866758;
+  B << 2.09180756177e-05, -4.59153636043e-06, 0.00833405032795, -0.00182269526971, -4.59153636043e-06, 2.09180756177e-05, -0.00182269526971, 0.00833405032795;
   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.00489267131849, 0.0, 1.91843505092e-05, 0.0, 0.957387962253, 0.0, 0.00756271754847, 0.0, 1.91843505092e-05, 1.0, 0.00489267131849, 0.0, 0.00756271754847, 0.0, 0.957387962253;
+  A << 1.0, 0.00494348612824, 0.0, 1.24048455404e-05, 0.0, 0.977484092703, 0.0, 0.00492433284048, 0.0, 1.24048455404e-05, 1.0, 0.00494348612824, 0.0, 0.00492433284048, 0.0, 0.977484092703;
   Eigen::Matrix<double, 4, 2> B;
-  B << 3.95240483493e-05, -7.06468379563e-06, 0.0156919866758, -0.0027849891551, -7.06468379563e-06, 3.95240483493e-05, -0.0027849891551, 0.0156919866758;
+  B << 2.09180756177e-05, -4.59153636043e-06, 0.00833405032795, -0.00182269526971, -4.59153636043e-06, 2.09180756177e-05, -0.00182269526971, 0.00833405032795;
   Eigen::Matrix<double, 2, 4> C;
   C << 1, 0, 0, 0, 0, 0, 1, 0;
   Eigen::Matrix<double, 2, 2> D;
@@ -73,41 +73,41 @@
 
 StateFeedbackController<4, 2, 2> MakeDrivetrainLowLowController() {
   Eigen::Matrix<double, 4, 2> L;
-  L << 1.25738796225, 0.00756271754847, 74.8971101634, 1.58403340092, 0.00756271754847, 1.25738796225, 1.58403340092, 74.8971101634;
+  L << 1.2774840927, 0.00492433284048, 79.1440456622, 1.05150722636, 0.00492433284048, 1.2774840927, 1.05150722636, 79.1440456622;
   Eigen::Matrix<double, 2, 4> K;
-  K << 154.070413936, 12.2263299292, 2.23580489274, 0.77037863276, 2.23580489276, 0.77037863276, 154.070413936, 12.2263299292;
+  K << 160.19677247, 15.5358468938, 1.60401194139, 1.33861213093, 1.60401194142, 1.33861213094, 160.19677247, 15.5358468938;
   Eigen::Matrix<double, 4, 4> A_inv;
-  A_inv << 1.0, -0.00511059808244, 0.0, 2.03320493463e-05, 0.0, 1.04457382236, 0.0, -0.00825142689119, 0.0, 2.03320493463e-05, 1.0, -0.00511059808244, 0.0, -0.00825142689119, 0.0, 1.04457382236;
+  A_inv << 1.0, -0.00505742153256, 0.0, 1.27875036472e-05, 0.0, 1.02306051542, 0.0, -0.00515393603998, 0.0, 1.27875036472e-05, 1.0, -0.00505742153256, 0.0, -0.00515393603998, 0.0, 1.02306051542;
   return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainLowLowPlantCoefficients());
 }
 
 StateFeedbackController<4, 2, 2> MakeDrivetrainLowHighController() {
   Eigen::Matrix<double, 4, 2> L;
-  L << 1.25738796225, 0.00756271754847, 74.8971101634, 1.58403340092, 0.00756271754847, 1.25738796225, 1.58403340092, 74.8971101634;
+  L << 1.2774840927, 0.00492433284048, 79.1440456622, 1.05150722636, 0.00492433284048, 1.2774840927, 1.05150722636, 79.1440456622;
   Eigen::Matrix<double, 2, 4> K;
-  K << 154.070413936, 12.2263299292, 2.23580489274, 0.77037863276, 2.23580489276, 0.77037863276, 154.070413936, 12.2263299292;
+  K << 160.19677247, 15.5358468938, 1.60401194139, 1.33861213093, 1.60401194142, 1.33861213094, 160.19677247, 15.5358468938;
   Eigen::Matrix<double, 4, 4> A_inv;
-  A_inv << 1.0, -0.00511059808244, 0.0, 2.03320493463e-05, 0.0, 1.04457382236, 0.0, -0.00825142689119, 0.0, 2.03320493463e-05, 1.0, -0.00511059808244, 0.0, -0.00825142689119, 0.0, 1.04457382236;
+  A_inv << 1.0, -0.00505742153256, 0.0, 1.27875036472e-05, 0.0, 1.02306051542, 0.0, -0.00515393603998, 0.0, 1.27875036472e-05, 1.0, -0.00505742153256, 0.0, -0.00515393603998, 0.0, 1.02306051542;
   return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainLowHighPlantCoefficients());
 }
 
 StateFeedbackController<4, 2, 2> MakeDrivetrainHighLowController() {
   Eigen::Matrix<double, 4, 2> L;
-  L << 1.25738796225, 0.00756271754847, 74.8971101634, 1.58403340092, 0.00756271754847, 1.25738796225, 1.58403340092, 74.8971101634;
+  L << 1.2774840927, 0.00492433284048, 79.1440456622, 1.05150722636, 0.00492433284048, 1.2774840927, 1.05150722636, 79.1440456622;
   Eigen::Matrix<double, 2, 4> K;
-  K << 154.070413936, 12.2263299292, 2.23580489274, 0.77037863276, 2.23580489276, 0.77037863276, 154.070413936, 12.2263299292;
+  K << 160.19677247, 15.5358468938, 1.60401194139, 1.33861213093, 1.60401194142, 1.33861213094, 160.19677247, 15.5358468938;
   Eigen::Matrix<double, 4, 4> A_inv;
-  A_inv << 1.0, -0.00511059808244, 0.0, 2.03320493463e-05, 0.0, 1.04457382236, 0.0, -0.00825142689119, 0.0, 2.03320493463e-05, 1.0, -0.00511059808244, 0.0, -0.00825142689119, 0.0, 1.04457382236;
+  A_inv << 1.0, -0.00505742153256, 0.0, 1.27875036472e-05, 0.0, 1.02306051542, 0.0, -0.00515393603998, 0.0, 1.27875036472e-05, 1.0, -0.00505742153256, 0.0, -0.00515393603998, 0.0, 1.02306051542;
   return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainHighLowPlantCoefficients());
 }
 
 StateFeedbackController<4, 2, 2> MakeDrivetrainHighHighController() {
   Eigen::Matrix<double, 4, 2> L;
-  L << 1.25738796225, 0.00756271754847, 74.8971101634, 1.58403340092, 0.00756271754847, 1.25738796225, 1.58403340092, 74.8971101634;
+  L << 1.2774840927, 0.00492433284048, 79.1440456622, 1.05150722636, 0.00492433284048, 1.2774840927, 1.05150722636, 79.1440456622;
   Eigen::Matrix<double, 2, 4> K;
-  K << 154.070413936, 12.2263299292, 2.23580489274, 0.77037863276, 2.23580489276, 0.77037863276, 154.070413936, 12.2263299292;
+  K << 160.19677247, 15.5358468938, 1.60401194139, 1.33861213093, 1.60401194142, 1.33861213094, 160.19677247, 15.5358468938;
   Eigen::Matrix<double, 4, 4> A_inv;
-  A_inv << 1.0, -0.00511059808244, 0.0, 2.03320493463e-05, 0.0, 1.04457382236, 0.0, -0.00825142689119, 0.0, 2.03320493463e-05, 1.0, -0.00511059808244, 0.0, -0.00825142689119, 0.0, 1.04457382236;
+  A_inv << 1.0, -0.00505742153256, 0.0, 1.27875036472e-05, 0.0, 1.02306051542, 0.0, -0.00515393603998, 0.0, 1.27875036472e-05, 1.0, -0.00505742153256, 0.0, -0.00515393603998, 0.0, 1.02306051542;
   return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainHighHighPlantCoefficients());
 }
 
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
index 3d08774..1641306 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
@@ -9,9 +9,9 @@
 
 StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowLowPlantCoefficients() {
   Eigen::Matrix<double, 2, 2> A;
-  A << 0.916648904963, 0.0144809094856, 0.0144809094856, 0.916648904963;
+  A << 0.955499400541, 0.00962691403749, 0.00962691403749, 0.955499400541;
   Eigen::Matrix<double, 2, 2> B;
-  B << 0.0306942437366, -0.00533263018418, -0.00533263018418, 0.0306942437366;
+  B << 0.0164714763931, -0.00356331126397, -0.00356331126397, 0.0164714763931;
   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.916648904963, 0.0144809094856, 0.0144809094856, 0.916648904963;
+  A << 0.955499400541, 0.00962691403749, 0.00962691403749, 0.955499400541;
   Eigen::Matrix<double, 2, 2> B;
-  B << 0.0306942437366, -0.00533263018418, -0.00533263018418, 0.0306942437366;
+  B << 0.0164714763931, -0.00356331126397, -0.00356331126397, 0.0164714763931;
   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.916648904963, 0.0144809094856, 0.0144809094856, 0.916648904963;
+  A << 0.955499400541, 0.00962691403749, 0.00962691403749, 0.955499400541;
   Eigen::Matrix<double, 2, 2> B;
-  B << 0.0306942437366, -0.00533263018418, -0.00533263018418, 0.0306942437366;
+  B << 0.0164714763931, -0.00356331126397, -0.00356331126397, 0.0164714763931;
   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.916648904963, 0.0144809094856, 0.0144809094856, 0.916648904963;
+  A << 0.955499400541, 0.00962691403749, 0.00962691403749, 0.955499400541;
   Eigen::Matrix<double, 2, 2> B;
-  B << 0.0306942437366, -0.00533263018418, -0.00533263018418, 0.0306942437366;
+  B << 0.0164714763931, -0.00356331126397, -0.00356331126397, 0.0164714763931;
   Eigen::Matrix<double, 2, 2> C;
   C << 1.0, 0.0, 0.0, 1.0;
   Eigen::Matrix<double, 2, 2> D;
@@ -73,41 +73,41 @@
 
 StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowLowController() {
   Eigen::Matrix<double, 2, 2> L;
-  L << 0.896648904963, 0.0144809094856, 0.0144809094856, 0.896648904963;
+  L << 0.935499400541, 0.00962691403749, 0.00962691403749, 0.935499400541;
   Eigen::Matrix<double, 2, 2> K;
-  K << 10.7218164758, 2.3345221426, 2.3345221426, 10.7218164758;
+  K << 22.7750288573, 5.51143253556, 5.51143253556, 22.7750288573;
   Eigen::Matrix<double, 2, 2> A_inv;
-  A_inv << 1.0912025564, -0.0172384490553, -0.0172384490553, 1.0912025564;
+  A_inv << 1.04667938127, -0.010545576923, -0.010545576923, 1.04667938127;
   return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainLowLowPlantCoefficients());
 }
 
 StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController() {
   Eigen::Matrix<double, 2, 2> L;
-  L << 0.896648904963, 0.0144809094856, 0.0144809094856, 0.896648904963;
+  L << 0.935499400541, 0.00962691403749, 0.00962691403749, 0.935499400541;
   Eigen::Matrix<double, 2, 2> K;
-  K << 10.7218164758, 2.3345221426, 2.3345221426, 10.7218164758;
+  K << 22.7750288573, 5.51143253556, 5.51143253556, 22.7750288573;
   Eigen::Matrix<double, 2, 2> A_inv;
-  A_inv << 1.0912025564, -0.0172384490553, -0.0172384490553, 1.0912025564;
+  A_inv << 1.04667938127, -0.010545576923, -0.010545576923, 1.04667938127;
   return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainLowHighPlantCoefficients());
 }
 
 StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController() {
   Eigen::Matrix<double, 2, 2> L;
-  L << 0.896648904963, 0.0144809094856, 0.0144809094856, 0.896648904963;
+  L << 0.935499400541, 0.00962691403749, 0.00962691403749, 0.935499400541;
   Eigen::Matrix<double, 2, 2> K;
-  K << 10.7218164758, 2.3345221426, 2.3345221426, 10.7218164758;
+  K << 22.7750288573, 5.51143253556, 5.51143253556, 22.7750288573;
   Eigen::Matrix<double, 2, 2> A_inv;
-  A_inv << 1.0912025564, -0.0172384490553, -0.0172384490553, 1.0912025564;
+  A_inv << 1.04667938127, -0.010545576923, -0.010545576923, 1.04667938127;
   return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainHighLowPlantCoefficients());
 }
 
 StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController() {
   Eigen::Matrix<double, 2, 2> L;
-  L << 0.896648904963, 0.0144809094856, 0.0144809094856, 0.896648904963;
+  L << 0.935499400541, 0.00962691403749, 0.00962691403749, 0.935499400541;
   Eigen::Matrix<double, 2, 2> K;
-  K << 10.7218164758, 2.3345221426, 2.3345221426, 10.7218164758;
+  K << 22.7750288573, 5.51143253556, 5.51143253556, 22.7750288573;
   Eigen::Matrix<double, 2, 2> A_inv;
-  A_inv << 1.0912025564, -0.0172384490553, -0.0172384490553, 1.0912025564;
+  A_inv << 1.04667938127, -0.010545576923, -0.010545576923, 1.04667938127;
   return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainHighHighPlantCoefficients());
 }