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/physics_test.py b/frc971/control_loops/swerve/physics_test.py
index af3dc17..6e7ddf4 100644
--- a/frc971/control_loops/swerve/physics_test.py
+++ b/frc971/control_loops/swerve/physics_test.py
@@ -677,6 +677,34 @@
self.assertAlmostEqual(Xdot[dynamics.VELOCITY_STATE_OMEGAS2, 0], 0.0)
self.assertAlmostEqual(Xdot[dynamics.VELOCITY_STATE_OMEGAS3, 0], 0.0)
+ def test_constant_torque(self):
+ """Tests that the robot exerts the same amount of torque no matter the orientation"""
+ steer_I = numpy.reshape(numpy.array([(i % 2) * 20 for i in range(8)]),
+ (8, 1))
+
+ X = utils.state_vector(velocity=numpy.array([[0.0], [0.0]]),
+ omega=0.0,
+ module_angles=[
+ 3 * numpy.pi / 4.0, -3 * numpy.pi / 4.0,
+ -numpy.pi / 4.0, numpy.pi / 4.0
+ ],
+ drive_wheel_velocity=1.0)
+ Xdot = self.swerve_full_dynamics(X, steer_I, skip_compare=True)
+
+ X_rot = utils.state_vector(velocity=numpy.array([[0.0], [0.0]]),
+ omega=0.0,
+ theta=numpy.pi,
+ module_angles=[
+ 3 * numpy.pi / 4.0, -3 * numpy.pi / 4.0,
+ -numpy.pi / 4.0, numpy.pi / 4.0
+ ],
+ drive_wheel_velocity=1.0)
+ Xdot_rot = self.swerve_full_dynamics(X_rot, steer_I, skip_compare=True)
+
+ self.assertGreater(Xdot[dynamics.STATE_OMEGA, 0], 0.0)
+ self.assertAlmostEquals(Xdot[dynamics.STATE_OMEGA, 0],
+ Xdot_rot[dynamics.STATE_OMEGA, 0])
+
if __name__ == "__main__":
unittest.main()