[maxswerve/swerveutils] Fix sign bug

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)