tweaked the slope of the line to make catching work better
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index cbd4d2a..4baabb6 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -132,7 +132,7 @@
{
const auto &P = position_error;
Eigen::Matrix<double, 1, 2> L45;
- L45 << sign(P(1, 0)), -sign(P(0, 0));
+ L45 << sign(P(1, 0)) * ::std::sqrt(3), -sign(P(0, 0));
const double w45 = 0;
Eigen::Matrix<double, 1, 2> LH;
diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index 9a24e22..5c275be 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -340,7 +340,7 @@
P = position_error
#K = numpy.matrix([[position_error[1, 0], -position_error[0, 0]]])
- L45 = numpy.matrix([[numpy.sign(P[1, 0]), -numpy.sign(P[0, 0])]])
+ L45 = numpy.matrix([[numpy.sign(P[1, 0]) * numpy.sqrt(3), -numpy.sign(P[0, 0])]])
if L45[0, 1] == 0:
L45[0, 1] = 1
if L45[0, 0] == 0:
@@ -548,6 +548,15 @@
R = numpy.matrix([[2.0], [0.05], [0.0], [0.0]])
run_test(claw, initial_X, R)
+ # Test a small separation error and a large position one.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[-0.5], [1.0], [0.0], [0.0]])
+ run_test(claw, initial_X, R)
+
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[-0.05], [2.0], [0.0], [0.0]])
+ run_test(claw, initial_X, R, show_graph=True)
+
# Write the generated constants out to a file.
if len(argv) != 3:
print "Expected .h file name and .cc file name for the claw."