diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 122e2d2..3e24dc2 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -150,7 +150,7 @@
                  /*[*/ 12 /*]]*/).finished()),
         loop_(new StateFeedbackLoop<2, 2, 2>(MakeVDrivetrainLoop())) {
 
-    ttrust_ = 1.0;
+    ttrust_ = 1.1;
 
     wheel_ = 0.0;
     throttle_ = 0.0;
@@ -158,7 +158,7 @@
     highgear_ = true;
   }
   void SetGoal(double wheel, double throttle, bool quickturn, bool highgear) {
-    const double kWheelNonLinearity = 0.1;
+    const double kWheelNonLinearity = 0.4;
     // 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);
@@ -197,6 +197,28 @@
             (ttrust_ * min_K_sum + min_FF_sum));
   }
 
+  double MaxVelocity() {
+    const Eigen::Matrix<double, 2, 2> FF =
+        loop_->B().inverse() *
+        (Eigen::Matrix<double, 2, 2>::Identity() - loop_->A());
+
+    constexpr int kHighGearController = 3;
+    const Eigen::Matrix<double, 2, 2> FF_high =
+        loop_->controller(kHighGearController).plant.B.inverse() *
+        (Eigen::Matrix<double, 2, 2>::Identity() -
+         loop_->controller(kHighGearController).plant.A);
+
+    ::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 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);
+    return adjusted_ff_voltage / min_FF_sum;
+  }
+
   void Update() {
     // FF * X = U (steady state)
     const Eigen::Matrix<double, 2, 2> FF =
@@ -210,31 +232,38 @@
     const double fvel = FilterVelocity(throttle_);
 
     const double sign_svel = wheel_ * ((fvel > 0.0) ? 1.0 : -1.0);
-    const double svel = ::std::abs(fvel) * wheel_;
-    const double left_velocity = fvel - svel;
-    const double right_velocity = fvel + svel;
-
-    // K * R = w
-    Eigen::Matrix<double,1,2> equality_k;
-    equality_k << 1 + sign_svel, -(1 - sign_svel);
-    const double equality_w = 0.0;
+    double steering_velocity;
+    if (quickturn_) {
+      steering_velocity = wheel_ * MaxVelocity();
+    } else {
+      steering_velocity = ::std::abs(fvel) * wheel_;
+    }
+    const double left_velocity = fvel - steering_velocity;
+    const double right_velocity = fvel + steering_velocity;
 
     // Integrate velocity to get the position.
     // This position is used to get integral control.
     loop_->R << left_velocity, right_velocity;
 
+    if (!quickturn_) {
+    // K * R = w
+    Eigen::Matrix<double,1,2> equality_k;
+    equality_k << 1 + sign_svel, -(1 - sign_svel);
+    const double equality_w = 0.0;
+
     // Construct a constraint on R by manipulating the constraint on U
     ::aos::controls::HPolytope<2> R_poly = ::aos::controls::HPolytope<2>(
         U_Poly_.H() * (loop_->K() + FF),
         U_Poly_.k() + U_Poly_.H() * loop_->K() * loop_->X_hat);
 
     // Limit R back inside the box.
-    const Eigen::Matrix<double, 2, 1> boxed_R =
+    loop_->R =
         CoerceGoal(R_poly, equality_k, equality_w, loop_->R);
+    }
 
-    const Eigen::Matrix<double, 2, 1> FF_volts = FF * boxed_R;
+    const Eigen::Matrix<double, 2, 1> FF_volts = FF * loop_->R;
     const Eigen::Matrix<double, 2, 1> U_ideal =
-        loop_->K() * (boxed_R - loop_->X_hat) + FF_volts;
+        loop_->K() * (loop_->R - loop_->X_hat) + FF_volts;
 
     for (int i = 0; i < 2; i++) {
       loop_->U[i] = ::aos::Clip(U_ideal[i], -12, 12);
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index cdfe744..249de70 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -5,6 +5,7 @@
 
 #include "aos/controls/polytope.h"
 #include "aos/common/control_loop/ControlLoop.h"
+#include "aos/controls/polytope.h"
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
 
 namespace frc971 {
@@ -23,7 +24,9 @@
   explicit DrivetrainLoop(
       control_loops::Drivetrain *my_drivetrain = &control_loops::drivetrain)
       : aos::control_loops::ControlLoop<control_loops::Drivetrain, true, false>(
-          my_drivetrain) {}
+          my_drivetrain) {
+    ::aos::controls::HPolytope<0>::Init();
+  }
 
  protected:
   // Executes one cycle of the control loop.
diff --git a/frc971/control_loops/drivetrain/drivetrain_motor_plant.cc b/frc971/control_loops/drivetrain/drivetrain_motor_plant.cc
index e543c9f..071b257 100644
--- a/frc971/control_loops/drivetrain/drivetrain_motor_plant.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_motor_plant.cc
@@ -9,9 +9,9 @@
 
 StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainPlantCoefficients() {
   Eigen::Matrix<double, 4, 4> A;
-  A << 1.0, 0.00931379160739, 0.0, 4.70184876909e-06, 0.0, 0.865971883056, 0.0, 0.000895808426591, 0.0, 4.70184876909e-06, 1.0, 0.00931379160739, 0.0, 0.000895808426591, 0.0, 0.865971883056;
+  A << 1.0, 0.00948696019317, 0.0, 3.55966215909e-06, 0.0, 0.899177606502, 0.0, 0.000686937184856, 0.0, 3.55966215909e-06, 1.0, 0.00948696019317, 0.0, 0.000686937184856, 0.0, 0.899177606502;
   Eigen::Matrix<double, 4, 2> B;
-  B << 0.000126707931029, -8.6819330098e-07, 0.0247482041615, -0.000165410440259, -8.6819330098e-07, 0.000126707931029, -0.000165410440259, 0.0247482041615;
+  B << 9.50723568824e-05, -6.59647588097e-07, 0.0186835844877, -0.000127297602107, -6.59647588097e-07, 9.50723568824e-05, -0.000127297602107, 0.0186835844877;
   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 @@
 
 StateFeedbackController<4, 2, 2> MakeDrivetrainController() {
   Eigen::Matrix<double, 4, 2> L;
-  L << 1.70597188306, 0.000895808426591, 66.3158545945, 0.117712892743, 0.000895808426591, 1.70597188306, 0.117712892743, 66.3158545945;
+  L << 1.7391776065, 0.000686937184856, 70.7236123469, 0.0920942992696, 0.000686937184856, 1.7391776065, 0.0920942992696, 70.7236123469;
   Eigen::Matrix<double, 2, 4> K;
-  K << 240.432225842, 14.3659115621, 1.60698530163, 0.13242189318, 1.60698530163, 0.13242189318, 240.432225842, 14.3659115621;
+  K << 318.476158856, 20.8163224602, 2.1698861574, 0.178798182963, 2.1698861574, 0.178798182963, 318.476158856, 20.8163224602;
   return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainPlantCoefficients());
 }
 
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.cc b/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.cc
index 31a029f..d07f22b 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.cc
@@ -75,7 +75,7 @@
   Eigen::Matrix<double, 2, 2> L;
   L << 0.879177606502, 0.000686937184856, 0.000686937184856, 0.879177606502;
   Eigen::Matrix<double, 2, 2> K;
-  K << 32.0714744818, 0.255280724401, 0.255280724401, 32.0714744818;
+  K << 21.3663935118, 0.182343374572, 0.182343374572, 21.3663935118;
   return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowLowPlantCoefficients());
 }
 
@@ -83,7 +83,7 @@
   Eigen::Matrix<double, 2, 2> L;
   L << 0.879178111554, 0.000716636558747, 0.000716636558747, 0.958034852635;
   Eigen::Matrix<double, 2, 2> K;
-  K << 32.0714744826, 0.255470827831, 0.558842435881, 76.1557821586;
+  K << 21.3663935124, 0.182479162737, 0.399173168486, 53.6921632348;
   return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowHighPlantCoefficients());
 }
 
@@ -91,7 +91,7 @@
   Eigen::Matrix<double, 2, 2> L;
   L << 0.958040379369, 0.000149803514919, 0.000149803514919, 0.87917258482;
   Eigen::Matrix<double, 2, 2> K;
-  K << 76.1557821586, 0.558842435881, 0.255470827831, 32.0714744826;
+  K << 53.6921632348, 0.399173168486, 0.182479162737, 21.3663935124;
   return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighLowPlantCoefficients());
 }
 
@@ -99,7 +99,7 @@
   Eigen::Matrix<double, 2, 2> L;
   L << 0.958035518136, 0.000156145735499, 0.000156145735499, 0.958035518136;
   Eigen::Matrix<double, 2, 2> K;
-  K << 76.1557821586, 0.558929371597, 0.558929371597, 76.1557821586;
+  K << 53.6921632349, 0.399235265426, 0.399235265426, 53.6921632349;
   return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighHighPlantCoefficients());
 }
 
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index 9fa7841..0251408 100755
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -117,7 +117,7 @@
     # FF * X = U (steady state)
     self.FF = self.B.I * (numpy.eye(2) - self.A)
 
-    self.PlaceControllerPoles([0.3, 0.3])
+    self.PlaceControllerPoles([0.5, 0.5])
     self.PlaceObserverPoles([0.02, 0.02])
 
     self.G_high = self._drivetrain.G_high