Worked through the shooter loop and fixed a bunch of bugs. Switched the internal state in the loop to use the 0 point of the spring, fixed problems with the shooter, and pushed calibratoin into the loop. Lots better now.
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 0384747..c58d7e5 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -314,6 +314,7 @@
void Update(bool stop_motors) {
if (stop_motors) {
U.setZero();
+ U_uncapped.setZero();
} else {
U = U_uncapped = K() * (R - X_hat);
CapU();