diff --git a/examples/maxswerve/swerveutils.py b/examples/maxswerve/swerveutils.py index 9f2df22..1212b81 100644 --- a/examples/maxswerve/swerveutils.py +++ b/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: diff --git a/examples/maxswerve/tests/test_swerveutils.py b/examples/maxswerve/tests/test_swerveutils.py new file mode 100644 index 0000000..67041e8 --- /dev/null +++ b/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)