commit 06ee48e099a08ac07977e50e939447899f64617e
author Austin Schuh <aschuh@localhost> Sat Mar 02 01:47:54 2013 -0800
committer Austin Schuh <austin.linux@gmail.com> Tue Mar 05 21:34:45 2013 -0800
tree cfcad9963407dc6c98566963c0b26e8735f42cdd
parent b1cdb38fdd4c4db4a94e0f271b29e5f32275cfbd

The wrist goal can no longer run away.

frc971/control_loops/state_feedback_loop.h

diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index cf3d7e4..c49ca44 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -89,6 +89,7 @@
   Eigen::Matrix<double, number_of_states, 1> X_hat;
   Eigen::Matrix<double, number_of_states, 1> R;
   Eigen::Matrix<double, number_of_inputs, 1> U;
+  Eigen::Matrix<double, number_of_inputs, 1> U_uncapped;
   Eigen::Matrix<double, number_of_outputs, 1> U_ff;
   Eigen::Matrix<double, number_of_outputs, 1> Y;
 
@@ -106,6 +107,7 @@
     X_hat.setZero();
     R.setZero();
     U.setZero();
+    U_uncapped.setZero();
     U_ff.setZero();
     Y.setZero();
   }
@@ -137,7 +139,7 @@
         U[i] = 0.0;
       }
     } else {
-      U.noalias() = K * (R - X_hat);
+      U = U_uncapped = K * (R - X_hat);
       CapU();
     }