commit 6dd2c53a3206aedab068d11730e5c6d3c679aed3
author Brian Silverman <brians> Sat Mar 29 23:34:39 2014 -0700
committer Brian Silverman <brians> Sat Mar 29 23:34:39 2014 -0700
tree c037598ae8a0ecff0668b2fd88328c5a55cec497
parent ab63da3d90eedaad395b545d912f509de4479f2a

tuned and redid the capping on the claw to prioritize separation error

frc971/control_loops/coerce_goal.h

diff --git a/frc971/control_loops/coerce_goal.h b/frc971/control_loops/coerce_goal.h
index 43707b4..59049c5 100644
--- a/frc971/control_loops/coerce_goal.h
+++ b/frc971/control_loops/coerce_goal.h
@@ -8,14 +8,23 @@
 namespace frc971 {
 namespace control_loops {
 
+Eigen::Matrix<double, 2, 1> DoCoerceGoal(const aos::controls::HPolytope<2> &region,
+                                         const Eigen::Matrix<double, 1, 2> &K,
+                                         double w,
+                                         const Eigen::Matrix<double, 2, 1> &R,
+                                         bool *is_inside);
+
 // Intersects a line with a region, and finds the closest point to R.
 // Finds a point that is closest to R inside the region, and on the line
 // defined by K X = w.  If it is not possible to find a point on the line,
 // finds a point that is inside the region and closest to the line.
-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);
+static inline Eigen::Matrix<double, 2, 1>
+    CoerceGoal(const aos::controls::HPolytope<2> &region,
+               const Eigen::Matrix<double, 1, 2> &K,
+               double w,
+               const Eigen::Matrix<double, 2, 1> &R) {
+  return DoCoerceGoal(region, K, w, R, nullptr);
+}
 
 }  // namespace control_loops
 }  // namespace frc971