Fix singularities in atan2 at 0, 0 in swerve physics
The derivative of atan2 is
-y / (x^2 + y^2), x / (x^2 + y^2)
This explodes at 0, 0. Be much more gentle at 0, 0 in our calculation
to reduce the rate of change. This makes the solver converge
*significantly* faster at 0, 0. We still can't go sideways yet from 0,
0.
Change-Id: If5496714ec1fd36fac76913d208e803257dcd02f
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/control_loops/swerve/BUILD b/frc971/control_loops/swerve/BUILD
index fcd60a1..670bcc4 100644
--- a/frc971/control_loops/swerve/BUILD
+++ b/frc971/control_loops/swerve/BUILD
@@ -208,6 +208,7 @@
":physics_test_utils",
"@pip//casadi",
"@pip//numpy",
+ "@pip//scipy",
],
)