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_goal.fbs b/y2022/control_loops/superstructure/superstructure_goal.fbs
index 37e63b5..ada39a2 100644
--- a/y2022/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2022/control_loops/superstructure/superstructure_goal.fbs
@@ -2,6 +2,19 @@
namespace y2022.control_loops.superstructure;
+table CatapultGoal {
+ // If true, fire! The robot will only fire when ready.
+ fire:bool (id: 0);
+
+ // The target shot position and velocity. If these are provided before fire
+ // is called, the optimizer can pre-compute the trajectory.
+ shot_position:double (id: 1);
+ shot_velocity:double (id: 2);
+
+ // The target position to return the catapult to when not shooting.
+ return_position:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 3);
+}
+
table Goal {
// Height of the climber above rest point
climber:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 0);
@@ -21,6 +34,9 @@
roller_speed_compensation:double (id: 6);
turret:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 7);
+
+ // Catapult goal state.
+ catapult:CatapultGoal (id: 8);
}
root_type Goal;