[maxswerve/swerveutils] Fix sign bug

Author: epanov1602

examples/maxswerve/swerveutils.py

@@ -41,7 +41,7 @@ def stepTowardsCircular(current: float, target: float, stepsize: float) -> float
     current = wrapAngle(current)
     target = wrapAngle(target)
 
-    stepDirection = math.copysign(target - current, 1)
+    stepDirection = math.copysign(1, target - current)
     difference = abs(current - target)
 
     if difference <= stepsize:

examples/maxswerve/tests/test_swerveutils.py

@@ -0,0 +1,18 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import swerveutils
+
+
+def test_stepTowardsCircular1():
+    current = 0.6408134451373411
+    stepsize = 0.3455804605358387
+    target = 0.0  # stepping towards zero direction
+    result = swerveutils.stepTowardsCircular(
+        current=current, stepsize=stepsize, target=target
+    )
+    # stepping towards zero angle should result in smaller absolute value
+    assert abs(result) < abs(current)