Fix bug in physics and a few in physics_debug
It turns out we didn't rotate the mounting location when crossing it with force to get torque. We also didn't use calculate current using the PID when calculating the xdot after integrating it. I also added a few more plots that may be useful for debugging in the future.
Signed-off-by: justinT21 <jjturcot@gmail.com>
Change-Id: I17357833debdfd34977e9ef3f8b11812a835ddce
diff --git a/frc971/control_loops/swerve/jax_dynamics.py b/frc971/control_loops/swerve/jax_dynamics.py
index 8b05ef4..3810884 100644
--- a/frc971/control_loops/swerve/jax_dynamics.py
+++ b/frc971/control_loops/swerve/jax_dynamics.py
@@ -189,7 +189,7 @@
F = Rthetaplusthetas @ jax.numpy.array([Fwx, Fwy])
- torque = force_cross(mounting_location, F)
+ torque = force_cross(Rtheta @ mounting_location, F)
X_dot_contribution = jax.numpy.hstack((jax.numpy.zeros(
(4, )), ) * (module_index) + (jax.numpy.array([
@@ -288,7 +288,7 @@
F = Rthetaplusthetas @ jax.numpy.array([Fwx, Fwy])
- torque = force_cross(mounting_location, F)
+ torque = force_cross(Rtheta @ mounting_location, F)
X_dot_contribution = jax.numpy.hstack((jax.numpy.zeros(
(2, )), ) * (module_index) + (jax.numpy.array([