added unsafe distance after intaking algae

.git-blame-ignore-revs

@@ -0,0 +1,4 @@
+# dos2unix tests
+728f9f2d8ee4aa18b5dcc3d114e64fdd21904bce
+# dos2unix utilities
+498f22fb696fe7ac146c45a0d9e3c50d2abb0935

.pre-commit-config.yaml

@@ -22,7 +22,7 @@ repos:
       - id: check-yaml
 
   - repo: https://github.com/astral-sh/ruff-pre-commit
-    rev: v0.9.2
+    rev: v0.9.3
     hooks:
       - id: ruff
         args:

controllers/reef_intake.py

@@ -16,6 +16,8 @@ class ReefIntake(StateMachine):
     L2_INTAKE_ANGLE = tunable(math.radians(-40.0))
     L3_INTAKE_ANGLE = tunable(math.radians(-10.0))
 
+    RETREAT_DISTANCE = tunable(0.6)
+
     def __init__(self):
         self.last_l3 = False
 
@@ -24,9 +26,12 @@ def intake(self) -> None:
 
     @state(first=True, must_finish=True)
     def intaking(self, initial_call: bool):
-        if self.algae_manipulator_component.has_algae():
+        if self.algae_manipulator_component.has_algae() and initial_call:
             self.done()
             return
+        elif self.algae_manipulator_component.has_algae():
+            self.next_state("safing")
+            return
 
         current_is_L3 = self.is_L3()
 
@@ -39,6 +44,17 @@ def intaking(self, initial_call: bool):
 
         self.algae_manipulator_component.intake()
 
+    @state(must_finish=True)
+    def safing(self, initial_call: bool):
+        if initial_call:
+            self.origin_robot_pose = self.chassis.get_pose()
+        robot_pose = self.chassis.get_pose()
+
+        distance = self.origin_robot_pose.translation() - robot_pose.translation()
+
+        if distance.norm() >= self.RETREAT_DISTANCE:
+            self.done()
+
     @feedback
     def is_L3(self) -> bool:
         return game.is_L3(game.nearest_reef_tag(self.chassis.get_pose()))

tests/autonomous_test.py

@@ -1,18 +1,18 @@
-import hal
-import pytest
-from robotpy_ext.autonomous.selector_tests import (  # type: ignore[import-untyped]
-    test_all_autonomous as _test_all_autonomous,
-)
-from wpilib.simulation import DriverStationSim
-
-
-@pytest.mark.slow_integration_test
-@pytest.mark.parametrize("alliance", ["Red", "Blue"])
-def test_all_autonomous(control, alliance):
-    station = getattr(hal.AllianceStationID, f"k{alliance}1")
-    DriverStationSim.setAllianceStationId(station)
-
-    _test_all_autonomous(control)
-
-    # Clean up the global simulation state we set.
-    DriverStationSim.setAllianceStationId(hal.AllianceStationID.kUnknown)
+import hal
+import pytest
+from robotpy_ext.autonomous.selector_tests import (  # type: ignore[import-untyped]
+    test_all_autonomous as _test_all_autonomous,
+)
+from wpilib.simulation import DriverStationSim
+
+
+@pytest.mark.slow_integration_test
+@pytest.mark.parametrize("alliance", ["Red", "Blue"])
+def test_all_autonomous(control, alliance):
+    station = getattr(hal.AllianceStationID, f"k{alliance}1")
+    DriverStationSim.setAllianceStationId(station)
+
+    _test_all_autonomous(control)
+
+    # Clean up the global simulation state we set.
+    DriverStationSim.setAllianceStationId(hal.AllianceStationID.kUnknown)

tests/fuzz_test.py

@@ -1,146 +1,146 @@
-from __future__ import annotations
-
-import os
-import random
-import typing
-
-import hal
-import pytest
-import wpilib.simulation
-from wpilib.simulation import DriverStationSim
-
-if typing.TYPE_CHECKING:
-    from pyfrc.test_support.controller import TestController
-
-pytestmark = pytest.mark.integration_test
-
-
-def rand_bool() -> bool:
-    return random.getrandbits(1) != 0
-
-
-def rand_axis() -> float:
-    """Get a random number between -1 and 1."""
-    return random.random() * 2 - 1
-
-
-def rand_pov() -> int:
-    """Pick a random POV hat value."""
-    return random.choice((-1, 0, 45, 90, 135, 180, 225, 270, 315))
-
-
-class AllTheThings:
-    """Fuzzer for robot hardware inputs."""
-
-    def __init__(self) -> None:
-        self.dios = [
-            dio
-            for dio in map(wpilib.simulation.DIOSim, range(hal.getNumDigitalChannels()))
-            if dio.getInitialized()
-        ]
-
-    def fuzz(self) -> None:
-        for dio in self.dios:
-            if dio.getIsInput():  # pragma: no branch
-                dio.setValue(rand_bool())
-
-
-class DSInputs:
-    """Fuzzer for HIDs attached to the driver station."""
-
-    def __init__(self) -> None:
-        self.gamepad = wpilib.simulation.XboxControllerSim(0)
-        self.joystick = wpilib.simulation.JoystickSim(1)
-
-    def fuzz(self) -> None:
-        fuzz_xbox_gamepad(self.gamepad)
-        fuzz_joystick(self.joystick)
-
-
-def fuzz_joystick(joystick: wpilib.simulation.JoystickSim) -> None:
-    """Fuzz a Logitech Extreme 3D Pro flight stick."""
-    for axis in range(5):
-        joystick.setRawAxis(axis, rand_axis())
-    for button in range(12):
-        joystick.setRawButton(button, rand_bool())
-    joystick.setPOV(rand_pov())
-
-
-def fuzz_xbox_gamepad(gamepad: wpilib.simulation.XboxControllerSim) -> None:
-    """Fuzz an XInput gamepad."""
-    gamepad.setLeftX(rand_axis())
-    gamepad.setLeftY(rand_axis())
-    gamepad.setRightX(rand_axis())
-    gamepad.setRightY(rand_axis())
-    gamepad.setLeftTriggerAxis(random.random())
-    gamepad.setRightTriggerAxis(random.random())
-    for button in range(10):
-        gamepad.setRawButton(button, rand_bool())
-    gamepad.setPOV(rand_pov())
-
-
-def get_alliance_stations() -> list[str]:
-    stations = (1, 2, 3)
-    if "CI" in os.environ:  # pragma: no branch
-        return [
-            f"{alliance}{station}"
-            for alliance in ("Blue", "Red")
-            for station in stations
-        ]
-    else:  # pragma: no cover
-        return [f"Blue{random.choice(stations)}", f"Red{random.choice(stations)}"]
-
-
-@pytest.mark.parametrize("station", get_alliance_stations())
-def test_fuzz(control: TestController, station: str) -> None:
-    station_id = getattr(hal.AllianceStationID, f"k{station}")
-
-    with control.run_robot():
-        things = AllTheThings()
-        hids = DSInputs()
-
-        # Disabled mode
-        control.step_timing(seconds=0.2, autonomous=False, enabled=False)
-        DriverStationSim.setAllianceStationId(station_id)
-        things.fuzz()
-        hids.fuzz()
-        control.step_timing(seconds=0.2, autonomous=False, enabled=False)
-
-        # Autonomous mode
-        things.fuzz()
-        control.step_timing(seconds=0.2, autonomous=True, enabled=False)
-        things.fuzz()
-        control.step_timing(seconds=0.2, autonomous=True, enabled=True)
-
-        # Transition between autonomous and teleop
-        things.fuzz()
-        control.step_timing(seconds=0.2, autonomous=False, enabled=False)
-        things.fuzz()
-        control.step_timing(seconds=0.2, autonomous=False, enabled=True)
-
-        # Teleop
-        for _ in range(20):
-            things.fuzz()
-            hids.fuzz()
-            control.step_timing(seconds=0.1, autonomous=False, enabled=True)
-
-        DriverStationSim.setAllianceStationId(hal.AllianceStationID.kUnknown)
-
-
-def test_fuzz_test(control: TestController) -> None:
-    with control.run_robot():
-        hids = DSInputs()
-
-        # Start the robot in disabled mode for a short period
-        control.step_timing(seconds=0.5, autonomous=False, enabled=False)
-
-        DriverStationSim.setTest(True)
-        DriverStationSim.setEnabled(True)
-
-        assert control.robot_is_alive
-
-        for _ in range(20):
-            hids.fuzz()
-            DriverStationSim.notifyNewData()
-            wpilib.simulation.stepTiming(0.2)
-            assert control.robot_is_alive
+from __future__ import annotations
+
+import os
+import random
+import typing
+
+import hal
+import pytest
+import wpilib.simulation
+from wpilib.simulation import DriverStationSim
+
+if typing.TYPE_CHECKING:
+    from pyfrc.test_support.controller import TestController
+
+pytestmark = pytest.mark.integration_test
+
+
+def rand_bool() -> bool:
+    return random.getrandbits(1) != 0
+
+
+def rand_axis() -> float:
+    """Get a random number between -1 and 1."""
+    return random.random() * 2 - 1
+
+
+def rand_pov() -> int:
+    """Pick a random POV hat value."""
+    return random.choice((-1, 0, 45, 90, 135, 180, 225, 270, 315))
+
+
+class AllTheThings:
+    """Fuzzer for robot hardware inputs."""
+
+    def __init__(self) -> None:
+        self.dios = [
+            dio
+            for dio in map(wpilib.simulation.DIOSim, range(hal.getNumDigitalChannels()))
+            if dio.getInitialized()
+        ]
+
+    def fuzz(self) -> None:
+        for dio in self.dios:
+            if dio.getIsInput():  # pragma: no branch
+                dio.setValue(rand_bool())
+
+
+class DSInputs:
+    """Fuzzer for HIDs attached to the driver station."""
+
+    def __init__(self) -> None:
+        self.gamepad = wpilib.simulation.XboxControllerSim(0)
+        self.joystick = wpilib.simulation.JoystickSim(1)
+
+    def fuzz(self) -> None:
+        fuzz_xbox_gamepad(self.gamepad)
+        fuzz_joystick(self.joystick)
+
+
+def fuzz_joystick(joystick: wpilib.simulation.JoystickSim) -> None:
+    """Fuzz a Logitech Extreme 3D Pro flight stick."""
+    for axis in range(5):
+        joystick.setRawAxis(axis, rand_axis())
+    for button in range(12):
+        joystick.setRawButton(button, rand_bool())
+    joystick.setPOV(rand_pov())
+
+
+def fuzz_xbox_gamepad(gamepad: wpilib.simulation.XboxControllerSim) -> None:
+    """Fuzz an XInput gamepad."""
+    gamepad.setLeftX(rand_axis())
+    gamepad.setLeftY(rand_axis())
+    gamepad.setRightX(rand_axis())
+    gamepad.setRightY(rand_axis())
+    gamepad.setLeftTriggerAxis(random.random())
+    gamepad.setRightTriggerAxis(random.random())
+    for button in range(10):
+        gamepad.setRawButton(button, rand_bool())
+    gamepad.setPOV(rand_pov())
+
+
+def get_alliance_stations() -> list[str]:
+    stations = (1, 2, 3)
+    if "CI" in os.environ:  # pragma: no branch
+        return [
+            f"{alliance}{station}"
+            for alliance in ("Blue", "Red")
+            for station in stations
+        ]
+    else:  # pragma: no cover
+        return [f"Blue{random.choice(stations)}", f"Red{random.choice(stations)}"]
+
+
+@pytest.mark.parametrize("station", get_alliance_stations())
+def test_fuzz(control: TestController, station: str) -> None:
+    station_id = getattr(hal.AllianceStationID, f"k{station}")
+
+    with control.run_robot():
+        things = AllTheThings()
+        hids = DSInputs()
+
+        # Disabled mode
+        control.step_timing(seconds=0.2, autonomous=False, enabled=False)
+        DriverStationSim.setAllianceStationId(station_id)
+        things.fuzz()
+        hids.fuzz()
+        control.step_timing(seconds=0.2, autonomous=False, enabled=False)
+
+        # Autonomous mode
+        things.fuzz()
+        control.step_timing(seconds=0.2, autonomous=True, enabled=False)
+        things.fuzz()
+        control.step_timing(seconds=0.2, autonomous=True, enabled=True)
+
+        # Transition between autonomous and teleop
+        things.fuzz()
+        control.step_timing(seconds=0.2, autonomous=False, enabled=False)
+        things.fuzz()
+        control.step_timing(seconds=0.2, autonomous=False, enabled=True)
+
+        # Teleop
+        for _ in range(20):
+            things.fuzz()
+            hids.fuzz()
+            control.step_timing(seconds=0.1, autonomous=False, enabled=True)
+
+        DriverStationSim.setAllianceStationId(hal.AllianceStationID.kUnknown)
+
+
+def test_fuzz_test(control: TestController) -> None:
+    with control.run_robot():
+        hids = DSInputs()
+
+        # Start the robot in disabled mode for a short period
+        control.step_timing(seconds=0.5, autonomous=False, enabled=False)
+
+        DriverStationSim.setTest(True)
+        DriverStationSim.setEnabled(True)
+
+        assert control.robot_is_alive
+
+        for _ in range(20):
+            hids.fuzz()
+            DriverStationSim.notifyNewData()
+            wpilib.simulation.stepTiming(0.2)
+            assert control.robot_is_alive