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