Compensate steer coupling torque from velocity cost
We don't want to penalize using steer current to stay straight.
Compensate it out of the cost function.
Change-Id: I649ddb1b4c94b6746ea661252964b8f85633fa5c
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/control_loops/swerve/physics_test.py b/frc971/control_loops/swerve/physics_test.py
index 9aa6457..0a3fdb6 100644
--- a/frc971/control_loops/swerve/physics_test.py
+++ b/frc971/control_loops/swerve/physics_test.py
@@ -582,6 +582,23 @@
self.assertLess(xdot[dynamics.STATE_OMEGAD2, 0], -100.0)
self.assertLess(xdot[dynamics.STATE_OMEGAD3, 0], -100.0)
+ def test_steer_coupling(self):
+ """Tests that the steer coupling factor cancels out steer coupling torque."""
+ steer_I = numpy.array(
+ [[dynamics.STEER_CURRENT_COUPLING_FACTOR * 10.0], [10.0]] * 4)
+
+ X = utils.state_vector(
+ velocity=numpy.array([[0.0], [0.0]]),
+ omega=0.0,
+ )
+ X_velocity = self.to_velocity_state(X)
+ Xdot = self.velocity_swerve_physics(X_velocity, steer_I)
+
+ self.assertAlmostEqual(Xdot[dynamics.VELOCITY_STATE_OMEGAS0, 0], 0.0)
+ self.assertAlmostEqual(Xdot[dynamics.VELOCITY_STATE_OMEGAS1, 0], 0.0)
+ self.assertAlmostEqual(Xdot[dynamics.VELOCITY_STATE_OMEGAS2, 0], 0.0)
+ self.assertAlmostEqual(Xdot[dynamics.VELOCITY_STATE_OMEGAS3, 0], 0.0)
+
if __name__ == "__main__":
unittest.main()