def test_swerve_init(wpilib, hal_data): swerve = SwerveModule(0, 1) # Check that the drive and steer motor controllers have been created assert swerve._drive and isinstance(swerve._drive, wpilib.CANTalon) assert swerve._steer and isinstance(swerve._steer, wpilib.CANTalon) assert hal_data['CAN'][1]['feedback_device'] == wpilib.CANTalon.FeedbackDevice.AnalogEncoder # Test in relative encoder mode swerve = SwerveModule(2, 3, False) assert hal_data['CAN'][3]['feedback_device'] == wpilib.CANTalon.FeedbackDevice.QuadEncoder
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
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