Converted state feedback controller and plant over to being gain scheduled, and used them in the indexer. Also wrote the python to make it easy to design gain scheduled controllers.
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
index 4053eae..72597c6 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -253,6 +253,11 @@
status->ready_to_intake = false;
+ // Set the controller to use to be the one designed for the current number of
+ // discs in the hopper. This is safe since the controller prevents the index
+ // from being set out of bounds and picks the closest controller.
+ wrist_loop_->set_controller_index(frisbees_.size());
+
// Compute a safe index position that we can use.
if (position) {
wrist_loop_->Y << position->index_position;