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/BUILD b/y2022/BUILD
index 1aa4fa1..b4cac85 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -193,6 +193,7 @@
"//frc971/control_loops:pose",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//y2022/control_loops/drivetrain:polydrivetrain_plants",
+ "//y2022/control_loops/superstructure/catapult:catapult_plants",
"//y2022/control_loops/superstructure/climber:climber_plants",
"//y2022/control_loops/superstructure/intake:intake_plants",
"//y2022/control_loops/superstructure/turret:turret_plants",