Fix caster to be to the back in the swerve physcis
We were computing the moments as though caster was in -x in module
coordinates, but computing the slip angle/ratio as though it was in +x.
Fix the tests to catch this.
Change-Id: Iab7ba62c5ddedd11a4b1d667d1bc392ac46721ec
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 1236789..7f35f56 100644
--- a/frc971/control_loops/swerve/physics_test.py
+++ b/frc971/control_loops/swerve/physics_test.py
@@ -67,7 +67,7 @@
return lambda X, U: numpy.array(evaluated_fn(X, U))
-class TestHPolytope(unittest.TestCase):
+class TestSwervePhysics(unittest.TestCase):
I = numpy.zeros((8, 1))
def setUp(self):
@@ -137,7 +137,7 @@
assert_array_almost_equal(
patch_velocity,
- (dynamics.ROBOT_WIDTH / numpy.sqrt(2.0) +
+ (dynamics.ROBOT_WIDTH / numpy.sqrt(2.0) -
dynamics.CASTER) * omega * numpy.array([[
-numpy.sin(theta + module_center_of_mass_angle)
], [numpy.cos(theta + module_center_of_mass_angle)]]))
@@ -154,8 +154,7 @@
self.I)
assert_array_almost_equal(
- patch_velocity,
- dynamics.CASTER * omega *
+ patch_velocity, -dynamics.CASTER * omega *
numpy.array([[-numpy.sin(theta)], [numpy.cos(theta)]]))
# Now, test that the rotation back to wheel coordinates works.