commit 64433f13e6588ca974b9987ffb3904c4b3efcfca
author Austin Schuh <austin.linux@gmail.com> Mon Feb 21 19:40:38 2022 -0800
committer Austin Schuh <austin.linux@gmail.com> Wed Feb 23 20:28:55 2022 -0800
tree cf8f607d39d3c1a943d5313c8364974fdc1a752e
parent 03c02ec109d198bb3f2f966545f8427960c483a0

Add support for modeling the output delay

The FPGA adds a 1 cycle delay to everything.  We should model that
in our kalman filter for more accuracy during highly dynamic situations
(like a catapult fire).

Change-Id: I41efa81c6ab474904d6eb02c85a5e238c498f226
Signed-off-by: Austin Schuh <austin.linux@gmail.com>

y2014/control_loops/python/shooter.py

diff --git a/y2014/control_loops/python/shooter.py b/y2014/control_loops/python/shooter.py
index 710233b..c3e8d86 100755
--- a/y2014/control_loops/python/shooter.py
+++ b/y2014/control_loops/python/shooter.py
@@ -217,7 +217,8 @@
         U = sprung_shooter.K * (R - sprung_shooter.X_hat)
         U = ClipDeltaU(sprung_shooter, voltage, U)
         sprung_shooter.Y = raw_sprung_shooter.Y + 0.01
-        sprung_shooter.UpdateObserver(U)
+        sprung_shooter.CorrectObserver(U)
+        sprung_shooter.PredictObserver(U)
         voltage += U
         raw_sprung_shooter.Update(voltage)
         close_loop_x.append(raw_sprung_shooter.X[0, 0] * 10)
@@ -240,7 +241,8 @@
         U = shooter.K * (R - shooter.X_hat)
         U = ClipDeltaU(shooter, voltage, U)
         shooter.Y = raw_shooter.Y + 0.01
-        shooter.UpdateObserver(U)
+        shooter.CorrectObserver(U)
+        shooter.PredictObserver(U)
         voltage += U
         raw_shooter.Update(voltage)
         close_loop_x.append(raw_shooter.X[0, 0] * 10)