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/python/polydrivetrain.py

diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index f2dfdbe..2b24396 100755
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -28,9 +28,11 @@
   Returns:
     numpy.matrix (2 x 1), the point.
   """
+  return DoCoerceGoal(region, K, w, R)[0]
 
+def DoCoerceGoal(region, K, w, R):
   if region.IsInside(R):
-    return R
+    return (R, True)
 
   perpendicular_vector = K.T / numpy.linalg.norm(K)
   parallel_vector = numpy.matrix([[perpendicular_vector[1, 0]],
@@ -79,7 +81,7 @@
         min_distance = length
         closest_point = point
 
-    return closest_point
+    return (closest_point, True)
   else:
     # Find the vertex of the space that is closest to the line.
     region_vertices = region.Vertices()
@@ -92,7 +94,7 @@
         min_distance = length
         closest_point = point
 
-    return closest_point
+    return (closest_point, False)
 
 
 class VelocityDrivetrainModel(control_loop.ControlLoop):