Fix a drivetrain polytope bug
When both lines intersect the box, the code was assuming that the
intersection was farther from the origin than the box. However, that's
not true when the drivetrain has a long way to drive and a small angle
error.
This fixes the original issue which led to the polytope code for the
drivetrain being disabled, but there are other numerical precision
issues that are now showing up, so it's still not enabled.
Change-Id: I99a1299e538a395685c2e7e555dfccab48055349
diff --git a/y2014/control_loops/drivetrain/ssdrivetrain.cc b/y2014/control_loops/drivetrain/ssdrivetrain.cc
index 9576bcd..45ab4eb 100644
--- a/y2014/control_loops/drivetrain/ssdrivetrain.cc
+++ b/y2014/control_loops/drivetrain/ssdrivetrain.cc
@@ -92,7 +92,8 @@
adjusted_pos_error = adjusted_pos_error_h;
} else {
if (is_inside_h) {
- if (adjusted_pos_error_h.norm() > adjusted_pos_error_45.norm()) {
+ if (adjusted_pos_error_h.norm() > adjusted_pos_error_45.norm() ||
+ adjusted_pos_error_45.norm() > intersection.norm()) {
adjusted_pos_error = adjusted_pos_error_h;
} else {
adjusted_pos_error = adjusted_pos_error_45;