commit 322d2d1e2e388e7ee0d89c7d4a37160d3648fd65
author Austin Schuh <austin@peloton-tech.com> Sat Apr 18 22:57:42 2015 -0700
committer Brian Silverman <brians> Thu May 14 00:41:05 2015 -0400
tree 389799279b758aab41ecef2928ff5b84938b9ee9
parent 913c2f2988a81cb0124898e346ead0be92d42902

Added hack to fix bug in polytope code.

Change-Id: I10298757d477f6c1ed335e40db67e72d9ef98982

frc971/control_loops/drivetrain/drivetrain.cc

diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 08cf99e..03e0db8 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -55,8 +55,17 @@
       const Eigen::Matrix<double, 4, 1> error = R() - X_hat();
 
       if (::std::abs(U(0, 0)) > 12.0 || ::std::abs(U(1, 0)) > 12.0) {
+        mutable_U() =
+            U() * 12.0 / ::std::max(::std::abs(U(0, 0)), ::std::abs(U(1, 0)));
+        LOG_MATRIX(DEBUG, "U is now", U());
+        // TODO(Austin): Figure out why the polytope stuff wasn't working and
+        // remove this hack.
         output_was_capped_ = true;
+        return;
+
         LOG_MATRIX(DEBUG, "U at start", U());
+        LOG_MATRIX(DEBUG, "R at start", R());
+        LOG_MATRIX(DEBUG, "Xhat at start", X_hat());
 
         Eigen::Matrix<double, 2, 2> position_K;
         position_K << K(0, 0), K(0, 2),