Fixed shooter inversion bug.
CapU was assuming the shoulder and shooter were decoupled.
Change-Id: Ie4467e9bbfde1d8f02146e689cec8dfd54353295
diff --git a/y2016/control_loops/superstructure/superstructure_controls.cc b/y2016/control_loops/superstructure/superstructure_controls.cc
index 0a0a825..eb6432d 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.cc
+++ b/y2016/control_loops/superstructure/superstructure_controls.cc
@@ -354,11 +354,15 @@
// Shoulder saturated
if (!disable && loop_->U(0, 0) != loop_->U_uncapped(0, 0)) {
+ LOG(DEBUG, "Moving shoulder state. U: %f, %f\n", loop_->U(0, 0),
+ loop_->U_uncapped(0, 0));
shoulder_profile_.MoveCurrentState(loop_->R().block<2, 1>(0, 0));
}
// Wrist saturated
if (!disable && loop_->U(1, 0) != loop_->U_uncapped(1, 0)) {
+ LOG(DEBUG, "Moving shooter state. U: %f, %f\n", loop_->U(1, 0),
+ loop_->U_uncapped(1, 0));
wrist_profile_.MoveCurrentState(loop_->R().block<2, 1>(2, 0));
}
}