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/generate_physics.cc b/frc971/control_loops/swerve/generate_physics.cc
index 029f624..8a10f89 100644
--- a/frc971/control_loops/swerve/generate_physics.cc
+++ b/frc971/control_loops/swerve/generate_physics.cc
@@ -734,7 +734,7 @@
add_dense_dense(angle_cross(temp_matrix, omega_), robot_velocity,
temp_matrix2);
mul_dense_dense(R(add(theta_, result.thetas)),
- DenseMatrix(2, 1, {caster_, integer(0)}), temp_matrix);
+ DenseMatrix(2, 1, {neg(caster_), integer(0)}), temp_matrix);
add_dense_dense(temp_matrix2,
angle_cross(temp_matrix, add(omega_, result.omegas)),
result.contact_patch_velocity);