Gain schedule elevator and wrist.
Change-Id: I4326bc494737f3592f39aee8adb7cf8d92b0dff6
diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
index 8fe02a3..b6ad9e8 100644
--- a/y2019/control_loops/superstructure/superstructure.cc
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -83,6 +83,19 @@
}
}
+ if (unsafe_goal) {
+ if (!unsafe_goal->suction.grab_piece) {
+ wrist_.set_controller_index(0);
+ elevator_.set_controller_index(0);
+ } else if (unsafe_goal->suction.gamepiece_mode == 0) {
+ wrist_.set_controller_index(1);
+ elevator_.set_controller_index(1);
+ } else {
+ wrist_.set_controller_index(2);
+ elevator_.set_controller_index(2);
+ }
+ }
+
// TODO(theo) move these up when Iterate() is split
// update the goals
collision_avoidance_.UpdateGoal(status, unsafe_goal);