diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 9db00b5..9d7e223 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -224,6 +224,48 @@
   }
 }
 
+// Tests that the robot successfully drives straight forward.
+// This used to not work due to a U-capping bug.
+TEST_F(DrivetrainTest, DriveStraightForward) {
+  my_drivetrain_queue_.goal.MakeWithBuilder()
+      .control_loop_driving(true)
+      .left_goal(4.0)
+      .right_goal(4.0)
+      .Send();
+  for (int i = 0; i < 500; ++i) {
+    drivetrain_motor_plant_.SendPositionMessage();
+    drivetrain_motor_.Iterate();
+    drivetrain_motor_plant_.Simulate();
+    SimulateTimestep(true);
+    ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
+    EXPECT_FLOAT_EQ(my_drivetrain_queue_.output->left_voltage,
+                    my_drivetrain_queue_.output->right_voltage);
+    EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -3);
+    EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -3);
+  }
+  VerifyNearGoal();
+}
+
+// Tests that the robot successfully drives close to straight.
+// This used to fail in simulation due to libcdd issues with U-capping.
+TEST_F(DrivetrainTest, DriveAlmostStraightForward) {
+  my_drivetrain_queue_.goal.MakeWithBuilder()
+      .control_loop_driving(true)
+      .left_goal(4.0)
+      .right_goal(3.9)
+      .Send();
+  for (int i = 0; i < 500; ++i) {
+    drivetrain_motor_plant_.SendPositionMessage();
+    drivetrain_motor_.Iterate();
+    drivetrain_motor_plant_.Simulate();
+    SimulateTimestep(true);
+    ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
+    EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -3);
+    EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -3);
+  }
+  VerifyNearGoal();
+}
+
 ::aos::controls::HPolytope<2> MakeBox(double x1_min, double x1_max,
                                       double x2_min, double x2_max) {
   Eigen::Matrix<double, 4, 2> box_H;
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 049b576..9fdfe1c 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -18,98 +18,95 @@
 DrivetrainMotorsSS::LimitedDrivetrainLoop::LimitedDrivetrainLoop(
     StateFeedbackLoop<4, 2, 2> &&loop)
     : StateFeedbackLoop<4, 2, 2>(::std::move(loop)),
-      U_Poly_((Eigen::Matrix<double, 4, 2>() << 1, 0, -1, 0, 0, 1, 0, -1)
+      U_poly_((Eigen::Matrix<double, 4, 2>() << 1, 0, -1, 0, 0, 1, 0, -1)
                   .finished(),
               (Eigen::Matrix<double, 4, 1>() << 12.0, 12.0, 12.0, 12.0)
                   .finished()) {
   ::aos::controls::HPolytope<0>::Init();
-  T << 1, -1, 1, 1;
-  T_inverse = T.inverse();
+  T_ << 1, 1, 1, -1;
+  T_inverse_ = T_.inverse();
 }
 
+// This intentionally runs the U-capping code even when it's unnecessary to help
+// make it more deterministic. Only running it when one or both sides want
+// out-of-range voltages could lead to things like running out of CPU under
+// certain situations, which would be bad.
 void DrivetrainMotorsSS::LimitedDrivetrainLoop::CapU() {
+  output_was_capped_ = ::std::abs(U(0, 0)) > 12.0 || ::std::abs(U(1, 0)) > 12.0;
+
   const Eigen::Matrix<double, 4, 1> error = R() - X_hat();
 
-  if (::std::abs(U(0, 0)) > 12.0 || ::std::abs(U(1, 0)) > 12.0) {
-    mutable_U() =
-        U() * 12.0 / ::std::max(::std::abs(U(0, 0)), ::std::abs(U(1, 0)));
-    LOG_MATRIX(DEBUG, "U is now", U());
-    // TODO(Austin): Figure out why the polytope stuff wasn't working and
-    // remove this hack.
-    output_was_capped_ = true;
-    return;
+  LOG_MATRIX(DEBUG, "U at start", U());
+  LOG_MATRIX(DEBUG, "R at start", R());
+  LOG_MATRIX(DEBUG, "Xhat at start", X_hat());
 
-    LOG_MATRIX(DEBUG, "U at start", U());
-    LOG_MATRIX(DEBUG, "R at start", R());
-    LOG_MATRIX(DEBUG, "Xhat at start", X_hat());
+  Eigen::Matrix<double, 2, 2> position_K;
+  position_K << K(0, 0), K(0, 2), K(1, 0), K(1, 2);
+  Eigen::Matrix<double, 2, 2> velocity_K;
+  velocity_K << K(0, 1), K(0, 3), K(1, 1), K(1, 3);
 
-    Eigen::Matrix<double, 2, 2> position_K;
-    position_K << K(0, 0), K(0, 2), K(1, 0), K(1, 2);
-    Eigen::Matrix<double, 2, 2> velocity_K;
-    velocity_K << K(0, 1), K(0, 3), K(1, 1), K(1, 3);
+  Eigen::Matrix<double, 2, 1> position_error;
+  position_error << error(0, 0), error(2, 0);
+  // drive_error = [total_distance_error, left_error - right_error]
+  const auto drive_error = T_inverse_ * position_error;
+  Eigen::Matrix<double, 2, 1> velocity_error;
+  velocity_error << error(1, 0), error(3, 0);
+  LOG_MATRIX(DEBUG, "error", error);
 
-    Eigen::Matrix<double, 2, 1> position_error;
-    position_error << error(0, 0), error(2, 0);
-    const auto drive_error = T_inverse * position_error;
-    Eigen::Matrix<double, 2, 1> velocity_error;
-    velocity_error << error(1, 0), error(3, 0);
-    LOG_MATRIX(DEBUG, "error", error);
+  const ::aos::controls::HPolytope<2> pos_poly(U_poly_, position_K * T_,
+                                               -velocity_K * velocity_error);
 
-    const auto &poly = U_Poly_;
-    const Eigen::Matrix<double, 4, 2> pos_poly_H = poly.H() * position_K * T;
-    const Eigen::Matrix<double, 4, 1> pos_poly_k =
-        poly.k() - poly.H() * velocity_K * velocity_error;
-    const ::aos::controls::HPolytope<2> pos_poly(pos_poly_H, pos_poly_k);
+  Eigen::Matrix<double, 2, 1> adjusted_pos_error;
+  {
+    const auto &P = drive_error;
 
-    Eigen::Matrix<double, 2, 1> adjusted_pos_error;
-    {
-      const auto &P = drive_error;
+    Eigen::Matrix<double, 1, 2> L45;
+    L45 << ::aos::sign(P(1, 0)), -::aos::sign(P(0, 0));
+    const double w45 = 0;
 
-      Eigen::Matrix<double, 1, 2> L45;
-      L45 << ::aos::sign(P(1, 0)), -::aos::sign(P(0, 0));
-      const double w45 = 0;
+    Eigen::Matrix<double, 1, 2> LH;
+    if (::std::abs(P(0, 0)) > ::std::abs(P(1, 0))) {
+      LH << 0, 1;
+    } else {
+      LH << 1, 0;
+    }
+    const double wh = LH.dot(P);
 
-      Eigen::Matrix<double, 1, 2> LH;
-      if (::std::abs(P(0, 0)) > ::std::abs(P(1, 0))) {
-        LH << 0, 1;
-      } else {
-        LH << 1, 0;
-      }
-      const double wh = LH.dot(P);
+    Eigen::Matrix<double, 2, 2> standard;
+    standard << L45, LH;
+    Eigen::Matrix<double, 2, 1> W;
+    W << w45, wh;
+    const Eigen::Matrix<double, 2, 1> intersection = standard.inverse() * W;
 
-      Eigen::Matrix<double, 2, 2> standard;
-      standard << L45, LH;
-      Eigen::Matrix<double, 2, 1> W;
-      W << w45, wh;
-      const Eigen::Matrix<double, 2, 1> intersection = standard.inverse() * W;
-
-      bool is_inside_h;
-      const auto adjusted_pos_error_h =
-          DoCoerceGoal(pos_poly, LH, wh, drive_error, &is_inside_h);
-      const auto adjusted_pos_error_45 =
-          DoCoerceGoal(pos_poly, L45, w45, intersection, nullptr);
-      if (pos_poly.IsInside(intersection)) {
-        adjusted_pos_error = adjusted_pos_error_h;
-      } else {
-        if (is_inside_h) {
-          if (adjusted_pos_error_h.norm() > adjusted_pos_error_45.norm() ||
-              adjusted_pos_error_45.norm() > intersection.norm()) {
-            adjusted_pos_error = adjusted_pos_error_h;
-          } else {
-            adjusted_pos_error = adjusted_pos_error_45;
-          }
+    bool is_inside_h, is_inside_45;
+    const auto adjusted_pos_error_h =
+        DoCoerceGoal(pos_poly, LH, wh, drive_error, &is_inside_h);
+    const auto adjusted_pos_error_45 =
+        DoCoerceGoal(pos_poly, L45, w45, intersection, &is_inside_45);
+    if (pos_poly.IsInside(intersection)) {
+      adjusted_pos_error = adjusted_pos_error_h;
+    } else {
+      if (is_inside_h) {
+        if (adjusted_pos_error_h.norm() > adjusted_pos_error_45.norm() ||
+            adjusted_pos_error_45.norm() > intersection.norm()) {
+          adjusted_pos_error = adjusted_pos_error_h;
         } else {
           adjusted_pos_error = adjusted_pos_error_45;
         }
+      } else {
+        adjusted_pos_error = adjusted_pos_error_45;
       }
     }
+  }
 
-    LOG_MATRIX(DEBUG, "adjusted_pos_error", adjusted_pos_error);
-    mutable_U() =
-        velocity_K * velocity_error + position_K * T * adjusted_pos_error;
-    LOG_MATRIX(DEBUG, "U is now", U());
-  } else {
-    output_was_capped_ = false;
+  mutable_U() =
+      velocity_K * velocity_error + position_K * T_ * adjusted_pos_error;
+  LOG_MATRIX(DEBUG, "U is now", U());
+
+  if (!output_was_capped_) {
+    if ((U() - U_uncapped()).norm() > 0.0001) {
+      LOG(FATAL, "U unnecessarily capped\n");
+    }
   }
 }
 
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.h b/frc971/control_loops/drivetrain/ssdrivetrain.h
index 1ed3eb7..193dbb2 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.h
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.h
@@ -27,9 +27,15 @@
    private:
     void CapU() override;
 
-    const ::aos::controls::HPolytope<2> U_Poly_;
-    Eigen::Matrix<double, 2, 2> T, T_inverse;
-    bool output_was_capped_ = false;;
+    // Reprsents +/- full power on each motor in U-space, aka the square from
+    // (-12, -12) to (12, 12).
+    const ::aos::controls::HPolytope<2> U_poly_;
+
+    // multiplying by T converts [left_error, right_error] to
+    // [left_right_error_difference, total_distance_error].
+    Eigen::Matrix<double, 2, 2> T_, T_inverse_;
+
+    bool output_was_capped_ = false;
   };
 
   DrivetrainMotorsSS(const DrivetrainConfig &dt_config);