diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index aa18aec..8d071dd 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -522,8 +522,8 @@
   VerifyNearGoal();
 }
 
-::aos::controls::HPolytope<2> MakeBox(double x1_min, double x1_max,
-                                      double x2_min, double x2_max) {
+::aos::controls::HVPolytope<2, 4, 4> MakeBox(double x1_min, double x1_max,
+                                             double x2_min, double x2_max) {
   Eigen::Matrix<double, 4, 2> box_H;
   box_H << /*[[*/ 1.0, 0.0 /*]*/,
       /*[*/ -1.0, 0.0 /*]*/,
@@ -535,7 +535,8 @@
       /*[*/ x2_max /*]*/,
       /*[*/ -x2_min /*]]*/;
   ::aos::controls::HPolytope<2> t_poly(box_H, box_k);
-  return t_poly;
+  return ::aos::controls::HVPolytope<2, 4, 4>(t_poly.H(), t_poly.k(),
+                                              t_poly.Vertices());
 }
 
 class CoerceGoalTest : public ::testing::Test {
@@ -545,7 +546,7 @@
 
 // WHOOOHH!
 TEST_F(CoerceGoalTest, Inside) {
-  ::aos::controls::HPolytope<2> box = MakeBox(1, 2, 1, 2);
+  ::aos::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
 
   Eigen::Matrix<double, 1, 2> K;
   K << /*[[*/ 1, -1 /*]]*/;
@@ -561,7 +562,7 @@
 }
 
 TEST_F(CoerceGoalTest, Outside_Inside_Intersect) {
-  ::aos::controls::HPolytope<2> box = MakeBox(1, 2, 1, 2);
+  ::aos::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
 
   Eigen::Matrix<double, 1, 2> K;
   K << 1, -1;
@@ -577,7 +578,7 @@
 }
 
 TEST_F(CoerceGoalTest, Outside_Inside_no_Intersect) {
-  ::aos::controls::HPolytope<2> box = MakeBox(3, 4, 1, 2);
+  ::aos::controls::HVPolytope<2, 4, 4> box = MakeBox(3, 4, 1, 2);
 
   Eigen::Matrix<double, 1, 2> K;
   K << 1, -1;
@@ -593,7 +594,7 @@
 }
 
 TEST_F(CoerceGoalTest, Middle_Of_Edge) {
-  ::aos::controls::HPolytope<2> box = MakeBox(0, 4, 1, 2);
+  ::aos::controls::HVPolytope<2, 4, 4> box = MakeBox(0, 4, 1, 2);
 
   Eigen::Matrix<double, 1, 2> K;
   K << -1, 1;
@@ -609,7 +610,7 @@
 }
 
 TEST_F(CoerceGoalTest, PerpendicularLine) {
-  ::aos::controls::HPolytope<2> box = MakeBox(1, 2, 1, 2);
+  ::aos::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
 
   Eigen::Matrix<double, 1, 2> K;
   K << 1, 1;
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index 121a231..6d3de59 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -16,10 +16,6 @@
 namespace control_loops {
 namespace drivetrain {
 
-using ::frc971::control_loops::GearLogging;
-using ::frc971::control_loops::CIMLogging;
-using ::frc971::control_loops::CoerceGoal;
-
 PolyDrivetrain::PolyDrivetrain(const DrivetrainConfig &dt_config,
                                StateFeedbackLoop<7, 2, 3> *kf)
     : kf_(kf),
@@ -30,7 +26,9 @@
               (Eigen::Matrix<double, 4, 1>() << /*[[*/ 12 /*]*/,
                /*[*/ 12 /*]*/,
                /*[*/ 12 /*]*/,
-               /*[*/ 12 /*]]*/).finished()),
+               /*[*/ 12 /*]]*/).finished(),
+              (Eigen::Matrix<double, 2, 4>() << /*[[*/ 12, 12, -12, -12 /*]*/,
+               /*[*/ -12, 12, 12, -12 /*]*/).finished()),
       loop_(new StateFeedbackLoop<2, 2, 2>(dt_config.make_v_drivetrain_loop())),
       ttrust_(1.1),
       wheel_(0.0),
@@ -239,13 +237,16 @@
       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());
+      ::aos::controls::HVPolytope<2, 4, 4> R_poly_hv(
+          U_Poly_.static_H() * (loop_->K() + FF),
+          U_Poly_.static_k() + U_Poly_.static_H() * loop_->K() * loop_->X_hat(),
+          (loop_->K() + FF).inverse() *
+              ::aos::controls::ShiftPoints<2, 4>(U_Poly_.StaticVertices(),
+                                                 loop_->K() * loop_->X_hat()));
 
       // Limit R back inside the box.
       loop_->mutable_R() =
-          CoerceGoal(R_poly, equality_k, equality_w, loop_->R());
+          CoerceGoal(R_poly_hv, equality_k, equality_w, loop_->R());
     }
 
     const Eigen::Matrix<double, 2, 1> FF_volts = FF * loop_->R();
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index 1ea38a7..20f1a49 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -47,7 +47,7 @@
  private:
   StateFeedbackLoop<7, 2, 3> *kf_;
 
-  const ::aos::controls::HPolytope<2> U_Poly_;
+  const ::aos::controls::HVPolytope<2, 4, 4> U_Poly_;
 
   ::std::unique_ptr<StateFeedbackLoop<2, 2, 2>> loop_;
 
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 235738d..ff7ed6e 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -13,8 +13,6 @@
 namespace control_loops {
 namespace drivetrain {
 
-using ::frc971::control_loops::DoCoerceGoal;
-
 void DrivetrainMotorsSS::ScaleCapU(Eigen::Matrix<double, 2, 1> *U) {
   output_was_capped_ =
       ::std::abs((*U)(0, 0)) > 12.0 || ::std::abs((*U)(1, 0)) > 12.0;
@@ -53,9 +51,15 @@
   Eigen::Matrix<double, 2, 1> U_integral;
   U_integral << kf_->X_hat(4, 0), kf_->X_hat(5, 0);
 
-  const ::aos::controls::HPolytope<2> pos_poly(
-      U_poly_, position_K * T_,
-      -velocity_K * velocity_error + U_integral - kf_->ff_U());
+  const ::aos::controls::HVPolytope<2, 4, 4> pos_poly_hv(
+      U_poly_.static_H() * position_K * T_,
+      U_poly_.static_H() *
+              (-velocity_K * velocity_error + U_integral - kf_->ff_U()) +
+          U_poly_.static_k(),
+      (position_K * T_).inverse() *
+          ::aos::controls::ShiftPoints<2, 4>(
+              U_poly_.StaticVertices(),
+              -velocity_K * velocity_error + U_integral - kf_->ff_U()));
 
   Eigen::Matrix<double, 2, 1> adjusted_pos_error;
   {
@@ -81,10 +85,10 @@
 
     bool is_inside_h, is_inside_45;
     const auto adjusted_pos_error_h =
-        DoCoerceGoal(pos_poly, LH, wh, drive_error, &is_inside_h);
+        DoCoerceGoal(pos_poly_hv, 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)) {
+        DoCoerceGoal(pos_poly_hv, L45, w45, intersection, &is_inside_45);
+    if (pos_poly_hv.IsInside(intersection)) {
       adjusted_pos_error = adjusted_pos_error_h;
     } else {
       if (is_inside_h) {
@@ -115,10 +119,16 @@
                                        double *integrated_kf_heading)
     : dt_config_(dt_config),
       kf_(kf),
-      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()),
+      U_poly_((Eigen::Matrix<double, 4, 2>() << /*[[*/ 1, 0 /*]*/,
+               /*[*/ -1, 0 /*]*/,
+               /*[*/ 0, 1 /*]*/,
+               /*[*/ 0, -1 /*]]*/).finished(),
+              (Eigen::Matrix<double, 4, 1>() << /*[[*/ 12 /*]*/,
+               /*[*/ 12 /*]*/,
+               /*[*/ 12 /*]*/,
+               /*[*/ 12 /*]]*/).finished(),
+              (Eigen::Matrix<double, 2, 4>() << /*[[*/ 12, 12, -12, -12 /*]*/,
+               /*[*/ -12, 12, 12, -12 /*]*/).finished()),
       linear_profile_(::aos::controls::kLoopFrequency),
       angular_profile_(::aos::controls::kLoopFrequency),
       integrated_kf_heading_(integrated_kf_heading) {
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.h b/frc971/control_loops/drivetrain/ssdrivetrain.h
index 15a4823..c066c1b 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.h
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.h
@@ -63,7 +63,7 @@
 
   // 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_;
+  const ::aos::controls::HVPolytope<2, 4, 4> U_poly_;
 
   // multiplying by T converts [left_error, right_error] to
   // [left_right_error_difference, total_distance_error].