diff --git a/aos/common/util/trapezoid_profile.h b/aos/common/util/trapezoid_profile.h
index 1051eec..92e3dbe 100644
--- a/aos/common/util/trapezoid_profile.h
+++ b/aos/common/util/trapezoid_profile.h
@@ -24,6 +24,15 @@
   const Eigen::Matrix<double, 2, 1> &Update(double goal_position,
                                             double goal_velocity);
 
+  // Useful for preventing windup etc.
+  void MoveGoal(double dx) {
+    output_(0, 0) += dx;
+  }
+
+  void SetGoal(double x) {
+    output_(0, 0) = x;
+  }
+
   void set_maximum_acceleration(double maximum_acceleration) {
     maximum_acceleration_ = maximum_acceleration;
   }
diff --git a/frc971/actions/drivetrain_action.cc b/frc971/actions/drivetrain_action.cc
index 24a766c..07a2abb 100644
--- a/frc971/actions/drivetrain_action.cc
+++ b/frc971/actions/drivetrain_action.cc
@@ -6,6 +6,7 @@
 #include "aos/common/util/phased_loop.h"
 #include "aos/common/logging/logging.h"
 #include "aos/common/util/trapezoid_profile.h"
+#include "aos/common/commonmath.h"
 
 #include "frc971/actions/drivetrain_action.h"
 #include "frc971/constants.h"
@@ -18,6 +19,8 @@
     : actions::ActionBase<actions::DrivetrainActionQueueGroup>(s) {}
 
 void DrivetrainAction::RunAction() {
+  static const auto K = constants::GetValues().make_drivetrain_loop().K();
+
   const double yoffset = action_q_->goal->y_offset;
   const double turn_offset =
       action_q_->goal->theta_offset * constants::GetValues().turn_width / 2.0;
@@ -46,6 +49,56 @@
     left_goal_state = drive_profile_goal_state - turn_profile_goal_state;
     right_goal_state = drive_profile_goal_state + turn_profile_goal_state;
 
+    control_loops::drivetrain.status.FetchLatest();
+    if (control_loops::drivetrain.status.get()) {
+      const auto &status = *control_loops::drivetrain.status;
+      if (::std::abs(status.uncapped_left_voltage -
+                     status.uncapped_right_voltage) >
+          24) {
+        // They're more than 24V apart, so stop moving forwards and let it deal
+        // with spinning first.
+        profile.SetGoal(
+            (status.filtered_left_position + status.filtered_right_position) /
+            2.0);
+      } else {
+        static const double divisor = K(0, 1) + K(0, 3);
+        double dx_left, dx_right;
+        if (status.uncapped_left_voltage > 12.0) {
+          dx_left = (status.uncapped_left_voltage - 12.0) / divisor;
+        } else if (status.uncapped_left_voltage < -12.0) {
+          dx_left = (status.uncapped_left_voltage + 12.0) / divisor;
+        } else {
+          dx_left = 0;
+        }
+        if (status.uncapped_right_voltage > 12.0) {
+          dx_right = (status.uncapped_right_voltage - 12.0) / divisor;
+        } else if (status.uncapped_right_voltage < -12.0) {
+          dx_right = (status.uncapped_right_voltage + 12.0) / divisor;
+        } else {
+          dx_right = 0;
+        }
+        double dx;
+        if (dx_left == 0 && dx_right == 0) {
+          dx = 0;
+        } else if (dx_left != 0 && dx_right != 0 &&
+                   ::aos::sign(dx_left) != ::aos::sign(dx_right)) {
+          // Both saturating in opposite directions. Don't do anything.
+          dx = 0;
+        } else if (::std::abs(dx_left) > ::std::abs(dx_right)) {
+          dx = dx_left;
+        } else {
+          dx = dx_right;
+        }
+        if (dx != 0) {
+          LOG(DEBUG, "adjusting goal by %f\n", dx);
+          profile.MoveGoal(-dx);
+        }
+      }
+    } else {
+      // If we ever get here, that's bad and we should just give up
+      LOG(FATAL, "no drivetrain status!\n");
+    }
+
     if (::std::abs(drive_profile_goal_state(0, 0) - yoffset) < epsilon &&
         ::std::abs(turn_profile_goal_state(0, 0) - turn_offset) < epsilon) {
       break;
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index 82f690d..ad8350e 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -247,7 +247,7 @@
   {
     if (ShouldExitAuto()) return;
     // Drive to the goal.
-    auto drivetrain_action = SetDriveGoal(-kShootDistance);
+    auto drivetrain_action = SetDriveGoal(-kShootDistance, 2.5);
     time::SleepFor(time::Time::InSeconds(0.75));
     PositionClawForShot();
     LOG(INFO, "Waiting until drivetrain is finished\n");
@@ -281,7 +281,8 @@
     LOG(INFO, "Claw ready for intake at %f\n",
         (::aos::time::Time::Now() - start_time).ToSeconds());
     PositionClawBackIntake();
-    auto drivetrain_action = SetDriveGoal(kShootDistance + kPickupDistance);
+    auto drivetrain_action =
+        SetDriveGoal(kShootDistance + kPickupDistance, 2.5);
     LOG(INFO, "Waiting until drivetrain is finished\n");
     WaitUntilDoneOrCanceled(drivetrain_action.get());
     if (ShouldExitAuto()) return;
@@ -295,7 +296,8 @@
   {
     LOG(INFO, "Driving back at %f\n",
         (::aos::time::Time::Now() - start_time).ToSeconds());
-    auto drivetrain_action = SetDriveGoal(-(kShootDistance + kPickupDistance));
+    auto drivetrain_action =
+        SetDriveGoal(-(kShootDistance + kPickupDistance), 2.5);
     time::SleepFor(time::Time::InSeconds(0.3));
     if (ShouldExitAuto()) return;
     PositionClawForShot();
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 299437e..e755f3d 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -41,11 +41,16 @@
       T_inverse = T.inverse();
     }
 
+    bool output_was_capped() const {
+      return output_was_capped_;
+    }
+
    private:
     virtual void CapU() {
       const Eigen::Matrix<double, 4, 1> error = R - X_hat;
 
-      if (U(0, 0) > 12.0 || U(1, 0) > 12.0) {
+      if (::std::abs(U(0, 0)) > 12.0 || ::std::abs(U(1, 0)) > 12.0) {
+        output_was_capped_ = true;
         LOG_MATRIX(DEBUG, "U at start", U);
 
         Eigen::Matrix<double, 2, 2> position_K;
@@ -113,11 +118,14 @@
         LOG_MATRIX(DEBUG, "adjusted_pos_error", adjusted_pos_error);
         U = velocity_K * velocity_error + position_K * T * adjusted_pos_error;
         LOG_MATRIX(DEBUG, "U is now", U);
+      } else {
+        output_was_capped_ = false;
       }
     }
 
     const ::aos::controls::HPolytope<2> U_Poly_;
     Eigen::Matrix<double, 2, 2> T, T_inverse;
+    bool output_was_capped_;
   };
 
   DrivetrainMotorsSS()
@@ -130,8 +138,8 @@
         raw_left_(0.0),
         raw_right_(0.0),
         control_loop_driving_(false) {
-    // High gear on both.
-    loop_->set_controller_index(3);
+    // Low gear on both.
+    loop_->set_controller_index(0);
   }
 
   void SetGoal(double left, double left_velocity, double right,
@@ -176,20 +184,24 @@
     LOG_MATRIX(DEBUG, "E", E);
   }
 
-  double GetEstimatedRobotSpeed() {
+  double GetEstimatedRobotSpeed() const {
     // lets just call the average of left and right velocities close enough
     return (loop_->X_hat(1, 0) + loop_->X_hat(3, 0)) / 2;
   }
   
-  double GetEstimatedLeftEncoder() {
+  double GetEstimatedLeftEncoder() const {
     return loop_->X_hat(0, 0);
   }
   
-  double GetEstimatedRightEncoder() {
+  double GetEstimatedRightEncoder() const {
     return loop_->X_hat(2, 0);
   }
 
-  void SendMotors(Drivetrain::Output *output) {
+  bool OutputWasCapped() const {
+    return loop_->output_was_capped();
+  }
+
+  void SendMotors(Drivetrain::Output *output) const {
     if (output) {
       output->left_voltage = loop_->U(0, 0);
       output->right_voltage = loop_->U(1, 0);
@@ -198,6 +210,8 @@
     }
   }
 
+  const LimitedDrivetrainLoop &loop() const { return *loop_; }
+
  private:
   ::std::unique_ptr<LimitedDrivetrainLoop> loop_;
 
@@ -707,6 +721,9 @@
     status->robot_speed = dt_closedloop.GetEstimatedRobotSpeed();
     status->filtered_left_position = dt_closedloop.GetEstimatedLeftEncoder();
     status->filtered_right_position = dt_closedloop.GetEstimatedRightEncoder();
+    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);
   }
 }
 
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 32eb2fb..0774fbc 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -23,15 +23,15 @@
   implements aos.control_loops.ControlLoop;
 
   message Goal {
-    float steering;
-    float throttle;
+    double steering;
+    double throttle;
     bool highgear;
     bool quickturn;
     bool control_loop_driving;
-    float left_goal;
-    float left_velocity_goal;
-    float right_goal;
-    float right_velocity_goal;
+    double left_goal;
+    double left_velocity_goal;
+    double right_goal;
+    double right_velocity_goal;
   };
 
   message Position {
@@ -43,17 +43,22 @@
   };
 
   message Output {
-    float left_voltage;
-    float right_voltage;
+    double left_voltage;
+    double right_voltage;
     bool left_high;
     bool right_high;
   };
 
   message Status {
-    bool is_done;
     double robot_speed;
     double filtered_left_position;
     double filtered_right_position;
+
+    double uncapped_left_voltage;
+    double uncapped_right_voltage;
+    bool output_was_capped;
+
+    bool is_done;
   };
 
   queue Goal goal;