commit cc6eed074c613bb45dec2e4cb4debe24cb8e9a69
author justinT21 <jjturcot@gmail.com> Mon Aug 26 00:15:57 2024 -0700
committer justinT21 <jjturcot@gmail.com> Mon Sep 02 21:45:34 2024 -0700
tree 1c52a5edfa290de08076bfcd2bdc202be178baeb
parent b8ffb7de076b03a56c1466837938c69ffc911397

Add sigmoid to replace abs(x) in atan2(y, x) to keep it smooth on the x-axis

Signed-off-by: justinT21 <jjturcot@gmail.com>
Change-Id: Iff4891607ad77d82716a2825f03b4ee086f0eb9f

frc971/control_loops/swerve/generate_physics.cc

diff --git a/frc971/control_loops/swerve/generate_physics.cc b/frc971/control_loops/swerve/generate_physics.cc
index e712a39..d647d4b 100644
--- a/frc971/control_loops/swerve/generate_physics.cc
+++ b/frc971/control_loops/swerve/generate_physics.cc
@@ -567,12 +567,15 @@
     result_py.emplace_back("    ])");
     result_py.emplace_back("");
     constexpr double kLogGain = 1.0 / 0.05;
+    constexpr double kAbsGain = 1.0 / 0.05;
     result_py.emplace_back("def soft_atan2(y, x):");
     result_py.emplace_back("    return casadi.arctan2(");
     result_py.emplace_back("        y,");
     result_py.emplace_back("        casadi.logsumexp(casadi.SX(numpy.array(");
-    result_py.emplace_back(absl::Substitute(
-        "            [1.0, casadi.fabs(x) * $0.0]))) / $0.0)", kLogGain));
+    result_py.emplace_back(
+        absl::Substitute("            [1.0, x * (1.0 - 2.0 / (1 + "
+                         "casadi.exp($1.0 * x))) * $0.0]))) / $0.0)",
+                         kLogGain, kAbsGain));
     result_py.emplace_back("");
 
     result_py.emplace_back("# Returns the derivative of our state vector");