commit fb0e0ae6e502c59a42e6ca68731bed9843f1fef7
author James Kuszmaul <james@hp-debian> Tue Mar 25 07:04:47 2014 -0700
committer James Kuszmaul <james@hp-debian> Tue Mar 25 07:04:47 2014 -0700
tree db893fae26b533115ce092590b327462911cafe5
parent fe6d37ef91684770fb3790d170226cb788d2015a

Separated CoerceGoal out from drivetrain.

frc971/control_loops/drivetrain/drivetrain.cc

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()