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/python/catapult_lib.py b/y2022/control_loops/python/catapult_lib.py
index d6040d1..4a88b14 100644
--- a/y2022/control_loops/python/catapult_lib.py
+++ b/y2022/control_loops/python/catapult_lib.py
@@ -15,9 +15,9 @@
class Catapult(angular_system.AngularSystem):
def __init__(self, params, name="Catapult"):
super(Catapult, self).__init__(params, name)
- # Signal that we have a single cycle output delay to compensate for in
+ # Signal that we have a 2 cycle output delay to compensate for in
# our observer.
- self.delayed_u = True
+ self.delayed_u = 2
self.InitializeState()
@@ -25,9 +25,9 @@
class IntegralCatapult(angular_system.IntegralAngularSystem):
def __init__(self, params, name="IntegralCatapult"):
super(IntegralCatapult, self).__init__(params, name=name)
- # Signal that we have a single cycle output delay to compensate for in
+ # Signal that we have a 2 cycle output delay to compensate for in
# our observer.
- self.delayed_u = True
+ self.delayed_u = 2
self.InitializeState()
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),