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.cc

diff --git a/frc971/control_loops/coerce_goal.cc b/frc971/control_loops/coerce_goal.cc
index b32b590..de6b757 100644
--- a/frc971/control_loops/coerce_goal.cc
+++ b/frc971/control_loops/coerce_goal.cc
@@ -7,11 +7,13 @@
 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) {
+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) {
   if (region.IsInside(R)) {
+    if (is_inside) *is_inside = true;
     return R;
   }
   Eigen::Matrix<double, 2, 1> parallel_vector;
@@ -36,6 +38,7 @@
         min_distance_sqr = length;
       }
     }
+    if (is_inside) *is_inside = true;
     return closest_point;
   } else {
     Eigen::Matrix<double, 2, Eigen::Dynamic> region_vertices =
@@ -50,6 +53,7 @@
         min_distance = length;
       }
     }
+    if (is_inside) *is_inside = false;
     return (Eigen::Matrix<double, 2, 1>() << region_vertices(0, closest_i),
             region_vertices(1, closest_i)).finished();
   }