def test_dont_unwind_module(hal_data):
# If the modules are "wound up" (ie have rotated full revolutions)
# they should still go to the nearest correct angle
# Set module pointing forward but with two full revolutions
swerve = SwerveModule(0, 1)
hal_data['CAN'][1]['pos'] = 2048
swerve._steer.set(2048)
swerve.steer(0.0, 1.0)
assert abs(swerve.direction - 4.0 * math.pi) < epsilon
swerve.steer(math.pi / 4.0, 1.0)
assert abs(swerve.direction - 4.25 * math.pi) < epsilon
def test_swerve_steer():
swerve = SwerveModule(0, 1)
# A call to SwerveModule.steer() with one argument should set the speed to 0
swerve._drive.set(1.0)
swerve.steer(0.1)
assert swerve.speed == 0.0
assert abs(swerve.direction - 0.1) < epsilon
# Rescale values to the range [0, 2*pi)
reset_module(swerve)
swerve.steer(2.1 * math.pi)
assert abs(swerve.direction - 0.1 * math.pi) < epsilon
reset_module(swerve)
swerve.steer(-2.1 * math.pi)
assert abs(swerve.direction - -0.1 * math.pi) < epsilon
# Make sure the swerve module calculates the quickest way to the desired heading
reset_module(swerve)
swerve.steer(math.pi, 1.0)
assert swerve.speed == -1.0
assert abs(swerve.direction) < epsilon