Add support for multiple cycles of delay for U
Falcons are best modeled as having even more delay. Sigh
Change-Id: Ia108f8cbd81572245c91e6727b0c7e46b6c15843
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2022/control_loops/superstructure/catapult/catapult.cc b/y2022/control_loops/superstructure/catapult/catapult.cc
index 2093174..bcfeb24 100644
--- a/y2022/control_loops/superstructure/catapult/catapult.cc
+++ b/y2022/control_loops/superstructure/catapult/catapult.cc
@@ -359,10 +359,13 @@
// hardware applies it, we need to run the optimizer for the position at
// the *next* control loop cycle.
- const Eigen::Vector3d next_X =
- catapult_.controller().plant().A() * catapult_.estimated_state() +
- catapult_.controller().plant().B() *
- catapult_.controller().observer().last_U();
+ Eigen::Vector3d next_X = catapult_.estimated_state();
+ for (int i = catapult_.controller().plant().coefficients().delayed_u;
+ i > 1; --i) {
+ next_X = catapult_.controller().plant().A() * next_X +
+ catapult_.controller().plant().B() *
+ catapult_.controller().observer().last_U(i - 1);
+ }
catapult_mpc_.SetState(
next_X.block<2, 1>(0, 0),