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>

frc971/control_loops/python/drivetrain.py

diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index 4069036..264b4a6 100644
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -544,7 +544,8 @@
     for _ in range(300):
         U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat), drivetrain.U_min,
                        drivetrain.U_max)
-        drivetrain.UpdateObserver(U)
+        drivetrain.CorrectObserver(U)
+        drivetrain.PredictObserver(U)
         drivetrain.Update(U + numpy.matrix([[1.0], [1.0]]))
         close_loop_left.append(drivetrain.X[0, 0])
         close_loop_right.append(drivetrain.X[2, 0])
@@ -588,7 +589,8 @@
     for _ in range(300):
         U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat), drivetrain.U_min,
                        drivetrain.U_max)
-        drivetrain.UpdateObserver(U)
+        drivetrain.CorrectObserver(U)
+        drivetrain.PredictObserver(U)
         drivetrain.Update(U)
         close_loop_left.append(drivetrain.X[0, 0])
         close_loop_right.append(drivetrain.X[2, 0])
@@ -612,7 +614,8 @@
     for _ in range(200):
         U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat), drivetrain.U_min,
                        drivetrain.U_max)
-        drivetrain.UpdateObserver(U)
+        drivetrain.CorrectObserver(U)
+        drivetrain.PredictObserver(U)
         drivetrain.Update(U)
         close_loop_left.append(drivetrain.X[0, 0])
         close_loop_right.append(drivetrain.X[2, 0])
@@ -633,7 +636,8 @@
     for _ in range(300):
         U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat), drivetrain.U_min,
                        drivetrain.U_max)
-        drivetrain.UpdateObserver(U)
+        drivetrain.CorrectObserver(U)
+        drivetrain.PredictObserver(U)
         drivetrain.Update(U)
         close_loop_left.append(drivetrain.X[0, 0])
         close_loop_right.append(drivetrain.X[2, 0])