Add catapult MPC and controller class
We want to shoot with the MPC, but we want to decelerate and reset with
a more traditional controller. So, transition to the MPC, and back to a
profiled subsystem.
This makes some tweaks to the solver to get it to converge more
reliably. There's apparently a scale factor which was scaling down the
cost matrices based on the initial p matrix, causing it to not solve
reliably... Good fun.
Change-Id: I721eeaf0b214f8f03cad3112acbef1477671e533
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2022/control_loops/superstructure/superstructure_position.fbs b/y2022/control_loops/superstructure/superstructure_position.fbs
index 8de272b..ac6f094 100644
--- a/y2022/control_loops/superstructure/superstructure_position.fbs
+++ b/y2022/control_loops/superstructure/superstructure_position.fbs
@@ -14,11 +14,14 @@
flipper_arm_left:frc971.RelativePosition (id: 4);
flipper_arm_right:frc971.RelativePosition (id: 5);
-
- // True is broken
+ // True means there is a ball in front of the sensor.
intake_beambreak_front:bool (id:6);
intake_beambreak_back:bool (id:7);
turret_beambreak:bool (id:8);
+
+ // Position of the catapult. Positive is up to fire. Zero is horizontal
+ // with the drive base.
+ catapult:frc971.PotAndAbsolutePosition (id: 9);
}
root_type Position;