Beispiel #1
0
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
Beispiel #2
0
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
Beispiel #3
0
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