Put step number on top of LQR plots
Makes it easier to tell when they update.
Change-Id: I28b982926bd55d2f123c84befc54beb1e458cd63
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/control_loops/swerve/velocity_controller/lqr_plot.py b/frc971/control_loops/swerve/velocity_controller/lqr_plot.py
index 2bc65ff..01ebf47 100644
--- a/frc971/control_loops/swerve/velocity_controller/lqr_plot.py
+++ b/frc971/control_loops/swerve/velocity_controller/lqr_plot.py
@@ -340,6 +340,7 @@
return
self.last_trajectory_step = data.step
logging.info('Updating trajectory plots')
+ self.fig0.suptitle(f'Step {data.step}')
# Put data in the trajectory plots.
self.x.set_data(data.t, data.X[:, 0])
@@ -371,6 +372,7 @@
return
logging.info('Updating cost plots')
self.last_cost_step = data.step
+ self.costfig.suptitle(f'Step {data.step}')
# Put data in the cost plots.
if hasattr(self, 'costsurf'):
for surf in self.costsurf:
@@ -407,6 +409,7 @@
return
self.last_U_step = data.step
logging.info('Updating U plots')
+ self.Ufig.suptitle(f'Step {data.step}')
# Put data in the controller plots.
if hasattr(self, 'Usurf'):
for surf in self.Usurf: