diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index e7aae01..b54949d 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -13,6 +13,7 @@
 #include "aos/common/logging/queue_logging.h"
 
 #include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/coerce_goal.h"
 #include "frc971/control_loops/drivetrain/polydrivetrain_cim_plant.h"
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "frc971/queues/gyro_angle.q.h"
@@ -26,53 +27,6 @@
 // Width of the robot.
 const double width = 22.0 / 100.0 * 2.54;
 
-Eigen::Matrix<double, 2, 1> CoerceGoal(aos::controls::HPolytope<2> &region,
-                                       const Eigen::Matrix<double, 1, 2> &K,
-                                       double w,
-                                       const Eigen::Matrix<double, 2, 1> &R) {
-  if (region.IsInside(R)) {
-    return R;
-  }
-  Eigen::Matrix<double, 2, 1> parallel_vector;
-  Eigen::Matrix<double, 2, 1> perpendicular_vector;
-  perpendicular_vector = K.transpose().normalized();
-  parallel_vector << perpendicular_vector(1, 0), -perpendicular_vector(0, 0);
-
-  aos::controls::HPolytope<1> t_poly(
-      region.H() * parallel_vector,
-      region.k() - region.H() * perpendicular_vector * w);
-
-  Eigen::Matrix<double, 1, Eigen::Dynamic> vertices = t_poly.Vertices();
-  if (vertices.innerSize() > 0) {
-    double min_distance_sqr = 0;
-    Eigen::Matrix<double, 2, 1> closest_point;
-    for (int i = 0; i < vertices.innerSize(); i++) {
-      Eigen::Matrix<double, 2, 1> point;
-      point = parallel_vector * vertices(0, i) + perpendicular_vector * w;
-      const double length = (R - point).squaredNorm();
-      if (i == 0 || length < min_distance_sqr) {
-        closest_point = point;
-        min_distance_sqr = length;
-      }
-    }
-    return closest_point;
-  } else {
-    Eigen::Matrix<double, 2, Eigen::Dynamic> region_vertices =
-        region.Vertices();
-    double min_distance = INFINITY;
-    int closest_i = 0;
-    for (int i = 0; i < region_vertices.outerSize(); i++) {
-      const double length = ::std::abs(
-          (perpendicular_vector.transpose() * (region_vertices.col(i)))(0, 0));
-      if (i == 0 || length < min_distance) {
-        closest_i = i;
-        min_distance = length;
-      }
-    }
-    return region_vertices.col(closest_i);
-  }
-}
-
 class DrivetrainMotorsSS {
  public:
   DrivetrainMotorsSS()
diff --git a/frc971/control_loops/drivetrain/drivetrain.gyp b/frc971/control_loops/drivetrain/drivetrain.gyp
index adb9f94..5941f63 100644
--- a/frc971/control_loops/drivetrain/drivetrain.gyp
+++ b/frc971/control_loops/drivetrain/drivetrain.gyp
@@ -46,6 +46,7 @@
         '<(DEPTH)/frc971/frc971.gyp:constants',
         '<(DEPTH)/aos/build/externals.gyp:libcdd',
         '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+        '<(DEPTH)/frc971/control_loops/control_loops.gyp:coerce_goal',
         '<(DEPTH)/frc971/queues/queues.gyp:queues',
         '<(AOS)/common/util/util.gyp:log_interval',
         '<(AOS)/common/logging/logging.gyp:queue_logging',
@@ -53,6 +54,7 @@
       'export_dependent_settings': [
         '<(DEPTH)/aos/build/externals.gyp:libcdd',
         '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+        '<(DEPTH)/frc971/control_loops/control_loops.gyp:coerce_goal',
         '<(AOS)/common/common.gyp:controls',
         'drivetrain_loop',
       ],
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 9ee7a27..2d5e9c9 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -12,11 +12,6 @@
 namespace frc971 {
 namespace control_loops {
 
-Eigen::Matrix<double, 2, 1> CoerceGoal(aos::controls::HPolytope<2> &region,
-                                       const Eigen::Matrix<double, 1, 2> &K,
-                                       double w,
-                                       const Eigen::Matrix<double, 2, 1> &R);
-
 class DrivetrainLoop
     : public aos::control_loops::ControlLoop<control_loops::Drivetrain, true, false> {
  public:
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index eb1ede2..73f8525 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -11,6 +11,7 @@
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "frc971/control_loops/drivetrain/drivetrain.h"
 #include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/coerce_goal.h"
 #include "frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
 #include "frc971/queues/gyro_angle.q.h"