[robotpy] Port more wpilibc tests to python

wpilibc/src/test/python/drive/test_mecanum_drive.py

@@ -0,0 +1,242 @@
+import math
+
+import pytest
+from wpimath import Rotation2d
+
+from wpilib import MecanumDrive
+
+
+class MockMotorController:
+    def __init__(self):
+        self.throttle = 0
+
+    def setThrottle(self, throttle):
+        self.throttle = throttle
+
+    def getThrottle(self):
+        return self.throttle
+
+
+def test_cartesian_ik():
+    # Forward
+    v = MecanumDrive.driveCartesianIK(1.0, 0.0, 0.0)
+    assert v.frontLeft == pytest.approx(1.0)
+    assert v.frontRight == pytest.approx(1.0)
+    assert v.rearLeft == pytest.approx(1.0)
+    assert v.rearRight == pytest.approx(1.0)
+
+    # Left
+    v = MecanumDrive.driveCartesianIK(0.0, -1.0, 0.0)
+    assert v.frontLeft == pytest.approx(-1.0)
+    assert v.frontRight == pytest.approx(1.0)
+    assert v.rearLeft == pytest.approx(1.0)
+    assert v.rearRight == pytest.approx(-1.0)
+
+    # Right
+    v = MecanumDrive.driveCartesianIK(0.0, 1.0, 0.0)
+    assert v.frontLeft == pytest.approx(1.0)
+    assert v.frontRight == pytest.approx(-1.0)
+    assert v.rearLeft == pytest.approx(-1.0)
+    assert v.rearRight == pytest.approx(1.0)
+
+    # Rotate CCW
+    v = MecanumDrive.driveCartesianIK(0.0, 0.0, -1.0)
+    assert v.frontLeft == pytest.approx(-1.0)
+    assert v.frontRight == pytest.approx(1.0)
+    assert v.rearLeft == pytest.approx(-1.0)
+    assert v.rearRight == pytest.approx(1.0)
+
+    # Rotate CW
+    v = MecanumDrive.driveCartesianIK(0.0, 0.0, 1.0)
+    assert v.frontLeft == pytest.approx(1.0)
+    assert v.frontRight == pytest.approx(-1.0)
+    assert v.rearLeft == pytest.approx(1.0)
+    assert v.rearRight == pytest.approx(-1.0)
+
+
+def test_cartesian_ik_gyro_90_cw():
+    gyro = Rotation2d.fromDegrees(90)
+
+    # Forward in global frame; left in robot frame
+    v = MecanumDrive.driveCartesianIK(1.0, 0.0, 0.0, gyro)
+    assert v.frontLeft == pytest.approx(-1.0)
+    assert v.frontRight == pytest.approx(1.0)
+    assert v.rearLeft == pytest.approx(1.0)
+    assert v.rearRight == pytest.approx(-1.0)
+
+    # Left in global frame; backward in robot frame
+    v = MecanumDrive.driveCartesianIK(0.0, -1.0, 0.0, gyro)
+    assert v.frontLeft == pytest.approx(-1.0)
+    assert v.frontRight == pytest.approx(-1.0)
+    assert v.rearLeft == pytest.approx(-1.0)
+    assert v.rearRight == pytest.approx(-1.0)
+
+    # Right in global frame; forward in robot frame
+    v = MecanumDrive.driveCartesianIK(0.0, 1.0, 0.0, gyro)
+    assert v.frontLeft == pytest.approx(1.0)
+    assert v.frontRight == pytest.approx(1.0)
+    assert v.rearLeft == pytest.approx(1.0)
+    assert v.rearRight == pytest.approx(1.0)
+
+    # Rotate CCW
+    v = MecanumDrive.driveCartesianIK(0.0, 0.0, -1.0, gyro)
+    assert v.frontLeft == pytest.approx(-1.0)
+    assert v.frontRight == pytest.approx(1.0)
+    assert v.rearLeft == pytest.approx(-1.0)
+    assert v.rearRight == pytest.approx(1.0)
+
+    # Rotate CW
+    v = MecanumDrive.driveCartesianIK(0.0, 0.0, 1.0, gyro)
+    assert v.frontLeft == pytest.approx(1.0)
+    assert v.frontRight == pytest.approx(-1.0)
+    assert v.rearLeft == pytest.approx(1.0)
+    assert v.rearRight == pytest.approx(-1.0)
+
+
+def test_cartesian(wpilib_state):
+    fl = MockMotorController()
+    rl = MockMotorController()
+    fr = MockMotorController()
+    rr = MockMotorController()
+    drive = MecanumDrive(
+        fl.setThrottle,
+        rl.setThrottle,
+        fr.setThrottle,
+        rr.setThrottle,
+    )
+    drive.setDeadband(0.0)
+
+    # Forward
+    drive.driveCartesian(1.0, 0.0, 0.0)
+    assert fl.getThrottle() == pytest.approx(1.0)
+    assert fr.getThrottle() == pytest.approx(1.0)
+    assert rl.getThrottle() == pytest.approx(1.0)
+    assert rr.getThrottle() == pytest.approx(1.0)
+
+    # Left
+    drive.driveCartesian(0.0, -1.0, 0.0)
+    assert fl.getThrottle() == pytest.approx(-1.0)
+    assert fr.getThrottle() == pytest.approx(1.0)
+    assert rl.getThrottle() == pytest.approx(1.0)
+    assert rr.getThrottle() == pytest.approx(-1.0)
+
+    # Right
+    drive.driveCartesian(0.0, 1.0, 0.0)
+    assert fl.getThrottle() == pytest.approx(1.0)
+    assert fr.getThrottle() == pytest.approx(-1.0)
+    assert rl.getThrottle() == pytest.approx(-1.0)
+    assert rr.getThrottle() == pytest.approx(1.0)
+
+    # Rotate CCW
+    drive.driveCartesian(0.0, 0.0, -1.0)
+    assert fl.getThrottle() == pytest.approx(-1.0)
+    assert fr.getThrottle() == pytest.approx(1.0)
+    assert rl.getThrottle() == pytest.approx(-1.0)
+    assert rr.getThrottle() == pytest.approx(1.0)
+
+    # Rotate CW
+    drive.driveCartesian(0.0, 0.0, 1.0)
+    assert fl.getThrottle() == pytest.approx(1.0)
+    assert fr.getThrottle() == pytest.approx(-1.0)
+    assert rl.getThrottle() == pytest.approx(1.0)
+    assert rr.getThrottle() == pytest.approx(-1.0)
+
+
+def test_cartesian_gyro_90_cw(wpilib_state):
+    fl = MockMotorController()
+    rl = MockMotorController()
+    fr = MockMotorController()
+    rr = MockMotorController()
+    drive = MecanumDrive(
+        fl.setThrottle,
+        rl.setThrottle,
+        fr.setThrottle,
+        rr.setThrottle,
+    )
+    drive.setDeadband(0.0)
+
+    gyro = Rotation2d.fromDegrees(90)
+
+    # Forward in global frame; left in robot frame
+    drive.driveCartesian(1.0, 0.0, 0.0, gyro)
+    assert fl.getThrottle() == pytest.approx(-1.0)
+    assert fr.getThrottle() == pytest.approx(1.0)
+    assert rl.getThrottle() == pytest.approx(1.0)
+    assert rr.getThrottle() == pytest.approx(-1.0)
+
+    # Left in global frame; backward in robot frame
+    drive.driveCartesian(0.0, -1.0, 0.0, gyro)
+    assert fl.getThrottle() == pytest.approx(-1.0)
+    assert fr.getThrottle() == pytest.approx(-1.0)
+    assert rl.getThrottle() == pytest.approx(-1.0)
+    assert rr.getThrottle() == pytest.approx(-1.0)
+
+    # Right in global frame; forward in robot frame
+    drive.driveCartesian(0.0, 1.0, 0.0, gyro)
+    assert fl.getThrottle() == pytest.approx(1.0)
+    assert fr.getThrottle() == pytest.approx(1.0)
+    assert rl.getThrottle() == pytest.approx(1.0)
+    assert rr.getThrottle() == pytest.approx(1.0)
+
+    # Rotate CCW
+    drive.driveCartesian(0.0, 0.0, -1.0, gyro)
+    assert fl.getThrottle() == pytest.approx(-1.0)
+    assert fr.getThrottle() == pytest.approx(1.0)
+    assert rl.getThrottle() == pytest.approx(-1.0)
+    assert rr.getThrottle() == pytest.approx(1.0)
+
+    # Rotate CW
+    drive.driveCartesian(0.0, 0.0, 1.0, gyro)
+    assert fl.getThrottle() == pytest.approx(1.0)
+    assert fr.getThrottle() == pytest.approx(-1.0)
+    assert rl.getThrottle() == pytest.approx(1.0)
+    assert rr.getThrottle() == pytest.approx(-1.0)
+
+
+def test_polar(wpilib_state):
+    fl = MockMotorController()
+    rl = MockMotorController()
+    fr = MockMotorController()
+    rr = MockMotorController()
+    drive = MecanumDrive(
+        fl.setThrottle,
+        rl.setThrottle,
+        fr.setThrottle,
+        rr.setThrottle,
+    )
+    drive.setDeadband(0.0)
+
+    # Forward
+    drive.drivePolar(1.0, Rotation2d.fromDegrees(0), 0.0)
+    assert fl.getThrottle() == pytest.approx(1.0)
+    assert fr.getThrottle() == pytest.approx(1.0)
+    assert rl.getThrottle() == pytest.approx(1.0)
+    assert rr.getThrottle() == pytest.approx(1.0)
+
+    # Left
+    drive.drivePolar(1.0, Rotation2d.fromDegrees(-90), 0.0)
+    assert fl.getThrottle() == pytest.approx(-1.0)
+    assert fr.getThrottle() == pytest.approx(1.0)
+    assert rl.getThrottle() == pytest.approx(1.0)
+    assert rr.getThrottle() == pytest.approx(-1.0)
+
+    # Right
+    drive.drivePolar(1.0, Rotation2d.fromDegrees(90), 0.0)
+    assert fl.getThrottle() == pytest.approx(1.0)
+    assert fr.getThrottle() == pytest.approx(-1.0)
+    assert rl.getThrottle() == pytest.approx(-1.0)
+    assert rr.getThrottle() == pytest.approx(1.0)
+
+    # Rotate CCW
+    drive.drivePolar(0.0, Rotation2d.fromDegrees(0), -1.0)
+    assert fl.getThrottle() == pytest.approx(-1.0)
+    assert fr.getThrottle() == pytest.approx(1.0)
+    assert rl.getThrottle() == pytest.approx(-1.0)
+    assert rr.getThrottle() == pytest.approx(1.0)
+
+    # Rotate CW
+    drive.drivePolar(0.0, Rotation2d.fromDegrees(0), 1.0)
+    assert fl.getThrottle() == pytest.approx(1.0)
+    assert fr.getThrottle() == pytest.approx(-1.0)
+    assert rl.getThrottle() == pytest.approx(1.0)
+    assert rr.getThrottle() == pytest.approx(-1.0)

wpilibc/src/test/python/driverstation/joystick_test_macros.py

@@ -0,0 +1,45 @@
+import pytest
+
+
+def axis_test(controller_clazz, sim_clazz, axis_name):
+    joy = controller_clazz(2)
+    joysim = sim_clazz(joy)
+
+    sim_setter_func = getattr(joysim, "set" + axis_name)
+    joy_get_func = getattr(joy, "get" + axis_name)
+
+    sim_setter_func(0.35)
+    joysim.notifyNewData()
+    assert joy_get_func() == pytest.approx(0.35, abs=0.001)
+
+
+def button_test(controller_clazz, sim_clazz, btn_name):
+    joy = controller_clazz(1)
+    joysim = sim_clazz(joy)
+
+    joy_get_func = getattr(joy, "get" + btn_name)
+    joy_get_pressed_func = getattr(joy, "get" + btn_name + "Pressed")
+    joy_get_released_func = getattr(joy, "get" + btn_name + "Released")
+
+    sim_setter_func = getattr(joysim, "set" + btn_name)
+
+    sim_setter_func(False)
+    joysim.notifyNewData()
+    assert False == joy_get_func()
+
+    # need to call pressed and released to clear flags
+    joy_get_pressed_func()
+    joy_get_released_func()
+
+    sim_setter_func(True)
+    joysim.notifyNewData()
+    assert True == joy_get_func()
+    assert True == joy_get_pressed_func()
+    assert False == joy_get_released_func()
+
+
+    sim_setter_func(False)
+    joysim.notifyNewData()
+    assert False == joy_get_func()
+    assert False == joy_get_pressed_func()
+    assert True == joy_get_released_func()

wpilibc/src/test/python/driverstation/test_driver_station.py

@@ -0,0 +1,53 @@
+import pytest
+
+from wpilib import DriverStationBackend, Joystick
+from wpilib.simulation import DriverStationSim, stepTiming
+
+
+@pytest.mark.parametrize(
+    "axes_max, buttons_max, povs_max, expected",
+    [
+        (0, 0, 0, False),
+        (1, 0, 0, True),
+        (0, 1, 0, True),
+        (0, 0, 1, True),
+        (1, 1, 1, True),
+        (4, 10, 1, True),
+    ],
+)
+def test_is_joystick_connected(wpilib_state, axes_max, buttons_max, povs_max, expected):
+    DriverStationSim.setJoystickAxesMaximumIndex(1, axes_max)
+    DriverStationSim.setJoystickButtonsMaximumIndex(1, buttons_max)
+    DriverStationSim.setJoystickPOVsMaximumIndex(1, povs_max)
+    DriverStationSim.notifyNewData()
+
+    assert DriverStationBackend.isJoystickConnected(1) == expected
+
+
+@pytest.mark.parametrize(
+    "fms_attached, silenced, expected_silenced, expected_warning", 
+    [
+        (False, True, True, ""),
+        (False, False, False, "Warning: Joystick on port 0 not available, check if all controllers are plugged in"),
+        (True, True, False, "Warning: Joystick on port 0 not available, check if all controllers are plugged in"), # FMS overrides silence
+        (True, False, False, "Warning: Joystick on port 0 not available, check if all controllers are plugged in"),
+    ],
+)
+def test_joystick_connection_warnings(
+    wpilib_state, fms_attached, silenced, expected_silenced, expected_warning, capsys
+):
+    DriverStationSim.setFmsAttached(fms_attached)
+    DriverStationSim.notifyNewData()
+    DriverStationBackend.silenceJoystickConnectionWarning(silenced)
+
+    joystick = Joystick(0)
+    joystick.getRawButton(1)
+
+    stepTiming(1.0)
+    assert (
+        DriverStationBackend.isJoystickConnectionWarningSilenced() == expected_silenced
+    )
+
+    # Capture stderr to check for warnings
+    captured = capsys.readouterr()
+    assert expected_warning in captured.err