commit fb47490f874e2e2d7f562c05421a558a2d4ca6d6
author Austin Schuh <austin.linux@gmail.com> Sun Sep 22 23:38:29 2024 -0700
committer Gerrit Code Review <gerrit@software.frc971.org> Sun Sep 22 23:38:29 2024 -0700
tree cee11677d77c286e357aeace9544441c01bb077e
parent e3b7f4e731f15a836dc7307513dc9afb276462e6
parent 62d9bde7f7dd1e54993405e34866324e00b3e56e

Merge "Fix physics debug" into main

frc971/control_loops/swerve/physics_debug.py

diff --git a/frc971/control_loops/swerve/physics_debug.py b/frc971/control_loops/swerve/physics_debug.py
index db9ea85..9a89cc3 100644
--- a/frc971/control_loops/swerve/physics_debug.py
+++ b/frc971/control_loops/swerve/physics_debug.py
@@ -10,7 +10,7 @@
 class PhysicsDebug(object):
 
     def wrap(self, python_module):
-        self.swerve_physics = utils.wrap(python_module.swerve_physics)
+        self.swerve_physics = utils.wrap(python_module.swerve_full_dynamics)
         self.contact_patch_velocity = [
             utils.wrap_module(python_module.contact_patch_velocity, i)
             for i in range(4)
@@ -102,7 +102,7 @@
                               [40.0], [0.0]])
 
         def calc_I(t, x):
-            x_goal = numpy.zeros((16, 1))
+            x_goal = numpy.zeros(16)
 
             Kp_steer = 15.0
             Kp_drive = 0.0
@@ -245,8 +245,7 @@
             U_control = numpy.hstack((
                 U_control,
                 numpy.reshape(
-                    calc_I(result.t[i], numpy.reshape(result.y[:, i],
-                                                      (25, 1))),
+                    calc_I(result.t[i], result.y[:, i]),
                     (8, 1),
                 ),
             ))