From 1a99a348cba1173a84c74e7eba62656da6469e90 Mon Sep 17 00:00:00 2001 From: PJ Reiniger Date: Fri, 14 Nov 2025 00:55:54 -0500 Subject: [PATCH] [robotpy] Mirror robotpy's commands-v2 (#8369) Project import generated by Copybara. GitOrigin-RevId: 715c8e8372d936f447f2937aab6b1a22dc619126 --- .styleguide | 2 + .../src/main/python/commands2/__init__.py | 88 +++ .../main/python/commands2/button/__init__.py | 19 + .../commands2/button/commandgenerichid.py | 224 ++++++ .../commands2/button/commandjoystick.py | 226 ++++++ .../commands2/button/commandps4controller.py | 280 ++++++++ .../commands2/button/commandxboxcontroller.py | 262 +++++++ .../python/commands2/button/joystickbutton.py | 19 + .../python/commands2/button/networkbutton.py | 126 ++++ .../main/python/commands2/button/povbutton.py | 20 + .../main/python/commands2/button/trigger.py | 304 +++++++++ commandsv2/src/main/python/commands2/cmd.py | 239 +++++++ .../src/main/python/commands2/command.py | 588 ++++++++++++++++ .../main/python/commands2/commandscheduler.py | 643 ++++++++++++++++++ .../python/commands2/conditionalcommand.py | 93 +++ .../main/python/commands2/deferredcommand.py | 71 ++ .../src/main/python/commands2/exceptions.py | 19 + .../python/commands2/functionalcommand.py | 59 ++ .../main/python/commands2/instantcommand.py | 33 + .../main/python/commands2/notifiercommand.py | 46 ++ .../python/commands2/parallelcommandgroup.py | 95 +++ .../python/commands2/paralleldeadlinegroup.py | 140 ++++ .../python/commands2/parallelracegroup.py | 90 +++ .../src/main/python/commands2/pidcommand.py | 78 +++ .../src/main/python/commands2/pidsubsystem.py | 100 +++ .../src/main/python/commands2/printcommand.py | 21 + .../python/commands2/profiledpidcommand.py | 81 +++ .../python/commands2/profiledpidsubsystem.py | 81 +++ .../src/main/python/commands2/proxycommand.py | 126 ++++ commandsv2/src/main/python/commands2/py.typed | 0 .../main/python/commands2/repeatcommand.py | 68 ++ .../src/main/python/commands2/runcommand.py | 28 + .../main/python/commands2/schedulecommand.py | 31 + .../main/python/commands2/selectcommand.py | 86 +++ .../commands2/sequentialcommandgroup.py | 101 +++ .../main/python/commands2/startendcommand.py | 36 + .../src/main/python/commands2/subsystem.py | 248 +++++++ .../commands2/swervecontrollercommand.py | 111 +++ .../main/python/commands2/sysid/__init__.py | 4 + .../python/commands2/sysid/sysidroutine.py | 177 +++++ .../python/commands2/timedcommandrobot.py | 16 + .../commands2/trapezoidprofilecommand.py | 64 ++ .../commands2/trapezoidprofilesubsystem.py | 94 +++ .../src/main/python/commands2/typing.py | 30 + commandsv2/src/main/python/commands2/util.py | 25 + .../src/main/python/commands2/waitcommand.py | 41 ++ .../main/python/commands2/waituntilcommand.py | 80 +++ .../main/python/commands2/wrappercommand.py | 90 +++ .../src/test/python/compositiontestbase.py | 166 +++++ commandsv2/src/test/python/conftest.py | 20 + .../test/python/test_command_decorators.py | 222 ++++++ .../test/python/test_command_requirements.py | 58 ++ .../src/test/python/test_command_schedule.py | 90 +++ .../test/python/test_commandgroup_error.py | 37 + .../test/python/test_conditional_command.py | 55 ++ .../src/test/python/test_default_command.py | 73 ++ .../src/test/python/test_deferred_command.py | 63 ++ .../test/python/test_functional_command.py | 36 + .../src/test/python/test_instant_command.py | 21 + .../src/test/python/test_networkbutton.py | 29 + .../src/test/python/test_notifier_command.py | 22 + .../test/python/test_parallelcommandgroup.py | 117 ++++ .../test/python/test_paralleldeadlinegroup.py | 119 ++++ .../src/test/python/test_parallelracegroup.py | 183 +++++ commandsv2/src/test/python/test_pidcommand.py | 114 ++++ .../src/test/python/test_printcommand.py | 17 + .../test/python/test_profiledpidsubsystem.py | 119 ++++ .../src/test/python/test_proxycommand.py | 38 ++ .../src/test/python/test_repeatcommand.py | 69 ++ .../test/python/test_robotdisabledcommand.py | 154 +++++ commandsv2/src/test/python/test_runcommand.py | 22 + .../src/test/python/test_schedulecommand.py | 36 + commandsv2/src/test/python/test_scheduler.py | 134 ++++ .../test/python/test_schedulingrecursion.py | 165 +++++ .../src/test/python/test_selectcommand.py | 94 +++ .../python/test_sequentialcommandgroup.py | 114 ++++ .../src/test/python/test_startendcommand.py | 30 + .../python/test_swervecontrollercommand.py | 142 ++++ .../src/test/python/test_sysidroutine.py | 168 +++++ .../python/test_trapezoidprofilecommand.py | 142 ++++ commandsv2/src/test/python/test_trigger.py | 236 +++++++ .../src/test/python/test_waitcommand.py | 50 ++ .../src/test/python/test_waituntilcommand.py | 22 + commandsv2/src/test/python/util.py | 294 ++++++++ 84 files changed, 8974 insertions(+) create mode 100644 commandsv2/src/main/python/commands2/__init__.py create mode 100644 commandsv2/src/main/python/commands2/button/__init__.py create mode 100644 commandsv2/src/main/python/commands2/button/commandgenerichid.py create mode 100644 commandsv2/src/main/python/commands2/button/commandjoystick.py create mode 100644 commandsv2/src/main/python/commands2/button/commandps4controller.py create mode 100644 commandsv2/src/main/python/commands2/button/commandxboxcontroller.py create mode 100644 commandsv2/src/main/python/commands2/button/joystickbutton.py create mode 100644 commandsv2/src/main/python/commands2/button/networkbutton.py create mode 100644 commandsv2/src/main/python/commands2/button/povbutton.py create mode 100644 commandsv2/src/main/python/commands2/button/trigger.py create mode 100644 commandsv2/src/main/python/commands2/cmd.py create mode 100644 commandsv2/src/main/python/commands2/command.py create mode 100644 commandsv2/src/main/python/commands2/commandscheduler.py create mode 100644 commandsv2/src/main/python/commands2/conditionalcommand.py create mode 100644 commandsv2/src/main/python/commands2/deferredcommand.py create mode 100644 commandsv2/src/main/python/commands2/exceptions.py create mode 100644 commandsv2/src/main/python/commands2/functionalcommand.py create mode 100644 commandsv2/src/main/python/commands2/instantcommand.py create mode 100644 commandsv2/src/main/python/commands2/notifiercommand.py create mode 100644 commandsv2/src/main/python/commands2/parallelcommandgroup.py create mode 100644 commandsv2/src/main/python/commands2/paralleldeadlinegroup.py create mode 100644 commandsv2/src/main/python/commands2/parallelracegroup.py create mode 100644 commandsv2/src/main/python/commands2/pidcommand.py create mode 100644 commandsv2/src/main/python/commands2/pidsubsystem.py create mode 100644 commandsv2/src/main/python/commands2/printcommand.py create mode 100644 commandsv2/src/main/python/commands2/profiledpidcommand.py create mode 100644 commandsv2/src/main/python/commands2/profiledpidsubsystem.py create mode 100644 commandsv2/src/main/python/commands2/proxycommand.py create mode 100644 commandsv2/src/main/python/commands2/py.typed create mode 100644 commandsv2/src/main/python/commands2/repeatcommand.py create mode 100644 commandsv2/src/main/python/commands2/runcommand.py create mode 100644 commandsv2/src/main/python/commands2/schedulecommand.py create mode 100644 commandsv2/src/main/python/commands2/selectcommand.py create mode 100644 commandsv2/src/main/python/commands2/sequentialcommandgroup.py create mode 100644 commandsv2/src/main/python/commands2/startendcommand.py create mode 100644 commandsv2/src/main/python/commands2/subsystem.py create mode 100644 commandsv2/src/main/python/commands2/swervecontrollercommand.py create mode 100644 commandsv2/src/main/python/commands2/sysid/__init__.py create mode 100644 commandsv2/src/main/python/commands2/sysid/sysidroutine.py create mode 100644 commandsv2/src/main/python/commands2/timedcommandrobot.py create mode 100644 commandsv2/src/main/python/commands2/trapezoidprofilecommand.py create mode 100644 commandsv2/src/main/python/commands2/trapezoidprofilesubsystem.py create mode 100644 commandsv2/src/main/python/commands2/typing.py create mode 100644 commandsv2/src/main/python/commands2/util.py create mode 100644 commandsv2/src/main/python/commands2/waitcommand.py create mode 100644 commandsv2/src/main/python/commands2/waituntilcommand.py create mode 100644 commandsv2/src/main/python/commands2/wrappercommand.py create mode 100644 commandsv2/src/test/python/compositiontestbase.py create mode 100644 commandsv2/src/test/python/conftest.py create mode 100644 commandsv2/src/test/python/test_command_decorators.py create mode 100644 commandsv2/src/test/python/test_command_requirements.py create mode 100644 commandsv2/src/test/python/test_command_schedule.py create mode 100644 commandsv2/src/test/python/test_commandgroup_error.py create mode 100644 commandsv2/src/test/python/test_conditional_command.py create mode 100644 commandsv2/src/test/python/test_default_command.py create mode 100644 commandsv2/src/test/python/test_deferred_command.py create mode 100644 commandsv2/src/test/python/test_functional_command.py create mode 100644 commandsv2/src/test/python/test_instant_command.py create mode 100644 commandsv2/src/test/python/test_networkbutton.py create mode 100644 commandsv2/src/test/python/test_notifier_command.py create mode 100644 commandsv2/src/test/python/test_parallelcommandgroup.py create mode 100644 commandsv2/src/test/python/test_paralleldeadlinegroup.py create mode 100644 commandsv2/src/test/python/test_parallelracegroup.py create mode 100644 commandsv2/src/test/python/test_pidcommand.py create mode 100644 commandsv2/src/test/python/test_printcommand.py create mode 100644 commandsv2/src/test/python/test_profiledpidsubsystem.py create mode 100644 commandsv2/src/test/python/test_proxycommand.py create mode 100644 commandsv2/src/test/python/test_repeatcommand.py create mode 100644 commandsv2/src/test/python/test_robotdisabledcommand.py create mode 100644 commandsv2/src/test/python/test_runcommand.py create mode 100644 commandsv2/src/test/python/test_schedulecommand.py create mode 100644 commandsv2/src/test/python/test_scheduler.py create mode 100644 commandsv2/src/test/python/test_schedulingrecursion.py create mode 100644 commandsv2/src/test/python/test_selectcommand.py create mode 100644 commandsv2/src/test/python/test_sequentialcommandgroup.py create mode 100644 commandsv2/src/test/python/test_startendcommand.py create mode 100644 commandsv2/src/test/python/test_swervecontrollercommand.py create mode 100644 commandsv2/src/test/python/test_sysidroutine.py create mode 100644 commandsv2/src/test/python/test_trapezoidprofilecommand.py create mode 100644 commandsv2/src/test/python/test_trigger.py create mode 100644 commandsv2/src/test/python/test_waitcommand.py create mode 100644 commandsv2/src/test/python/test_waituntilcommand.py create mode 100644 commandsv2/src/test/python/util.py diff --git a/.styleguide b/.styleguide index f5da2f7ac8..b84f74df9d 100644 --- a/.styleguide +++ b/.styleguide @@ -68,6 +68,8 @@ generatedFileExclude { apriltag/src/main/python/ apriltag/src/test/python/ + commandsv2/src/main/python/ + commandsv2/src/test/python/ datalog/src/main/python/ datalog/src/test/python/ hal/src/main/python/ diff --git a/commandsv2/src/main/python/commands2/__init__.py b/commandsv2/src/main/python/commands2/__init__.py new file mode 100644 index 0000000000..1823bf17c1 --- /dev/null +++ b/commandsv2/src/main/python/commands2/__init__.py @@ -0,0 +1,88 @@ +from .command import Command, InterruptionBehavior + +from . import button +from . import cmd +from . import typing + +from .commandscheduler import CommandScheduler +from .conditionalcommand import ConditionalCommand +from .deferredcommand import DeferredCommand +from .exceptions import IllegalCommandUse +from .functionalcommand import FunctionalCommand +from .instantcommand import InstantCommand +from .notifiercommand import NotifierCommand +from .parallelcommandgroup import ParallelCommandGroup +from .paralleldeadlinegroup import ParallelDeadlineGroup +from .parallelracegroup import ParallelRaceGroup +from .pidcommand import PIDCommand +from .pidsubsystem import PIDSubsystem +from .printcommand import PrintCommand +from .profiledpidcommand import ProfiledPIDCommand +from .profiledpidsubsystem import ProfiledPIDSubsystem +from .proxycommand import ProxyCommand +from .repeatcommand import RepeatCommand +from .runcommand import RunCommand +from .schedulecommand import ScheduleCommand +from .selectcommand import SelectCommand +from .sequentialcommandgroup import SequentialCommandGroup +from .startendcommand import StartEndCommand +from .subsystem import Subsystem +from .swervecontrollercommand import SwerveControllerCommand +from .timedcommandrobot import TimedCommandRobot +from .trapezoidprofilecommand import TrapezoidProfileCommand +from .trapezoidprofilesubsystem import TrapezoidProfileSubsystem +from .waitcommand import WaitCommand +from .waituntilcommand import WaitUntilCommand +from .wrappercommand import WrapperCommand + +from typing import TYPE_CHECKING + +__all__ = [ + "button", + "cmd", + "Command", + "CommandScheduler", + "ConditionalCommand", + "DeferredCommand", + "FunctionalCommand", + "IllegalCommandUse", + "InstantCommand", + "InterruptionBehavior", + "NotifierCommand", + "ParallelCommandGroup", + "ParallelDeadlineGroup", + "ParallelRaceGroup", + "PIDCommand", + "PIDSubsystem", + "PrintCommand", + "ProfiledPIDCommand", + "ProfiledPIDSubsystem", + "ProxyCommand", + "RepeatCommand", + "RunCommand", + "ScheduleCommand", + "SelectCommand", + "SequentialCommandGroup", + "StartEndCommand", + "Subsystem", + "SwerveControllerCommand", + "TimedCommandRobot", + "TrapezoidProfileCommand", + "TrapezoidProfileSubsystem", + "WaitCommand", + "WaitUntilCommand", + "WrapperCommand", +] + +if not TYPE_CHECKING: + + def __getattr__(attr): + if attr == "SubsystemBase": + import warnings + + warnings.warn( + "SubsystemBase is deprecated", DeprecationWarning, stacklevel=2 + ) + return Subsystem + + raise AttributeError(f"module {__name__!r} has no attribute {attr!r}") diff --git a/commandsv2/src/main/python/commands2/button/__init__.py b/commandsv2/src/main/python/commands2/button/__init__.py new file mode 100644 index 0000000000..f6947397cb --- /dev/null +++ b/commandsv2/src/main/python/commands2/button/__init__.py @@ -0,0 +1,19 @@ +from .commandgenerichid import CommandGenericHID +from .commandjoystick import CommandJoystick +from .commandps4controller import CommandPS4Controller +from .commandxboxcontroller import CommandXboxController +from .joystickbutton import JoystickButton +from .networkbutton import NetworkButton +from .povbutton import POVButton +from .trigger import Trigger + +__all__ = [ + "Trigger", + "CommandGenericHID", + "CommandJoystick", + "CommandPS4Controller", + "CommandXboxController", + "JoystickButton", + "NetworkButton", + "POVButton", +] diff --git a/commandsv2/src/main/python/commands2/button/commandgenerichid.py b/commandsv2/src/main/python/commands2/button/commandgenerichid.py new file mode 100644 index 0000000000..96eaf93e8e --- /dev/null +++ b/commandsv2/src/main/python/commands2/button/commandgenerichid.py @@ -0,0 +1,224 @@ +# validated: 2024-01-20 DS 92149efa11fa button/CommandGenericHID.java +from typing import Optional + +from wpilib.event import EventLoop +from wpilib.interfaces import GenericHID + +from ..commandscheduler import CommandScheduler +from .trigger import Trigger + + +class CommandGenericHID: + """ + A version of :class:`wpilib.interfaces.GenericHID` with :class:`.Trigger` factories for command-based. + """ + + def __init__(self, port: int): + """ + Construct an instance of a device. + + :param port: The port on the Driver Station that the device is plugged into. + """ + self._hid = GenericHID(port) + + def getHID(self) -> GenericHID: + """ + Get the underlying GenericHID object. + """ + return self._hid + + def button(self, button: int, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around this button's digital signal. + + :param button: The button index + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: A trigger instance attached to the event loop + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getRawButton(button)) + + def pov( + self, angle: int, *, pov: int = 0, loop: Optional[EventLoop] = None + ) -> Trigger: + """ + Constructs a Trigger instance based around this angle of a POV on the HID. + + The POV angles start at 0 in the up direction, and increase clockwise (e.g. right is 90, + upper-left is 315). + + :param angle: POV angle in degrees, or -1 for the center / not pressed. + :param pov: index of the POV to read (starting at 0). Defaults to 0. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: a Trigger instance based around this angle of a POV on the HID. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getPOV(pov) == angle) + + def povUp(self) -> Trigger: + """ + Constructs a Trigger instance based around the 0 degree angle (up) of the default (index 0) POV + on the HID, attached to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: a Trigger instance based around the 0 degree angle of a POV on the HID. + """ + return self.pov(0) + + def povUpRight(self) -> Trigger: + """ + Constructs a Trigger instance based around the 45 degree angle (right up) of the default (index + 0) POV on the HID, attached to :func:`commands2.CommandScheduler.getDefaultButtonLoop`. + + :returns: a Trigger instance based around the 45 degree angle of a POV on the HID. + """ + return self.pov(45) + + def povRight(self) -> Trigger: + """ + Constructs a Trigger instance based around the 90 degree angle (right) of the default (index 0) + POV on the HID, attached to :func:`commands2.CommandScheduler.getDefaultButtonLoop`. + + :returns: a Trigger instance based around the 90 degree angle of a POV on the HID. + """ + return self.pov(90) + + def povDownRight(self) -> Trigger: + """ + Constructs a Trigger instance based around the 135 degree angle (right down) of the default + (index 0) POV on the HID, attached to :func:`commands2.CommandScheduler.getDefaultButtonLoop`. + + :returns: a Trigger instance based around the 135 degree angle of a POV on the HID. + """ + return self.pov(135) + + def povDown(self) -> Trigger: + """ + Constructs a Trigger instance based around the 180 degree angle (down) of the default (index 0) + POV on the HID, attached to :func:`commands2.CommandScheduler.getDefaultButtonLoop`. + + :returns: a Trigger instance based around the 180 degree angle of a POV on the HID. + """ + return self.pov(180) + + def povDownLeft(self) -> Trigger: + """ + Constructs a Trigger instance based around the 225 degree angle (down left) of the default + (index 0) POV on the HID, attached to :func:`commands2.CommandScheduler.getDefaultButtonLoop`. + + :returns: a Trigger instance based around the 225 degree angle of a POV on the HID. + """ + return self.pov(225) + + def povLeft(self) -> Trigger: + """ + Constructs a Trigger instance based around the 270 degree angle (left) of the default (index 0) + POV on the HID, attached to :func:`commands2.CommandScheduler.getDefaultButtonLoop`. + + :returns: a Trigger instance based around the 270 degree angle of a POV on the HID. + """ + return self.pov(270) + + def povUpLeft(self) -> Trigger: + """ + Constructs a Trigger instance based around the 315 degree angle (left up) of the default (index + 0) POV on the HID, attached to :func:`commands2.CommandScheduler.getDefaultButtonLoop`. + + :returns: a Trigger instance based around the 315 degree angle of a POV on the HID. + """ + return self.pov(315) + + def povCenter(self) -> Trigger: + """ + Constructs a Trigger instance based around the center (not pressed) position of the default + (index 0) POV on the HID, attached to :func:`commands2.CommandScheduler.getDefaultButtonLoop`. + + :returns: a Trigger instance based around the center position of a POV on the HID. + """ + return self.pov(-1) + + def axisLessThan( + self, axis: int, threshold: float, loop: Optional[EventLoop] = None + ) -> Trigger: + """ + Constructs a Trigger instance that is true when the axis value is less than ``threshold``, + attached to the given loop. + + :param axis: The axis to read, starting at 0 + :param threshold: The value below which this trigger should return true. + :param loop: the event loop instance to attach the trigger to + + :returns: a Trigger instance that is true when the axis value is less than the provided + threshold. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getRawAxis(axis) < threshold) + + def axisGreaterThan( + self, axis: int, threshold: float, loop: Optional[EventLoop] = None + ) -> Trigger: + """ + Constructs a Trigger instance that is true when the axis value is greater than + ``threshold``, attached to the given loop. + + :param axis: The axis to read, starting at 0 + :param threshold: The value above which this trigger should return true. + :param loop: the event loop instance to attach the trigger to. + + :returns: a Trigger instance that is true when the axis value is greater than the provided + threshold. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getRawAxis(axis) > threshold) + + def axisMagnitudeGreaterThan( + self, axis: int, threshold: float, loop: Optional[EventLoop] = None + ) -> Trigger: + """ + Constructs a Trigger instance that is true when the axis magnitude is greater than + ``threshold``, attached to the given loop. + + :param axis: The axis to read, starting at 0 + :param threshold: The value above which this trigger should return true. + :param loop: the event loop instance to attach the trigger to. + + :returns: a Trigger instance that is true when the axis magnitude is greater than the provided + threshold. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: abs(self._hid.getRawAxis(axis)) > threshold) + + def getRawAxis(self, axis: int) -> float: + """ + Get the value of the axis. + + :param axis: The axis to read, starting at 0. + :returns: The value of the axis. + """ + return self._hid.getRawAxis(axis) + + def setRumble(self, type: GenericHID.RumbleType, value: float): + """ + Set the rumble output for the HID. + The DS currently supports 2 rumble values, left rumble and right rumble. + + :param type: Which rumble value to set. + :param value: The normalized value (0 to 1) to set the rumble to. + """ + self._hid.setRumble(type, value) + + def isConnected(self): + """ + Get if the HID is connected. + + :returns: True if the HID is connected. + """ + return self._hid.isConnected() diff --git a/commandsv2/src/main/python/commands2/button/commandjoystick.py b/commandsv2/src/main/python/commands2/button/commandjoystick.py new file mode 100644 index 0000000000..0b43d8d2ec --- /dev/null +++ b/commandsv2/src/main/python/commands2/button/commandjoystick.py @@ -0,0 +1,226 @@ +# validated: 2024-01-20 DS 92aecab2ef05 button/CommandJoystick.java +from typing import Optional + +from wpilib import Joystick +from wpilib.event import EventLoop + +from ..commandscheduler import CommandScheduler +from .commandgenerichid import CommandGenericHID +from .trigger import Trigger + + +class CommandJoystick(CommandGenericHID): + """ + A version of :class:`wpilib.Joystick` with :class:`.Trigger` factories for command-based. + """ + + _hid: Joystick + + def __init__(self, port: int): + """ + Construct an instance of a controller. + + :param port: The port index on the Driver Station that the controller is plugged into. + """ + + super().__init__(port) + self._hid = Joystick(port) + + def getHID(self) -> Joystick: + """ + Get the underlying GenericHID object. + + :returns: the wrapped GenericHID object + """ + return self._hid + + def trigger(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the trigger button's digital signal. + + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: an event instance representing the trigger button's digital signal attached to the + given loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getTrigger()) + + def top(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the top button's digital signal. + + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: an event instance representing the top button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getTop()) + + def setXChannel(self, channel: int): + """ + Set the channel associated with the X axis. + + :param channel: The channel to set the axis to. + """ + self._hid.setXChannel(channel) + + def setYChannel(self, channel: int): + """ + Set the channel associated with the Y axis. + + :param channel: The channel to set the axis to. + """ + self._hid.setYChannel(channel) + + def setZChannel(self, channel: int): + """ + Set the channel associated with the Z axis. + + :param channel: The channel to set the axis to. + """ + self._hid.setZChannel(channel) + + def setThrottleChannel(self, channel: int): + """ + Set the channel associated with the throttle axis. + + :param channel: The channel to set the axis to. + """ + self._hid.setThrottleChannel(channel) + + def setTwistChannel(self, channel: int): + """ + Set the channel associated with the twist axis. + + :param channel: The channel to set the axis to. + """ + self._hid.setTwistChannel(channel) + + def getXChannel(self) -> int: + """ + Get the channel currently associated with the X axis. + + :returns: The channel for the axis. + """ + return self._hid.getXChannel() + + def getYChannel(self) -> int: + """ + Get the channel currently associated with the Y axis. + + :returns: The channel for the axis. + """ + return self._hid.getYChannel() + + def getZChannel(self) -> int: + """ + Get the channel currently associated with the Z axis. + + :returns: The channel for the axis. + """ + return self._hid.getZChannel() + + def getTwistChannel(self) -> int: + """ + Get the channel currently associated with the twist axis. + + :returns: The channel for the axis. + """ + return self._hid.getTwistChannel() + + def getThrottleChannel(self) -> int: + """ + Get the channel currently associated with the throttle axis. + + :returns: The channel for the axis. + """ + return self._hid.getThrottleChannel() + + def getX(self) -> float: + """ + Get the x position of the HID. + + This depends on the mapping of the joystick connected to the current port. On most + joysticks, positive is to the right. + + :returns: the x position + """ + return self._hid.getX() + + def getY(self) -> float: + """ + Get the y position of the HID. + + This depends on the mapping of the joystick connected to the current port. On most + joysticks, positive is to the back. + + :returns: the y position + """ + return self._hid.getY() + + def getZ(self) -> float: + """ + Get the z position of the HID. + + :returns: the z position + """ + return self._hid.getZ() + + def getTwist(self) -> float: + """ + Get the twist value of the current joystick. This depends on the mapping of the joystick + connected to the current port. + + :returns: The Twist value of the joystick. + """ + return self._hid.getTwist() + + def getThrottle(self) -> float: + """ + Get the throttle value of the current joystick. This depends on the mapping of the joystick + connected to the current port. + + :returns: The Throttle value of the joystick. + """ + return self._hid.getThrottle() + + def getMagnitude(self) -> float: + """ + Get the magnitude of the vector formed by the joystick's current position relative to its + origin. + + :returns: The magnitude of the direction vector + """ + return self._hid.getMagnitude() + + def getDirectionRadians(self) -> float: + """ + Get the direction of the vector formed by the joystick and its origin in radians. 0 is forward + and clockwise is positive. (Straight right is π/2.) + + :returns: The direction of the vector in radians + """ + # https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html#joystick-and-controller-coordinate-system + # A positive rotation around the X axis moves the joystick right, and a + # positive rotation around the Y axis moves the joystick backward. When + # treating them as translations, 0 radians is measured from the right + # direction, and angle increases clockwise. + # + # It's rotated 90 degrees CCW (y is negated and the arguments are reversed) + # so that 0 radians is forward. + return self._hid.getDirectionRadians() + + def getDirectionDegrees(self) -> float: + """ + Get the direction of the vector formed by the joystick and its origin in degrees. 0 is forward + and clockwise is positive. (Straight right is 90.) + + :returns: The direction of the vector in degrees + """ + return self._hid.getDirectionDegrees() diff --git a/commandsv2/src/main/python/commands2/button/commandps4controller.py b/commandsv2/src/main/python/commands2/button/commandps4controller.py new file mode 100644 index 0000000000..8703ef82dd --- /dev/null +++ b/commandsv2/src/main/python/commands2/button/commandps4controller.py @@ -0,0 +1,280 @@ +# validated: 2024-01-20 DS d426873ed15b button/CommandPS4Controller.java +from typing import Optional + +from wpilib import PS4Controller +from wpilib.event import EventLoop + +from ..commandscheduler import CommandScheduler +from .commandgenerichid import CommandGenericHID +from .trigger import Trigger + + +class CommandPS4Controller(CommandGenericHID): + """ + A version of PS4Controller with Trigger factories for command-based. + """ + + _hid: PS4Controller + + def __init__(self, port: int): + """ + Construct an instance of a device. + + :param port: The port index on the Driver Station that the device is plugged into. + """ + super().__init__(port) + self._hid = PS4Controller(port) + + def getHID(self) -> PS4Controller: + """ + Get the underlying GenericHID object. + + :returns: the wrapped GenericHID object + """ + return self._hid + + def L2(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the L2 button's digital signal. + + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: an event instance representing the L2 button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getL2Button()) + + def R2(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the R2 button's digital signal. + + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: an event instance representing the R2 button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getR2Button()) + + def L1(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the L1 button's digital signal. + + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: an event instance representing the L1 button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getL1Button()) + + def R1(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the R1 button's digital signal. + + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: an event instance representing the R1 button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getR1Button()) + + def L3(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the L3 button's digital signal. + + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: an event instance representing the L3 button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getL3Button()) + + def R3(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the R3 button's digital signal. + + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: an event instance representing the R3 button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getR3Button()) + + def square(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the square button's digital signal. + + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: an event instance representing the square button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getSquareButton()) + + def cross(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the cross button's digital signal. + + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: an event instance representing the cross button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getCrossButton()) + + def triangle(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the triangle button's digital signal. + + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: an event instance representing the triangle button's digital signal attached to the + given loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getTriangleButton()) + + def circle(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the circle button's digital signal. + + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: an event instance representing the circle button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getCircleButton()) + + def share(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the share button's digital signal. + + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: an event instance representing the share button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getShareButton()) + + def PS(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the PS button's digital signal. + + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: an event instance representing the PS button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getPSButton()) + + def options(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the options button's digital signal. + + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: an event instance representing the options button's digital signal attached to the + given loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getOptionsButton()) + + def touchpad(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the touchpad's digital signal. + + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: an event instance representing the touchpad's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getTouchpad()) + + def getLeftX(self) -> float: + """ + Get the X axis value of left side of the controller. Right is positive. + + :returns: the axis value. + """ + return self._hid.getLeftX() + + def getRightX(self) -> float: + """ + Get the X axis value of right side of the controller. Right is positive. + + :returns: the axis value. + """ + return self._hid.getRightX() + + def getLeftY(self) -> float: + """ + Get the Y axis value of left side of the controller. Back is positive. + + :returns: the axis value. + """ + return self._hid.getLeftY() + + def getRightY(self) -> float: + """ + Get the Y axis value of right side of the controller. Back is positive. + + :returns: the axis value. + """ + return self._hid.getRightY() + + def getL2Axis(self) -> float: + """ + Get the L2 axis value of the controller. Note that this axis is bound to the range of [0, 1] as + opposed to the usual [-1, 1]. + + :returns: the axis value. + """ + return self._hid.getL2Axis() + + def getR2Axis(self) -> float: + """ + Get the R2 axis value of the controller. Note that this axis is bound to the range of [0, 1] as + opposed to the usual [-1, 1]. + + :returns: the axis value. + """ + return self._hid.getR2Axis() diff --git a/commandsv2/src/main/python/commands2/button/commandxboxcontroller.py b/commandsv2/src/main/python/commands2/button/commandxboxcontroller.py new file mode 100644 index 0000000000..926883c6ce --- /dev/null +++ b/commandsv2/src/main/python/commands2/button/commandxboxcontroller.py @@ -0,0 +1,262 @@ +# validated: 2024-01-20 DS 3ba501f9478a button/CommandXboxController.java +from typing import Optional + +from wpilib import XboxController +from wpilib.event import EventLoop + +from ..commandscheduler import CommandScheduler +from .commandgenerichid import CommandGenericHID +from .trigger import Trigger + + +class CommandXboxController(CommandGenericHID): + """ + A version of XboxController with Trigger factories for command-based. + """ + + _hid: XboxController + + def __init__(self, port: int): + """ + Construct an instance of a controller. + + :param port: The port index on the Driver Station that the controller is plugged into. + """ + super().__init__(port) + self._hid = XboxController(port) + + def getHID(self) -> XboxController: + """ + Get the underlying GenericHID object. + + :returns: the wrapped GenericHID object + """ + return self._hid + + def leftBumper(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the left bumper's digital signal. + + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: an event instance representing the right bumper's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getLeftBumper()) + + def rightBumper(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the right bumper's digital signal. + + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: an event instance representing the left bumper's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getRightBumper()) + + def leftStick(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the left stick button's digital signal. + + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: an event instance representing the left stick button's digital signal attached to the + given loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getLeftStickButton()) + + def rightStick(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the right stick button's digital signal. + + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: an event instance representing the right stick button's digital signal attached to the + given loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getRightStickButton()) + + def a(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the A button's digital signal. + + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: an event instance representing the A button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getAButton()) + + def b(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the B button's digital signal. + + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: an event instance representing the B button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getBButton()) + + def x(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the X button's digital signal. + + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: an event instance representing the X button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getXButton()) + + def y(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the Y button's digital signal. + + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: an event instance representing the Y button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getYButton()) + + def start(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the start button's digital signal. + + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: an event instance representing the start button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getStartButton()) + + def back(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the back button's digital signal. + + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: an event instance representing the back button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getBackButton()) + + def leftTrigger( + self, threshold: float = 0.5, loop: Optional[EventLoop] = None + ) -> Trigger: + """ + Constructs a Trigger instance around the axis value of the left trigger. The returned trigger + will be true when the axis value is greater than {@code threshold}. + + :param threshold: the minimum axis value for the returned Trigger to be true. This value + should be in the range [0, 1] where 0 is the unpressed state of the axis. + :param loop: the event loop instance to attach the Trigger to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: a Trigger instance that is true when the left trigger's axis exceeds the provided + threshold, attached to the given event loop + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getLeftTriggerAxis() > threshold) + + def rightTrigger( + self, threshold: float = 0.5, loop: Optional[EventLoop] = None + ) -> Trigger: + """ + Constructs a Trigger instance around the axis value of the right trigger. The returned trigger + will be true when the axis value is greater than {@code threshold}. + + :param threshold: the minimum axis value for the returned Trigger to be true. This value + should be in the range [0, 1] where 0 is the unpressed state of the axis. + :param loop: the event loop instance to attach the Trigger to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: a Trigger instance that is true when the right trigger's axis exceeds the provided + threshold, attached to the given event loop + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getRightTriggerAxis() > threshold) + + def getLeftX(self) -> float: + """ + Get the X axis value of left side of the controller. Right is positive. + + :returns: The axis value. + """ + return self._hid.getLeftX() + + def getRightX(self) -> float: + """ + Get the X axis value of right side of the controller. Right is positive. + + :returns: The axis value. + """ + return self._hid.getRightX() + + def getLeftY(self) -> float: + """ + Get the Y axis value of left side of the controller. Back is positive. + + :returns: The axis value. + """ + return self._hid.getLeftY() + + def getRightY(self) -> float: + """ + Get the Y axis value of right side of the controller. Back is positive. + + :returns: The axis value. + """ + return self._hid.getRightY() + + def getLeftTriggerAxis(self) -> float: + """ + Get the left trigger (LT) axis value of the controller. Note that this axis is bound to the + range of [0, 1] as opposed to the usual [-1, 1]. + + :returns: The axis value. + """ + return self._hid.getLeftTriggerAxis() + + def getRightTriggerAxis(self) -> float: + """ + Get the right trigger (RT) axis value of the controller. Note that this axis is bound to the + range of [0, 1] as opposed to the usual [-1, 1]. + + :returns: The axis value. + """ + return self._hid.getRightTriggerAxis() diff --git a/commandsv2/src/main/python/commands2/button/joystickbutton.py b/commandsv2/src/main/python/commands2/button/joystickbutton.py new file mode 100644 index 0000000000..0fbb2b4a54 --- /dev/null +++ b/commandsv2/src/main/python/commands2/button/joystickbutton.py @@ -0,0 +1,19 @@ +# validated: 2024-01-20 DS 7a099cb02a33 button/JoystickButton.java +from wpilib.interfaces import GenericHID + +from .trigger import Trigger + + +class JoystickButton(Trigger): + """ + A Button that gets its state from a :class:`wpilib.interfaces.GenericHID`. + """ + + def __init__(self, joystick: GenericHID, buttonNumber: int): + """ + Creates a joystick button for triggering commands. + + :param joystick: The GenericHID object that has the button (e.g. Joystick, KinectStick, etc) + :param buttonNumber: The button number (see :func:`wpilib.interfaces.GenericHID.getRawButton` + """ + super().__init__(lambda: joystick.getRawButton(buttonNumber)) diff --git a/commandsv2/src/main/python/commands2/button/networkbutton.py b/commandsv2/src/main/python/commands2/button/networkbutton.py new file mode 100644 index 0000000000..34fe28aaac --- /dev/null +++ b/commandsv2/src/main/python/commands2/button/networkbutton.py @@ -0,0 +1,126 @@ +# validated: 2024-01-20 DS 7a099cb02a33 button/NetworkButton.java +from typing import overload + +from ntcore import BooleanSubscriber, BooleanTopic, NetworkTable, NetworkTableInstance + +from ..util import format_args_kwargs +from .trigger import Trigger + + +class NetworkButton(Trigger): + """ + A Button that uses a :class:`ntcore.NetworkTable` boolean field. + """ + + @overload + def __init__(self, topic: BooleanTopic) -> None: + """ + Creates a NetworkButton that commands can be bound to. + + :param topic: The boolean topic that contains the value. + """ + pass + + @overload + def __init__(self, sub: BooleanSubscriber) -> None: + """ + Creates a NetworkButton that commands can be bound to. + + :param sub: The boolean subscriber that provides the value. + """ + pass + + @overload + def __init__(self, table: NetworkTable, field: str) -> None: + """ + Creates a NetworkButton that commands can be bound to. + + :param table: The table where the networktable value is located. + :param field: The field that is the value. + """ + pass + + @overload + def __init__(self, table: str, field: str) -> None: + """ + Creates a NetworkButton that commands can be bound to. + + :param table: The table where the networktable value is located. + :param field: The field that is the value. + """ + pass + + @overload + def __init__(self, inst: NetworkTableInstance, table: str, field: str) -> None: + """ + Creates a NetworkButton that commands can be bound to. + + :param inst: The NetworkTable instance to use + :param table: The table where the networktable value is located. + :param field: the field that is the value. + """ + pass + + def __init__(self, *args, **kwargs) -> None: + def init_sub(sub: BooleanSubscriber): + return super(NetworkButton, self).__init__( + lambda: sub.getTopic().getInstance().isConnected() and sub.get() + ) + + def init_topic(topic: BooleanTopic): + init_sub(topic.subscribe(False)) + + def init_table_field(table: NetworkTable, field: str): + init_topic(table.getBooleanTopic(field)) + + def init_inst_table_field(inst: NetworkTableInstance, table: str, field: str): + init_table_field(inst.getTable(table), field) + + def init_str_table_field(table: str, field: str): + init_inst_table_field(NetworkTableInstance.getDefault(), table, field) + + num_args = len(args) + len(kwargs) + + if num_args == 1: + if "topic" in kwargs: + return init_topic(kwargs["topic"]) + if "sub" in kwargs: + return init_sub(kwargs["sub"]) + if isinstance(args[0], BooleanTopic): + return init_topic(args[0]) + if isinstance(args[0], BooleanSubscriber): + return init_sub(args[0]) + elif num_args == 2: + table, field, *_ = args + (None, None) + if "table" in kwargs: + table = kwargs["table"] + if "field" in kwargs: + field = kwargs["field"] + if table is not None and field is not None: + if isinstance(table, NetworkTable): + return init_table_field(table, field) + if isinstance(table, str): + return init_str_table_field(table, field) + elif num_args == 3: + inst, table, field, *_ = args + (None, None, None) + if "inst" in kwargs: + inst = kwargs["inst"] + if "table" in kwargs: + table = kwargs["table"] + if "field" in kwargs: + field = kwargs["field"] + if inst is not None and table is not None and field is not None: + return init_inst_table_field(inst, table, field) + + raise TypeError( + f""" +TypeError: NetworkButton(): incompatible function arguments. The following argument types are supported: + 1. (self: NetworkButton, topic: BooleanTopic) + 2. (self: NetworkButton, sub: BooleanSubscriber) + 3. (self: NetworkButton, table: NetworkTable, field: str) + 4. (self: NetworkButton, table: str, field: str) + 5. (self: NetworkButton, inst: NetworkTableInstance, table: str, field: str) + +Invoked with: {format_args_kwargs(self, *args, **kwargs)} +""" + ) diff --git a/commandsv2/src/main/python/commands2/button/povbutton.py b/commandsv2/src/main/python/commands2/button/povbutton.py new file mode 100644 index 0000000000..d7114dfdf7 --- /dev/null +++ b/commandsv2/src/main/python/commands2/button/povbutton.py @@ -0,0 +1,20 @@ +# validated: 2024-01-20 DS 7a099cb02a33 button/POVButton.java +from wpilib.interfaces import GenericHID + +from .trigger import Trigger + + +class POVButton(Trigger): + """ + A Button that gets its state from a POV on a :class:`wpilib.interfaces.GenericHID`. + """ + + def __init__(self, joystick: GenericHID, angle: int, povNumber: int = 0): + """ + Creates a POV button for triggering commands. + + :param joystick: The GenericHID object that has the POV + :param angle: The desired angle in degrees (e.g. 90, 270) + :param povNumber: The POV number (see :func:`wpilib.interfaces.GenericHID.getPOV`) + """ + super().__init__(lambda: joystick.getPOV(povNumber) == angle) diff --git a/commandsv2/src/main/python/commands2/button/trigger.py b/commandsv2/src/main/python/commands2/button/trigger.py new file mode 100644 index 0000000000..16127068cd --- /dev/null +++ b/commandsv2/src/main/python/commands2/button/trigger.py @@ -0,0 +1,304 @@ +# validated: 2024-04-02 DS 0b1345946950 button/Trigger.java +from types import SimpleNamespace +from typing import Callable, overload + +from typing_extensions import Self +from wpilib.event import EventLoop +from wpimath.filter import Debouncer + +from ..command import Command +from ..commandscheduler import CommandScheduler +from ..util import format_args_kwargs + + +class Trigger: + """ + This class provides an easy way to link commands to conditions. + + It is very easy to link a button to a command. For instance, you could link the trigger button + of a joystick to a "score" command. + """ + + _loop: EventLoop + _condition: Callable[[], bool] + + @overload + def __init__(self, condition: Callable[[], bool] = lambda: False): + """ + Creates a new trigger based on the given condition. + + Polled by the default scheduler button loop. + + :param condition: the condition represented by this trigger + """ + ... + + @overload + def __init__(self, loop: EventLoop, condition: Callable[[], bool]): + """ + Creates a new trigger based on the given condition. + + :param loop: The loop instance that polls this trigger. + :param condition: the condition represented by this trigger + """ + ... + + def __init__(self, *args, **kwargs): + def init_loop_condition(loop: EventLoop, condition: Callable[[], bool]): + assert callable(condition) + self._loop = loop + self._condition = condition + + def init_condition(condition: Callable[[], bool]): + init_loop_condition( + CommandScheduler.getInstance().getDefaultButtonLoop(), condition + ) + + num_args = len(args) + len(kwargs) + + if num_args == 0: + return init_condition(lambda: False) + elif num_args == 1 and len(kwargs) == 1: + if "condition" in kwargs: + return init_condition(kwargs["condition"]) + elif num_args == 1 and len(args) == 1: + if callable(args[0]): + return init_condition(args[0]) + elif num_args == 2: + loop, condition, *_ = args + (None, None) + if "loop" in kwargs: + loop = kwargs["loop"] + if "condition" in kwargs: + condition = kwargs["condition"] + if loop is not None and condition is not None: + return init_loop_condition(loop, condition) + + raise TypeError( + f""" +TypeError: Trigger(): incompatible function arguments. The following argument types are supported: + 1. (self: Trigger) + 2. (self: Trigger, condition: () -> bool) + 3. (self: Trigger, loop: EventLoop, condition: () -> bool) + +Invoked with: {format_args_kwargs(self, *args, **kwargs)} +""" + ) + + def onTrue(self, command: Command) -> Self: + """ + Starts the given command whenever the condition changes from `False` to `True`. + + :param command: the command to start + :returns: this trigger, so calls can be chained + """ + + state = SimpleNamespace(pressed_last=self._condition()) + + @self._loop.bind + def _(): + pressed = self._condition() + if not state.pressed_last and pressed: + command.schedule() + state.pressed_last = pressed + + return self + + def onFalse(self, command: Command) -> Self: + """ + Starts the given command whenever the condition changes from `True` to `False`. + + :param command: the command to start + :returns: this trigger, so calls can be chained + """ + + state = SimpleNamespace(pressed_last=self._condition()) + + @self._loop.bind + def _(): + pressed = self._condition() + if state.pressed_last and not pressed: + command.schedule() + state.pressed_last = pressed + + return self + + def onChange(self, command: Command) -> Self: + """ + Starts the command when the condition changes. + + :param command: the command t start + :returns: this trigger, so calls can be chained + """ + + state = SimpleNamespace(pressed_last=self._condition()) + + @self._loop.bind + def _(): + pressed = self._condition() + + if state.pressed_last != pressed: + command.schedule() + + state.pressed_last = pressed + + return self + + def whileTrue(self, command: Command) -> Self: + """ + Starts the given command when the condition changes to `True` and cancels it when the condition + changes to `False`. + + Doesn't re-start the command if it ends while the condition is still `True`. If the command + should restart, see :class:`commands2.RepeatCommand`. + + :param command: the command to start + :returns: this trigger, so calls can be chained + """ + + state = SimpleNamespace(pressed_last=self._condition()) + + @self._loop.bind + def _(): + pressed = self._condition() + if not state.pressed_last and pressed: + command.schedule() + elif state.pressed_last and not pressed: + command.cancel() + state.pressed_last = pressed + + return self + + def whileFalse(self, command: Command) -> Self: + """ + Starts the given command when the condition changes to `False` and cancels it when the + condition changes to `True`. + + Doesn't re-start the command if it ends while the condition is still `False`. If the command + should restart, see :class:`commands2.RepeatCommand`. + + :param command: the command to start + :returns: this trigger, so calls can be chained + """ + + state = SimpleNamespace(pressed_last=self._condition()) + + @self._loop.bind + def _(): + pressed = self._condition() + if state.pressed_last and not pressed: + command.schedule() + elif not state.pressed_last and pressed: + command.cancel() + state.pressed_last = pressed + + return self + + def toggleOnTrue(self, command: Command) -> Self: + """ + Toggles a command when the condition changes from `False` to `True`. + + :param command: the command to toggle + :returns: this trigger, so calls can be chained + """ + + state = SimpleNamespace(pressed_last=self._condition()) + + @self._loop.bind + def _(): + pressed = self._condition() + if not state.pressed_last and pressed: + if command.isScheduled(): + command.cancel() + else: + command.schedule() + state.pressed_last = pressed + + return self + + def toggleOnFalse(self, command: Command) -> Self: + """ + Toggles a command when the condition changes from `True` to `False`. + + :param command: the command to toggle + :returns: this trigger, so calls can be chained + """ + + state = SimpleNamespace(pressed_last=self._condition()) + + @self._loop.bind + def _(): + pressed = self._condition() + if state.pressed_last and not pressed: + if command.isScheduled(): + command.cancel() + else: + command.schedule() + state.pressed_last = pressed + + return self + + def __call__(self) -> bool: + return self._condition() + + def getAsBoolean(self) -> bool: + return self._condition() + + def __bool__(self) -> bool: + return self._condition() + + def __and__(self, other: Callable[[], bool]) -> "Trigger": + assert callable(other) + return Trigger(self._loop, lambda: self() and other()) + + def and_(self, other: Callable[[], bool]) -> "Trigger": + """ + Composes two triggers with logical AND. + + :param trigger: the condition to compose with + :returns: A trigger which is active when both component triggers are active. + """ + return self & other + + def __or__(self, other: Callable[[], bool]) -> "Trigger": + assert callable(other) + return Trigger(self._loop, lambda: self() or other()) + + def or_(self, other: Callable[[], bool]) -> "Trigger": + """ + Composes two triggers with logical OR. + + :param trigger: the condition to compose with + :returns: A trigger which is active when either component trigger is active. + """ + return self | other + + def __invert__(self) -> "Trigger": + return Trigger(self._loop, lambda: not self()) + + def negate(self) -> "Trigger": + """ + Creates a new trigger that is active when this trigger is inactive, i.e. that acts as the + negation of this trigger. + + :returns: the negated trigger + """ + return ~self + + def not_(self) -> "Trigger": + return ~self + + def debounce( + self, + seconds: float, + debounce_type: Debouncer.DebounceType = Debouncer.DebounceType.kRising, + ) -> "Trigger": + """ + Creates a new debounced trigger from this trigger - it will become active when this trigger has + been active for longer than the specified period. + + :param seconds: The debounce period. + :param type: The debounce type. + :returns: The debounced trigger. + """ + debouncer = Debouncer(seconds, debounce_type) + return Trigger(self._loop, lambda: debouncer.calculate(self())) diff --git a/commandsv2/src/main/python/commands2/cmd.py b/commandsv2/src/main/python/commands2/cmd.py new file mode 100644 index 0000000000..42b9043e09 --- /dev/null +++ b/commandsv2/src/main/python/commands2/cmd.py @@ -0,0 +1,239 @@ +# validated: 2024-01-20 DS 8aeee0362672 Commands.java +from typing import Any, Callable, Dict, Hashable + +from wpimath import units + +from .command import Command +from .conditionalcommand import ConditionalCommand +from .functionalcommand import FunctionalCommand +from .instantcommand import InstantCommand +from .parallelcommandgroup import ParallelCommandGroup +from .paralleldeadlinegroup import ParallelDeadlineGroup +from .parallelracegroup import ParallelRaceGroup +from .printcommand import PrintCommand +from .runcommand import RunCommand +from .selectcommand import SelectCommand +from .sequentialcommandgroup import SequentialCommandGroup +from .startendcommand import StartEndCommand +from .subsystem import Subsystem +from .waitcommand import WaitCommand +from .waituntilcommand import WaitUntilCommand + +# Is this whole file just to avoid the `new` keyword in Java? + + +def none() -> Command: + """ + Constructs a command that does nothing, finishing immediately. + + :returns: the command + """ + return InstantCommand() + + +def idle(*requirements: Subsystem) -> Command: + """ + Constructs a command that does nothing until interrupted. + + :param requirements: Subsystems to require + :return: the command + """ + return run(lambda: None, *requirements) + + +def runOnce(action: Callable[[], Any], *requirements: Subsystem) -> Command: + """ + Constructs a command that runs an action once and finishes. + + :param action: the action to run + :param requirements: subsystems the action requires + :returns: the command + """ + return InstantCommand(action, *requirements) + + +def run(action: Callable[[], Any], *requirements: Subsystem) -> Command: + """ + Constructs a command that runs an action every iteration until interrupted. + + :param action: the action to run + :param requirements: subsystems the action requires + :returns: the command + """ + return RunCommand(action, *requirements) + + +def startEnd( + run: Callable[[], Any], end: Callable[[], Any], *requirements: Subsystem +) -> Command: + """ + Constructs a command that runs an action once and another action when the command is + interrupted. + + :param start: the action to run on start + :param end: the action to run on interrupt + :param requirements: subsystems the action requires + :returns: the command + """ + return StartEndCommand(run, end, *requirements) + + +def runEnd( + run: Callable[[], Any], end: Callable[[], Any], *requirements: Subsystem +) -> Command: + """ + Constructs a command that runs an action every iteration until interrupted, and then runs a + second action. + + :param run: the action to run every iteration + :param end: the action to run on interrupt + :param requirements: subsystems the action requires + :returns: the command + """ + return FunctionalCommand( + lambda: None, run, lambda interrupted: end(), lambda: False, *requirements + ) + + +def startRun( + start: Callable[[], Any], run: Callable[[], Any], *requirements: Subsystem +) -> Command: + """ + Constructs a command that runs an action once and another action every iteration until interrupted. + + :param start: the action to run on start + :param run: the action to run every iteration + :param requirements: subsystems the action requires + :returns: the command + """ + return FunctionalCommand( + start, run, lambda interrupt: None, lambda: False, *requirements + ) + + +def print_(message: str) -> Command: + """ + Constructs a command that prints a message and finishes. + + :param message: the message to print + :returns: the command + """ + return PrintCommand(message) + + +def waitSeconds(seconds: units.seconds) -> Command: + """ + Constructs a command that does nothing, finishing after a specified duration. + + :param seconds: after how long the command finishes + :returns: the command + """ + return WaitCommand(seconds) + + +def waitUntil(condition: Callable[[], bool]) -> Command: + """ + Constructs a command that does nothing, finishing once a condition becomes true. + + :param condition: the condition + :returns: the command + """ + return WaitUntilCommand(condition) + + +def either(onTrue: Command, onFalse: Command, selector: Callable[[], bool]) -> Command: + """ + Runs one of two commands, based on the boolean selector function. + + :param onTrue: the command to run if the selector function returns true + :param onFalse: the command to run if the selector function returns false + :param selector: the selector function + :returns: the command + """ + return ConditionalCommand(onTrue, onFalse, selector) + + +def select( + commands: Dict[Hashable, Command], selector: Callable[[], Hashable] +) -> Command: + """ + Runs one of several commands, based on the selector function. + + :param selector: the selector function + :param commands: map of commands to select from + :returns: the command + """ + return SelectCommand(commands, selector) + + +def sequence(*commands: Command) -> Command: + """ + Runs a group of commands in series, one after the other. + + :param commands: the commands to include + :returns: the command group + """ + return SequentialCommandGroup(*commands) + + +def repeatingSequence(*commands: Command) -> Command: + """ + Runs a group of commands in series, one after the other. Once the last command ends, the group + is restarted. + + :param commands: the commands to include + :returns: the command group + """ + return sequence(*commands).repeatedly() + + +def parallel(*commands: Command) -> Command: + """ + Runs a group of commands at the same time. Ends once all commands in the group finish. + + :param commands: the commands to include + :returns: the command + """ + return ParallelCommandGroup(*commands) + + +def race(*commands: Command) -> Command: + """ + Runs a group of commands at the same time. Ends once any command in the group finishes, and + cancels the others. + + :param commands: the commands to include + :returns: the command group + """ + return ParallelRaceGroup(*commands) + + +def deadline(deadline: Command, *commands: Command) -> Command: + """ + Runs a group of commands at the same time. Ends once a specific command finishes, and cancels + the others. + + :param deadline: the deadline command + :param commands: the commands to include + :returns: the command group + """ + return ParallelDeadlineGroup(deadline, *commands) + + +__all__ = [ + "none", + "runOnce", + "run", + "startEnd", + "runEnd", + "print_", + "waitSeconds", + "waitUntil", + "either", + "select", + "sequence", + "repeatingSequence", + "parallel", + "race", + "deadline", +] diff --git a/commandsv2/src/main/python/commands2/command.py b/commandsv2/src/main/python/commands2/command.py new file mode 100644 index 0000000000..26506a047a --- /dev/null +++ b/commandsv2/src/main/python/commands2/command.py @@ -0,0 +1,588 @@ +# validated: 2024-01-20 DS f29a7d2e501b Command.java +# Don't import stuff from the package here, it'll cause a circular import + + +from __future__ import annotations + +from enum import Enum +from typing import TYPE_CHECKING, Any, Callable, Set, Union + +from typing_extensions import Self, TypeAlias + +if TYPE_CHECKING: + from .instantcommand import InstantCommand + from .subsystem import Subsystem + from .parallelracegroup import ParallelRaceGroup + from .sequentialcommandgroup import SequentialCommandGroup + from .paralleldeadlinegroup import ParallelDeadlineGroup + from .parallelcommandgroup import ParallelCommandGroup + from .repeatcommand import RepeatCommand + from .proxycommand import ProxyCommand + from .conditionalcommand import ConditionalCommand + from .wrappercommand import WrapperCommand + +from wpiutil import Sendable, SendableRegistry, SendableBuilder + + +class InterruptionBehavior(Enum): + """ + An enum describing the command's behavior when another command with a shared requirement is + scheduled. + """ + + kCancelIncoming = 0 + """ + This command ends, :func:`commands2.Command.end` is called, and the incoming command is + scheduled normally. + + This is the default behavior. + """ + + kCancelSelf = 1 + """ This command continues, and the incoming command is not scheduled.""" + + +class Command(Sendable): + """ + A state machine representing a complete action to be performed by the robot. Commands are run by + the :class:`commands2.CommandScheduler`, and can be composed into CommandGroups to allow users to build + complicated multistep actions without the need to roll the state machine logic themselves. + + Commands are run synchronously from the main robot loop; no multithreading is used, unless + specified explicitly from the command implementation. + """ + + InterruptionBehavior: TypeAlias = ( + InterruptionBehavior # type alias for 2023 location + ) + + requirements: Set[Subsystem] + + def __new__(cls, *args, **kwargs) -> Self: + instance = super().__new__( + cls, + ) + super().__init__(instance) + SendableRegistry.add(instance, cls.__name__) + instance.requirements = set() + return instance + + def __init__(self): + pass + + def initialize(self): + """The initial subroutine of a command. Called once when the command is initially scheduled.""" + pass + + def execute(self): + """The main body of a command. Called repeatedly while the command is scheduled.""" + pass + + def end(self, interrupted: bool): + """ + The action to take when the command ends. Called when either the command finishes normally, or + when it interrupted/canceled. + + Do not schedule commands here that share requirements with this command. Use :meth:`.andThen` instead. + + :param interrupted: whether the command was interrupted/canceled + """ + pass + + def isFinished(self) -> bool: + """ + Whether the command has finished. Once a command finishes, the scheduler will call its :meth:`commands2.Command.end` + method and un-schedule it. + + :returns: whether the command has finished. + """ + return False + + def getRequirements(self) -> Set[Subsystem]: + """ + Specifies the set of subsystems used by this command. Two commands cannot use the same + subsystem at the same time. If another command is scheduled that shares a requirement, :meth:`.getInterruptionBehavior` will be checked and followed. If no subsystems are required, return + an empty set. + + .. note:: it is recommended that user implementations contain the requirements as a field, and + return that field here, rather than allocating a new set every time this is called. + + :returns: the set of subsystems that are required + """ + return self.requirements + + def addRequirements(self, *requirements: Subsystem): + """ + Adds the specified subsystems to the requirements of the command. The scheduler will prevent + two commands that require the same subsystem from being scheduled simultaneously. + + .. note:: The scheduler determines the requirements of a command when it is scheduled, so + this method should normally be called from the command's constructor. + + :param requirements: the requirements to add + """ + self.requirements.update(requirements) + + def getName(self) -> str: + """ + Gets the name of this Command. + + :returns: Name + """ + return SendableRegistry.getName(self) + + def setName(self, name: str): + """ + Sets the name of this Command. + + :param name: Name + """ + SendableRegistry.setName(self, name) + + def getSubsystem(self) -> str: + """ + Gets the subsystem name of this Subsystem. + + :returns: Subsystem name + """ + return SendableRegistry.getSubsystem(self) + + def setSubsystem(self, subsystem: str): + """ + Sets the subsystem name of this Subsystem. + + :param subsystem: subsystem name + """ + SendableRegistry.setSubsystem(self, subsystem) + + def withTimeout(self, seconds: float) -> ParallelRaceGroup: + """ + Decorates this command with a timeout. If the specified timeout is exceeded before the command + finishes normally, the command will be interrupted and un-scheduled. Note that the timeout only + applies to the command returned by this method; the calling command is not itself changed. + + .. note:: This decorator works by adding this command to a composition. + The command the decorator was called on cannot be scheduled + independently or be added to a different composition (namely, + decorators), unless it is manually cleared from the list of composed + commands with :func:`commands2.CommandScheduler.removeComposedCommand`. + The command composition returned from this method can be further + decorated without issue. + + :param seconds: the timeout duration + :returns: the command with the timeout added + """ + from .waitcommand import WaitCommand + + return self.raceWith(WaitCommand(seconds)) + + def until(self, condition: Callable[[], bool]) -> ParallelRaceGroup: + """ + Decorates this command with an interrupt condition. If the specified condition becomes true + before the command finishes normally, the command will be interrupted and un-scheduled. + + .. note:: This decorator works by adding this command to a composition. + The command the decorator was called on cannot be scheduled + independently or be added to a different composition (namely, + decorators), unless it is manually cleared from the list of composed + commands with :func:`commands2.CommandScheduler.removeComposedCommand`. + The command composition returned from this method can be further + decorated without issue. + + :param condition: the interrupt condition + :returns: the command with the interrupt condition added + """ + from .waituntilcommand import WaitUntilCommand + + return self.raceWith(WaitUntilCommand(condition)) + + def onlyWhile(self, condition: Callable[[], bool]) -> ParallelRaceGroup: + """ + Decorates this command with a run condition. If the specified condition becomes false before + the command finishes normally, the command will be interrupted and un-scheduled. + + .. note:: This decorator works by adding this command to a composition. + The command the decorator was called on cannot be scheduled + independently or be added to a different composition (namely, + decorators), unless it is manually cleared from the list of composed + commands with :func:`commands2.CommandScheduler.removeComposedCommand`. + The command composition returned from this method can be further + decorated without issue. + + :param condition: the interrupt condition + :returns: the command with the interrupt condition added + """ + assert callable(condition) + return self.until(lambda: not condition()) + + def beforeStarting( + self, before: Union[Command, Callable[[], None]] + ) -> SequentialCommandGroup: + """ + Decorates this command with a callable or command to run before this command starts. + + .. note:: This decorator works by adding this command to a composition. + The command the decorator was called on cannot be scheduled + independently or be added to a different composition (namely, + decorators), unless it is manually cleared from the list of composed + commands with :func:`commands2.CommandScheduler.removeComposedCommand`. + The command composition returned from this method can be further + decorated without issue. + + :param before: the command to run before this one + :returns: the decorated command + """ + from .instantcommand import InstantCommand + from .sequentialcommandgroup import SequentialCommandGroup + + if callable(before): + before = InstantCommand(before) + + return SequentialCommandGroup(before, self) + + def andThen(self, *next: Command) -> SequentialCommandGroup: + """ + Decorates this command with a set of commands to run after it in sequence. Often more + convenient/less-verbose than constructing a new SequentialCommandGroup explicitly. + + .. note:: This decorator works by adding this command to a composition. + The command the decorator was called on cannot be scheduled + independently or be added to a different composition (namely, + decorators), unless it is manually cleared from the list of composed + commands with :func:`commands2.CommandScheduler.removeComposedCommand`. + The command composition returned from this method can be further + decorated without issue. + + :param next: the commands to run next + :returns: the decorated command + """ + from .sequentialcommandgroup import SequentialCommandGroup + + return SequentialCommandGroup(self, *next) + + def deadlineWith(self, *parallel: Command) -> ParallelDeadlineGroup: + """ + Decorates this command with a set of commands to run parallel to it, ending when the calling + command ends and interrupting all the others. Often more convenient/less-verbose than + constructing a new ParallelDeadlineGroup explicitly. + + .. note:: This decorator works by adding this command to a composition. + The command the decorator was called on cannot be scheduled + independently or be added to a different composition (namely, + decorators), unless it is manually cleared from the list of composed + commands with :func:`commands2.CommandScheduler.removeComposedCommand`. + The command composition returned from this method can be further + decorated without issue. + + :param parallel: the commands to run in parallel + :returns: the decorated command + """ + import warnings + + warnings.warn( + "deadlineWith is deprecated use deadlineFor instead", + DeprecationWarning, + stacklevel=2, + ) + + from .paralleldeadlinegroup import ParallelDeadlineGroup + + return ParallelDeadlineGroup(self, *parallel) + + def deadlineFor(self, *parallel: Command) -> ParallelDeadlineGroup: + """ + Decorates this command with a set of commands to run parallel to it, ending when the calling + command ends and interrupting all the others. Often more convenient/less-verbose than + constructing a new ParallelDeadlineGroup explicitly. + + .. note:: This decorator works by adding this command to a composition. + The command the decorator was called on cannot be scheduled + independently or be added to a different composition (namely, + decorators), unless it is manually cleared from the list of composed + commands with :func:`commands2.CommandScheduler.removeComposedCommand`. + The command composition returned from this method can be further + decorated without issue. + + :param parallel: the commands to run in parallel + :returns: the decorated command + """ + from .paralleldeadlinegroup import ParallelDeadlineGroup + + return ParallelDeadlineGroup(self, *parallel) + + def alongWith(self, *parallel: Command) -> ParallelCommandGroup: + """ + Decorates this command with a set of commands to run parallel to it, ending when the last + command ends. Often more convenient/less-verbose than constructing a new {@link + ParallelCommandGroup} explicitly. + + .. note:: This decorator works by adding this command to a composition. + The command the decorator was called on cannot be scheduled + independently or be added to a different composition (namely, + decorators), unless it is manually cleared from the list of composed + commands with :func:`commands2.CommandScheduler.removeComposedCommand`. + The command composition returned from this method can be further + decorated without issue. + + :param parallel: the commands to run in parallel + :returns: the decorated command + """ + from .parallelcommandgroup import ParallelCommandGroup + + return ParallelCommandGroup(self, *parallel) + + def raceWith(self, *parallel: Command) -> ParallelRaceGroup: + """ + Decorates this command with a set of commands to run parallel to it, ending when the first + command ends. Often more convenient/less-verbose than constructing a new {@link + ParallelRaceGroup} explicitly. + + .. note:: This decorator works by adding this command to a composition. + The command the decorator was called on cannot be scheduled + independently or be added to a different composition (namely, + decorators), unless it is manually cleared from the list of composed + commands with :func:`commands2.CommandScheduler.removeComposedCommand`. + The command composition returned from this method can be further + decorated without issue. + + :param parallel: the commands to run in parallel + :returns: the decorated command + """ + from .parallelracegroup import ParallelRaceGroup + + return ParallelRaceGroup(self, *parallel) + + def repeatedly(self) -> RepeatCommand: + """ + Decorates this command to run repeatedly, restarting it when it ends, until this command is + interrupted. The decorated command can still be canceled. + + .. note:: This decorator works by adding this command to a composition. + The command the decorator was called on cannot be scheduled + independently or be added to a different composition (namely, + decorators), unless it is manually cleared from the list of composed + commands with :func:`commands2.CommandScheduler.removeComposedCommand`. + The command composition returned from this method can be further + decorated without issue. + + :returns: the decorated command + """ + from .repeatcommand import RepeatCommand + + return RepeatCommand(self) + + def asProxy(self) -> ProxyCommand: + """ + Decorates this command to run "by proxy" by wrapping it in a ProxyCommand. Use this for + "forking off" from command compositions when the user does not wish to extend the command's + requirements to the entire command composition. ProxyCommand has unique implications and + semantics, see the `WPILib docs `_ for a full explanation. + + :returns: the decorated command + """ + from .proxycommand import ProxyCommand + + return ProxyCommand(self) + + def unless(self, condition: Callable[[], bool]) -> ConditionalCommand: + """ + Decorates this command to only run if this condition is not met. If the command is already + running and the condition changes to true, the command will not stop running. The requirements + of this command will be kept for the new conditional command. + + .. note:: This decorator works by adding this command to a composition. + The command the decorator was called on cannot be scheduled + independently or be added to a different composition (namely, + decorators), unless it is manually cleared from the list of composed + commands with :func:`commands2.CommandScheduler.removeComposedCommand`. + The command composition returned from this method can be further + decorated without issue. + + :param condition: the condition that will prevent the command from running + :returns: the decorated command + """ + from .conditionalcommand import ConditionalCommand + from .instantcommand import InstantCommand + + return ConditionalCommand(InstantCommand(), self, condition) + + def onlyIf(self, condition: Callable[[], bool]) -> ConditionalCommand: + """ + Decorates this command to only run if this condition is met. If the command is already running + and the condition changes to false, the command will not stop running. The requirements of this + command will be kept for the new conditional command. + + .. note:: This decorator works by adding this command to a composition. + The command the decorator was called on cannot be scheduled + independently or be added to a different composition (namely, + decorators), unless it is manually cleared from the list of composed + commands with :func:`commands2.CommandScheduler.removeComposedCommand`. + The command composition returned from this method can be further + decorated without issue. + + :param condition: the condition that will allow the command to run + :returns: the decorated command + """ + return self.unless(lambda: not condition()) + + def ignoringDisable(self, doesRunWhenDisabled: bool) -> WrapperCommand: + """ + Decorates this command to run or stop when disabled. + + :param doesRunWhenDisabled: true to run when disabled. + :returns: the decorated command + """ + from .wrappercommand import WrapperCommand + + class W(WrapperCommand): + def runsWhenDisabled(self) -> bool: + return doesRunWhenDisabled + + return W(self) + + def withInterruptBehavior(self, behavior: InterruptionBehavior) -> WrapperCommand: + """ + Decorates this command to have a different InterruptionBehavior interruption behavior. + + :param interruptBehavior: the desired interrupt behavior + :returns: the decorated command + """ + from .wrappercommand import WrapperCommand + + class W(WrapperCommand): + def getInterruptionBehavior(self) -> InterruptionBehavior: + return behavior + + return W(self) + + def finallyDo(self, end: Callable[[bool], Any]) -> WrapperCommand: + """ + Decorates this command with a lambda to call on interrupt or end, following the command's + inherent :func:`commands2.Command.end` method. + + :param end: a lambda accepting a boolean parameter specifying whether the command was + interrupted. + :return: the decorated command + """ + from .wrappercommand import WrapperCommand + + class W(WrapperCommand): + def end(self, interrupted: bool) -> None: + self._command.end(interrupted) + end(interrupted) + + return W(self) + + def handleInterrupt(self, handler: Callable[[], Any]) -> WrapperCommand: + """ + Decorates this command with a lambda to call on interrupt, following the command's inherent + :func:`commands2.Command.end` method. + + :param handler: a lambda to run when the command is interrupted + :returns: the decorated command + """ + return self.finallyDo(lambda interrupted: handler() if interrupted else None) + + def schedule(self) -> None: + """Schedules this command.""" + from .commandscheduler import CommandScheduler + + CommandScheduler.getInstance().schedule(self) + + def cancel(self) -> None: + """ + Cancels this command. Will call ``end(true)``. Commands will be canceled + regardless of InterruptionBehavior interruption behavior. + """ + from .commandscheduler import CommandScheduler + + CommandScheduler.getInstance().cancel(self) + + def isScheduled(self) -> bool: + """ + Whether the command is currently scheduled. Note that this does not detect whether the command + is in a composition, only whether it is directly being run by the scheduler. + + :returns: Whether the command is scheduled. + """ + from .commandscheduler import CommandScheduler + + return CommandScheduler.getInstance().isScheduled(self) + + def hasRequirement(self, requirement: Subsystem) -> bool: + """ + Whether the command requires a given subsystem. + + :param requirement: the subsystem to inquire about + :returns: whether the subsystem is required + """ + return requirement in self.requirements + + def getInterruptionBehavior(self) -> InterruptionBehavior: + """ + How the command behaves when another command with a shared requirement is scheduled. + + :returns: a variant of InterruptionBehavior, defaulting to {@link InterruptionBehavior#kCancelSelf kCancelSelf}. + """ + return InterruptionBehavior.kCancelSelf + + def runsWhenDisabled(self) -> bool: + """ + Whether the given command should run when the robot is disabled. Override to return true if the + command should run when disabled. + + :returns: whether the command should run when the robot is disabled + """ + return False + + def withName(self, name: str) -> WrapperCommand: + """ + Decorates this command with a name. + + :param name: the name of the command + :returns: the decorated command + """ + from .wrappercommand import WrapperCommand + + wrapper = WrapperCommand(self) + wrapper.setName(name) + return wrapper + + def initSendable(self, builder: SendableBuilder) -> None: + from .commandscheduler import CommandScheduler + + builder.setSmartDashboardType("Command") + builder.addStringProperty( + ".name", + self.getName, + lambda _: None, + ) + + def on_set(value: bool) -> None: + if value: + if not self.isScheduled(): + self.schedule() + else: + if self.isScheduled(): + self.cancel() + + builder.addBooleanProperty( + "running", + self.isScheduled, + on_set, + ) + builder.addBooleanProperty( + ".isParented", + lambda: CommandScheduler.getInstance().isComposed(self), + lambda _: None, + ) + builder.addStringProperty( + "interruptBehavior", + lambda: self.getInterruptionBehavior().name, + lambda _: None, + ) + builder.addBooleanProperty( + "runsWhenDisabled", + self.runsWhenDisabled, + lambda _: None, + ) diff --git a/commandsv2/src/main/python/commands2/commandscheduler.py b/commandsv2/src/main/python/commands2/commandscheduler.py new file mode 100644 index 0000000000..73cc471b75 --- /dev/null +++ b/commandsv2/src/main/python/commands2/commandscheduler.py @@ -0,0 +1,643 @@ +# validated: 2024-01-23 DS 8aeee0362672 CommandScheduler.java +from __future__ import annotations + +import inspect +import os.path +import traceback +from typing import Any, Callable, Dict, Iterable, List, Optional, Set, Union + +import hal +from typing_extensions import Self +from wpilib import ( + RobotBase, + RobotState, + TimedRobot, + Watchdog, + reportWarning, +) +from wpilib.event import EventLoop +from wpiutil import Sendable, SendableBuilder, SendableRegistry + +from .command import Command, InterruptionBehavior +from .exceptions import IllegalCommandUse +from .subsystem import Subsystem + +_cmd_path = os.path.dirname(__file__) + + +class CommandScheduler(Sendable): + """ + The scheduler responsible for running Commands. A Command-based robot should call + :meth:`.run` on the singleton instance in its periodic block in order to run commands + synchronously from the main loop. Subsystems should be registered with the scheduler using :meth:`.registerSubsystem` in order for their :meth:`commands2.Subsystem.periodic` + methods to be called and for their default commands to be scheduled. + """ + + _instance: Optional[CommandScheduler] = None + + def __new__(cls) -> CommandScheduler: + if cls._instance is None: + return super().__new__(cls) + return cls._instance + + @staticmethod + def getInstance() -> CommandScheduler: + """ + Returns the Scheduler instance. + + :returns: the instance + """ + return CommandScheduler() + + @staticmethod + def resetInstance() -> None: + """ + Resets the scheduler instance, which is useful for testing purposes. This should not be + called by user code. + """ + inst = CommandScheduler._instance + if inst: + inst._defaultButtonLoop.clear() + SendableRegistry.remove(inst) + + CommandScheduler._instance = None + + def __init__(self) -> None: + """ + Gets the scheduler instance. + """ + super().__init__() + if CommandScheduler._instance is not None: + return + CommandScheduler._instance = self + self._composedCommands: Dict[Command, str] = {} + + # A set of the currently-running commands. + self._scheduledCommands: Dict[Command, None] = {} + + # A map from required subsystems to their requiring commands. Also used as a set + # of the currently-required subsystems. + self._requirements: Dict[Subsystem, Command] = {} + + # A map from subsystems registered with the scheduler to their default commands. + # Also used as a list of currently-registered subsystems. + self._subsystems: Dict[Subsystem, Optional[Command]] = {} + + self._defaultButtonLoop = EventLoop() + + # The set of currently-registered buttons that will be polled every iteration. + self._activeButtonLoop = self._defaultButtonLoop + + self._disabled = False + + # Lists of user-supplied actions to be executed on scheduling events for every + # command. + self._initActions: List[Callable[[Command], None]] = [] + self._executeActions: List[Callable[[Command], None]] = [] + self._interruptActions: List[Callable[[Command, Optional[Command]], None]] = [] + self._finishActions: List[Callable[[Command], None]] = [] + + self._inRunLoop = False + self._toSchedule: Dict[Command, None] = {} + # python: toCancelInterruptors stored in _toCancel dict + self._toCancel: Dict[Command, Optional[Command]] = {} + # self._toCancelInterruptors: List[Optional[Command]] = [] + self._endingCommands: Set[Command] = set() + + self._watchdog = Watchdog(TimedRobot.kDefaultPeriod, lambda: None) + + hal.reportUsage("CommandScheduler", "") + + def setPeriod(self, period: float) -> None: + """ + Changes the period of the loop overrun watchdog. This should be kept in sync with the + TimedRobot period. + + :param period: Period in seconds. + """ + self._watchdog.setTimeout(period) + + def getDefaultButtonLoop(self) -> EventLoop: + """ + Get the default button poll. + + :returns: a reference to the default EventLoop object polling buttons. + """ + return self._defaultButtonLoop + + def getActiveButtonLoop(self) -> EventLoop: + """ + Get the active button poll. + + :returns: a reference to the current EventLoop object polling buttons. + """ + return self._activeButtonLoop + + def setActiveButtonLoop(self, loop: EventLoop) -> None: + """ + Replace the button poll with another one. + + :param loop: the new button polling loop object. + """ + self._activeButtonLoop = loop + + def _initCommand(self, command: Command, *requirements: Subsystem) -> None: + """ + Initializes a given command, adds its requirements to the list, and performs the init actions. + + :param command: The command to initialize + :param requirements: The command requirements + """ + self._scheduledCommands[command] = None + for requirement in requirements: + self._requirements[requirement] = command + command.initialize() + for action in self._initActions: + action(command) + self._watchdog.addEpoch(f"{command.getName()}.initialize()") + + def schedule(self, *commands: Command) -> None: + """ + Schedules a command for execution. Does nothing if the command is already scheduled. If a + command's requirements are not available, it will only be started if all the commands currently + using those requirements have been scheduled as interruptible. If this is the case, they will + be interrupted and the command will be scheduled. + + WARNING: using this function directly can often lead to unexpected behavior and should be + avoided. Instead Triggers should be used to schedule Commands. + + :param commands: the commands to schedule. + """ + for command in commands: + self._schedule(command) + + def _schedule(self, command: Optional[Command]) -> None: + if command is None: + reportWarning("Tried to schedule a null command!", True) + return + + if self._inRunLoop: + self._toSchedule[command] = None + return + + self.requireNotComposed(command) + + # Do nothing if the scheduler is disabled, the robot is disabled and the command + # doesn't run when disabled, or the command is already scheduled. + + if self._disabled: + return + + if self.isScheduled(command): + return + + if RobotState.isDisabled() and not command.runsWhenDisabled(): + return + + requirements = command.getRequirements() + + # Schedule the command if the requirements are not currently in-use. + if self._requirements.keys().isdisjoint(requirements): + self._initCommand(command, *requirements) + else: + # Else check if the requirements that are in use have all have interruptible + # commands, and if so, interrupt those commands and schedule the new + # command. + for requirement in requirements: + requiringCommand = self.requiring(requirement) + if ( + requiringCommand is not None + and requiringCommand.getInterruptionBehavior() + == InterruptionBehavior.kCancelIncoming + ): + return + + for requirement in requirements: + requiringCommand = self.requiring(requirement) + if requiringCommand is not None: + self._cancel(requiringCommand, command) + + self._initCommand(command, *requirements) + + def run(self) -> None: + """ + Runs a single iteration of the scheduler. The execution occurs in the following order: + + * Subsystem periodic methods are called. + * Button bindings are polled, and new commands are scheduled from them. + * Currently-scheduled commands are executed. + * End conditions are checked on currently-scheduled commands, and commands that are finished + have their end methods called and are removed. + * Any subsystems not being used as requirements have their default methods started. + """ + if self._disabled: + return + self._watchdog.reset() + + # Run the periodic method of all registered subsystems. + for subsystem in self._subsystems: + subsystem.periodic() + if RobotBase.isSimulation(): + subsystem.simulationPeriodic() + self._watchdog.addEpoch(f"{subsystem.getName()}.periodic()") + + # Cache the active instance to avoid concurrency problems if setActiveLoop() is + # called from inside the button bindings. + loopCache = self._activeButtonLoop + # Poll buttons for new commands to add. + loopCache.poll() + self._watchdog.addEpoch("buttons.run()") + + self._inRunLoop = True + isDisabled = RobotState.isDisabled() + + # Run scheduled commands, remove finished commands. + for command in self._scheduledCommands.copy(): + if isDisabled and not command.runsWhenDisabled(): + self._cancel(command, None) + continue + + command.execute() + for action in self._executeActions: + action(command) + self._watchdog.addEpoch(f"{command.getName()}.execute()") + if command.isFinished(): + self._endingCommands.add(command) + command.end(False) + for action in self._finishActions: + action(command) + self._endingCommands.remove(command) + self._scheduledCommands.pop(command) + for requirement in command.getRequirements(): + self._requirements.pop(requirement) + self._watchdog.addEpoch(f"{command.getName()}.end(False)") + + self._inRunLoop = False + + # Schedule/cancel commands from queues populated during loop + for command in self._toSchedule: + self._schedule(command) + + for command, interruptor in self._toCancel.items(): + self._cancel(command, interruptor) + + self._toSchedule.clear() + self._toCancel.clear() + + # Add default commands for un-required registered subsystems. + for subsystem, scommand in self._subsystems.items(): + if subsystem not in self._requirements and scommand is not None: + self._schedule(scommand) + + self._watchdog.disable() + if self._watchdog.isExpired(): + self._watchdog.printEpochs() + + def registerSubsystem(self, *subsystems: Subsystem) -> None: + """ + Registers subsystems with the scheduler. This must be called for the subsystem's periodic block + to run when the scheduler is run, and for the subsystem's default command to be scheduled. It + is recommended to call this from the constructor of your subsystem implementations. + + :param subsystems: the subsystem to register + """ + for subsystem in subsystems: + if subsystem is None: + reportWarning("Tried to register a null subsystem", True) + continue + if subsystem in self._subsystems: + reportWarning("Tried to register an already-registered subsystem", True) + continue + self._subsystems[subsystem] = None + + def unregisterSubsystem(self, *subsystems: Subsystem) -> None: + """ + Un-registers subsystems with the scheduler. The subsystem will no longer have its periodic + block called, and will not have its default command scheduled. + + :param subsystems: the subsystem to un-register + """ + for subsystem in subsystems: + self._subsystems.pop(subsystem, None) + + def unregisterAllSubsystems(self): + """ + Un-registers all registered Subsystems with the scheduler. All currently registered subsystems + will no longer have their periodic block called, and will not have their default command + scheduled. + """ + self._subsystems.clear() + + def setDefaultCommand(self, subsystem: Subsystem, defaultCommand: Command) -> None: + """ + Sets the default command for a subsystem. Registers that subsystem if it is not already + registered. Default commands will run whenever there is no other command currently scheduled + that requires the subsystem. Default commands should be written to never end (i.e. their + :func:`commands2.Command.isFinished` method should return False), as they would simply be re-scheduled if they + do. Default commands must also require their subsystem. + + :param subsystem: the subsystem whose default command will be set + :param defaultCommand: the default command to associate with the subsystem + """ + if subsystem is None: + reportWarning("Tried to set a default command for a null subsystem", True) + return + if defaultCommand is None: + reportWarning("Tried to set a null default command", True) + return + + self.requireNotComposed(defaultCommand) + + if subsystem not in defaultCommand.getRequirements(): + raise IllegalCommandUse( + "Default commands must require their subsystem!", + command=defaultCommand, + subsystem=subsystem, + ) + + if ( + defaultCommand.getInterruptionBehavior() + == InterruptionBehavior.kCancelIncoming + ): + reportWarning( + "Registering a non-interruptible default command\nThis will likely prevent any other commands from requiring this subsystem.", + True, + ) + # Warn, but allow -- there might be a use case for this. + + self._subsystems[subsystem] = defaultCommand + + def removeDefaultCommand(self, subsystem: Subsystem) -> None: + """ + Removes the default command for a subsystem. The current default command will run until another + command is scheduled that requires the subsystem, at which point the current default command + will not be re-scheduled. + + :param subsystem: the subsystem whose default command will be removed + """ + if subsystem is None: + reportWarning( + "Tried to remove a default command for a null subsystem", True + ) + return + + self._subsystems[subsystem] = None + + def getDefaultCommand(self, subsystem: Subsystem) -> Optional[Command]: + """ + Gets the default command associated with this subsystem. Null if this subsystem has no default + command associated with it. + + :param subsystem: the subsystem to inquire about + :returns: the default command associated with the subsystem + """ + return self._subsystems[subsystem] + + def cancel( + self, + *commands: Command, + ) -> None: + """ + Cancels commands. The scheduler will only call :meth:`commands2.Command.end` method of the + canceled command with ``True``, indicating they were canceled (as opposed to finishing + normally). + + Commands will be canceled regardless of InterruptionBehavior interruption behavior. + + :param commands: the commands to cancel + """ + for command in commands: + self._cancel(command, None) + + def _cancel(self, command: Command, interruptor: Optional[Command]): + if command is None: + reportWarning("Tried to cancel a null command", True) + return + + if command in self._endingCommands: + return + + if self._inRunLoop: + self._toCancel[command] = interruptor + return + + if not self.isScheduled(command): + return + + self._endingCommands.add(command) + command.end(True) + for action in self._interruptActions: + action(command, interruptor) + + self._endingCommands.remove(command) + self._scheduledCommands.pop(command) + for requirement in command.getRequirements(): + del self._requirements[requirement] + self._watchdog.addEpoch(f"{command.getName()}.end(true)") + + def cancelAll(self) -> None: + """Cancels all commands that are currently scheduled.""" + self.cancel(*self._scheduledCommands) + + def isScheduled(self, *commands: Command) -> bool: + """ + Whether the given commands are running. Note that this only works on commands that are directly + scheduled by the scheduler; it will not work on commands inside compositions, as the scheduler + does not see them. + + :param commands: the command to query + :returns: whether the command is currently scheduled + """ + return all(command in self._scheduledCommands for command in commands) + + def requiring(self, subsystem: Subsystem) -> Optional[Command]: + """ + Returns the command currently requiring a given subsystem. None if no command is currently + requiring the subsystem + + :param subsystem: the subsystem to be inquired about + :returns: the command currently requiring the subsystem, or None if no command is currently + scheduled + """ + return self._requirements.get(subsystem) + + def disable(self) -> None: + """Disables the command scheduler.""" + self._disabled = True + + def enable(self) -> None: + """Enables the command scheduler.""" + self._disabled = False + + def printWatchdogEpochs(self) -> None: + """ + Prints list of epochs added so far and their times. + """ + self._watchdog.printEpochs() + + def onCommandInitialize(self, action: Callable[[Command], Any]) -> None: + """ + Adds an action to perform on the initialization of any command by the scheduler. + + :param action: the action to perform + """ + assert callable(action) + self._initActions.append(action) + + def onCommandExecute(self, action: Callable[[Command], Any]) -> None: + """ + Adds an action to perform on the execution of any command by the scheduler. + + :param action: the action to perform + """ + assert callable(action) + self._executeActions.append(action) + + def onCommandInterrupt(self, action: Callable[[Command], Any]) -> None: + """ + Adds an action to perform on the interruption of any command by the scheduler. + + :param action: the action to perform + """ + + assert callable(action) + self._interruptActions.append(lambda command, interruptor: action(command)) + + def onCommandInterruptWithCause( + self, action: Callable[[Command, Optional[Command]], Any] + ) -> None: + """ + Adds an action to perform on the interruption of any command by the scheduler. The action receives the interrupted command and the command that interrupted it + + :param action: the action to perform + """ + + assert callable(action) + self._interruptActions.append(action) + + def onCommandFinish(self, action: Callable[[Command], Any]) -> None: + """ + Adds an action to perform on the finishing of any command by the scheduler. + + :param action: the action to perform + """ + assert callable(action) + self._finishActions.append(action) + + def registerComposedCommands(self, commands: Iterable[Command]) -> None: + """ + Register commands as composed. An exception will be thrown if these commands are scheduled + directly or added to a composition. + + :param commands: the commands to register + + :raises IllegalCommandUse: if the given commands have already been composed. + """ + cmds = tuple(commands) + if len(set(cmds)) != len(cmds): + raise IllegalCommandUse( + "Cannot compose a command twice in the same composition!" + ) + + self.requireNotComposedOrScheduled(*cmds) + + # Find where the user called us from + # - it would be better to give a full traceback, but this is fine for now + location = "" + + for f, lineno in traceback.walk_stack(None): + info = inspect.getframeinfo(f) + if not info.filename.startswith(_cmd_path): + location = f"{info.filename}:{lineno}" + break + + for cmd in cmds: + self._composedCommands[cmd] = location + + def clearComposedCommands(self) -> None: + """ + Clears the list of composed commands, allowing all commands to be freely used again. + + .. warning:: Using this haphazardly can result in unexpected/undesirable behavior. Do not use + this unless you fully understand what you are doing. + """ + self._composedCommands.clear() + + def removeComposedCommand(self, command: Command) -> None: + """ + Removes a single command from the list of composed commands, allowing it to be freely used + again. + + .. warning:: Using this haphazardly can result in unexpected/undesirable behavior. Do not use + this unless you fully understand what you are doing. + + :param command: the command to remove from the list of grouped commands + """ + self._composedCommands.pop(command, None) + + def requireNotComposed(self, *commands: Command) -> None: + """ + Requires that the specified command hasn't been already added to a composition. + + :param commands: The commands to check + + :raises IllegalCommandUse: if the given commands have already been composed. + """ + for command in commands: + location = self._composedCommands.get(command) + if location is not None: + raise IllegalCommandUse( + "Commands that have been composed may not be added to another" + f"composition or scheduled individually (originally composed at {location})", + command=command, + ) + + def requireNotComposedOrScheduled(self, *commands: Command) -> None: + """ + Requires that the specified command hasn't already been added to a composition, and is not + currently scheduled. + + :param command: The command to check + + :raises IllegalCommandUse: if the given command has already been composed or scheduled. + """ + for command in commands: + if self.isScheduled(command): + raise IllegalCommandUse( + "Commands that have been scheduled individually may not be added to a composition!", + command=command, + ) + self.requireNotComposed(command) + + def isComposed(self, command: Command) -> bool: + """ + Check if the given command has been composed. + + :param command: The command to check + :returns: true if composed + """ + return command in self._composedCommands + + def initSendable(self, builder: SendableBuilder): + builder.setSmartDashboardType("Scheduler") + builder.addStringArrayProperty( + "Names", + lambda: [command.getName() for command in self._scheduledCommands], + lambda _: None, + ) + builder.addIntegerArrayProperty( + "Ids", + lambda: [id(command) for command in self._scheduledCommands], + lambda _: None, + ) + + def cancel_commands(to_cancel: List[int]): + ids = {id(command): command for command in self._scheduledCommands} + for hash_value in to_cancel: + cancelCmd = ids.get(hash_value) + if cancelCmd is not None: + self.cancel(cancelCmd) + + builder.addIntegerArrayProperty( + "Cancel", lambda: [], lambda to_cancel: cancel_commands(to_cancel) # type: ignore + ) diff --git a/commandsv2/src/main/python/commands2/conditionalcommand.py b/commandsv2/src/main/python/commands2/conditionalcommand.py new file mode 100644 index 0000000000..8e9720f3c0 --- /dev/null +++ b/commandsv2/src/main/python/commands2/conditionalcommand.py @@ -0,0 +1,93 @@ +# validated: 2024-01-19 DS aaea85ff1656 ConditionalCommand.java +from __future__ import annotations + +from typing import Callable, Optional + +from wpiutil import SendableBuilder + +from .command import Command, InterruptionBehavior +from .commandscheduler import CommandScheduler + + +class ConditionalCommand(Command): + """ + A command composition that runs one of two commands, depending on the value of the given + condition when this command is initialized. + + The rules for command compositions apply: command instances that are passed to it cannot be + added to any other composition or scheduled individually, and the composition requires all + subsystems its components require. + """ + + selectedCommand: Optional[Command] + + def __init__( + self, onTrue: Command, onFalse: Command, condition: Callable[[], bool] + ): + """ + Creates a new ConditionalCommand. + + :param onTrue: the command to run if the condition is true + :param onFalse: the command to run if the condition is false + :param condition: the condition to determine which command to run + """ + super().__init__() + + CommandScheduler.getInstance().registerComposedCommands([onTrue, onFalse]) + + self.onTrue = onTrue + self.onFalse = onFalse + self.condition = condition + + self.addRequirements(*onTrue.getRequirements()) + self.addRequirements(*onFalse.getRequirements()) + + def initialize(self): + if self.condition(): + self.selectedCommand = self.onTrue + else: + self.selectedCommand = self.onFalse + + self.selectedCommand.initialize() + + def execute(self): + assert self.selectedCommand is not None + self.selectedCommand.execute() + + def end(self, interrupted: bool): + assert self.selectedCommand is not None + self.selectedCommand.end(interrupted) + + def isFinished(self) -> bool: + assert self.selectedCommand is not None + return self.selectedCommand.isFinished() + + def runsWhenDisabled(self) -> bool: + return self.onTrue.runsWhenDisabled() and self.onFalse.runsWhenDisabled() + + def getInterruptionBehavior(self) -> InterruptionBehavior: + if ( + self.onTrue.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf + or self.onFalse.getInterruptionBehavior() + == InterruptionBehavior.kCancelSelf + ): + return InterruptionBehavior.kCancelSelf + else: + return InterruptionBehavior.kCancelIncoming + + def initSendable(self, builder: SendableBuilder): + super().initSendable(builder) + builder.addStringProperty("onTrue", self.onTrue.getName, lambda _: None) + builder.addStringProperty("onFalse", self.onFalse.getName, lambda _: None) + + def _selected(): + if self.selectedCommand is None: + return "null" + else: + return self.selectedCommand.getName() + + builder.addStringProperty( + "selected", + _selected, + lambda _: None, + ) diff --git a/commandsv2/src/main/python/commands2/deferredcommand.py b/commandsv2/src/main/python/commands2/deferredcommand.py new file mode 100644 index 0000000000..3115290b15 --- /dev/null +++ b/commandsv2/src/main/python/commands2/deferredcommand.py @@ -0,0 +1,71 @@ +# validated: 2024-01-24 DS 192a28af4731 DeferredCommand.java +from typing import Callable + +from wpiutil import SendableBuilder + +from .command import Command +from .commandscheduler import CommandScheduler +from .printcommand import PrintCommand +from .subsystem import Subsystem + + +class DeferredCommand(Command): + """ + Defers Command construction to runtime. Runs the command returned by a supplier when this command + is initialized, and ends when it ends. Useful for performing runtime tasks before creating a new + command. If this command is interrupted, it will cancel the command. + + Note that the supplier *must* create a new Command each call. For selecting one of a + preallocated set of commands, use :class:`commands2.SelectCommand`. + """ + + def __init__(self, supplier: Callable[[], Command], *requirements: Subsystem): + """ + + Creates a new DeferredCommand that directly runs the supplied command when initialized, and + ends when it ends. Useful for lazily creating commands when the DeferredCommand is initialized, + such as if the supplied command depends on runtime state. The Supplier will be called + each time this command is initialized. The Supplier *must* create a new Command each call. + + :param supplier: The command supplier + :param requirements: The command requirements. + """ + super().__init__() + + assert callable(supplier) + + self._null_command = PrintCommand( + f"[DeferredCommand] Supplied command (from {supplier!r} was None!" + ) + self._supplier = supplier + self._command = self._null_command + self.addRequirements(*requirements) + + def initialize(self): + cmd = self._supplier() + if cmd is not None: + self._command = cmd + CommandScheduler.getInstance().registerComposedCommands([self._command]) + self._command.initialize() + + def execute(self): + self._command.execute() + + def isFinished(self): + return self._command.isFinished() + + def end(self, interrupted): + self._command.end(interrupted) + self._command = self._null_command + + def initSendable(self, builder: SendableBuilder): + super().initSendable(builder) + builder.addStringProperty( + "deferred", + lambda: ( + "null" + if self._command is self._null_command + else self._command.getName() + ), + lambda _: None, + ) diff --git a/commandsv2/src/main/python/commands2/exceptions.py b/commandsv2/src/main/python/commands2/exceptions.py new file mode 100644 index 0000000000..4d9c3f3bc9 --- /dev/null +++ b/commandsv2/src/main/python/commands2/exceptions.py @@ -0,0 +1,19 @@ +# notrack + +import typing + + +class IllegalCommandUse(Exception): + """ + This exception is raised when a command is used in a way that it shouldn't be. + + You shouldn't try to catch this exception, if it occurs it means your code is + doing something it probably shouldn't be doing + """ + + def __init__(self, msg: str, **kwargs: typing.Any) -> None: + if kwargs: + args_repr = ", ".join(f"{k}={v!r}" for k, v in kwargs.items()) + msg = f"{msg} ({args_repr})" + + super().__init__(msg) diff --git a/commandsv2/src/main/python/commands2/functionalcommand.py b/commandsv2/src/main/python/commands2/functionalcommand.py new file mode 100644 index 0000000000..0fd864f9e5 --- /dev/null +++ b/commandsv2/src/main/python/commands2/functionalcommand.py @@ -0,0 +1,59 @@ +# validated: 2024-01-19 DS 6e58db398d63 FunctionalCommand.java +from __future__ import annotations + +from typing import Any, Callable + +from .command import Command +from .subsystem import Subsystem + + +class FunctionalCommand(Command): + """ + A command that allows the user to pass in functions for each of the basic command methods through + the constructor. Useful for inline definitions of complex commands - note, however, that if a + command is beyond a certain complexity it is usually better practice to write a proper class for + it than to inline it. + """ + + def __init__( + self, + onInit: Callable[[], Any], + onExecute: Callable[[], Any], + onEnd: Callable[[bool], Any], + isFinished: Callable[[], bool], + *requirements: Subsystem, + ): + """ + Creates a new FunctionalCommand. + + :param onInit: the function to run on command initialization + :param onExecute: the function to run on command execution + :param onEnd: the function to run on command end + :param isFinished: the function that determines whether the command has finished + :param requirements: the subsystems required by this command + """ + super().__init__() + + assert callable(onInit) + assert callable(onExecute) + assert callable(onEnd) + assert callable(isFinished) + + self._onInit = onInit + self._onExecute = onExecute + self._onEnd = onEnd + self._isFinished = isFinished + + self.addRequirements(*requirements) + + def initialize(self): + self._onInit() + + def execute(self): + self._onExecute() + + def end(self, interrupted: bool): + self._onEnd(interrupted) + + def isFinished(self) -> bool: + return self._isFinished() diff --git a/commandsv2/src/main/python/commands2/instantcommand.py b/commandsv2/src/main/python/commands2/instantcommand.py new file mode 100644 index 0000000000..9a651ff9d6 --- /dev/null +++ b/commandsv2/src/main/python/commands2/instantcommand.py @@ -0,0 +1,33 @@ +# validated: 2024-01-19 DS 5cf961edb973 InstantCommand.java +from __future__ import annotations + +from typing import Callable, Optional + +from .functionalcommand import FunctionalCommand +from .subsystem import Subsystem + + +class InstantCommand(FunctionalCommand): + """ + A Command that runs instantly; it will initialize, execute once, and end on the same iteration of + the scheduler. Users can either pass in a Callable and a set of requirements, or else subclass + this command if desired. + """ + + def __init__( + self, toRun: Optional[Callable[[], None]] = None, *requirements: Subsystem + ): + """ + Creates a new InstantCommand that runs the given Callable with the given requirements. + + :param toRun: the Runnable to run + :param requirements: the subsystems required by this command + """ + assert toRun is None or callable(toRun) + super().__init__( + toRun or (lambda: None), + lambda: None, + lambda _: None, + lambda: True, + *requirements, + ) diff --git a/commandsv2/src/main/python/commands2/notifiercommand.py b/commandsv2/src/main/python/commands2/notifiercommand.py new file mode 100644 index 0000000000..4bfedf1f3c --- /dev/null +++ b/commandsv2/src/main/python/commands2/notifiercommand.py @@ -0,0 +1,46 @@ +# validated: 2024-01-19 DS 6e58db398d63 NotifierCommand.java +from __future__ import annotations + +from typing import Any, Callable + +from wpilib import Notifier +from wpimath import units + +from .command import Command +from .subsystem import Subsystem + + +class NotifierCommand(Command): + """ + A command that starts a notifier to run the given Callable periodically in a separate thread. Has + no end condition as-is; either subclass it or use :func:`commands2.Command.withTimeout` or :func:`commands2.Command.until` to give it one. + + .. warning:: Do not use this class unless you are confident in your ability + to make the executed code thread-safe. If you do not know what + "thread-safe" means, that is a good sign that you should not use + this class. + """ + + def __init__( + self, toRun: Callable[[], Any], period: units.seconds, *requirements: Subsystem + ): + """ + Creates a new NotifierCommand. + + :param toRun: the Callable for the notifier to run + :param period: the period at which the notifier should run, in seconds + :param requirements: the subsystems required by this command + """ + super().__init__() + + assert callable(toRun) + + self._notifier = Notifier(toRun) + self._period = period + self.addRequirements(*requirements) + + def initialize(self): + self._notifier.startPeriodic(self._period) + + def end(self, interrupted: bool): + self._notifier.stop() diff --git a/commandsv2/src/main/python/commands2/parallelcommandgroup.py b/commandsv2/src/main/python/commands2/parallelcommandgroup.py new file mode 100644 index 0000000000..897e5ab002 --- /dev/null +++ b/commandsv2/src/main/python/commands2/parallelcommandgroup.py @@ -0,0 +1,95 @@ +# validated: 2024-01-19 DS aaea85ff1656 ParallelCommandGroup.java +from __future__ import annotations + +from typing import Dict + +from .command import Command, InterruptionBehavior +from .commandscheduler import CommandScheduler +from .exceptions import IllegalCommandUse +from .util import flatten_args_commands + + +class ParallelCommandGroup(Command): + """ + A command composition that runs a set of commands in parallel, ending when the last command ends. + + The rules for command compositions apply: command instances that are passed to it cannot be + added to any other composition or scheduled individually, and the composition requires all + subsystems its components require. + """ + + def __init__(self, *commands: Command): + """ + Creates a new ParallelCommandGroup. The given commands will be executed simultaneously. The + command composition will finish when the last command finishes. If the composition is + interrupted, only the commands that are still running will be interrupted. + + :param commands: the commands to include in this composition. + """ + super().__init__() + self._commands: Dict[Command, bool] = {} + self._runsWhenDisabled = True + self._interruptBehavior = InterruptionBehavior.kCancelIncoming + self.addCommands(*commands) + + def addCommands(self, *commands: Command): + """ + Adds the given commands to the group. + + :param commands: Commands to add to the group + """ + commands = flatten_args_commands(commands) + if True in self._commands.values(): + raise IllegalCommandUse( + "Commands cannot be added to a composition while it is running" + ) + + CommandScheduler.getInstance().registerComposedCommands(commands) + + for command in commands: + in_common = command.getRequirements().intersection(self.requirements) + if in_common: + raise IllegalCommandUse( + "Multiple commands in a parallel composition cannot require the same subsystems.", + common=in_common, + ) + + self._commands[command] = False + self.requirements.update(command.getRequirements()) + self._runsWhenDisabled = ( + self._runsWhenDisabled and command.runsWhenDisabled() + ) + + if command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf: + self._interruptBehavior = InterruptionBehavior.kCancelSelf + + def initialize(self): + for command in self._commands: + command.initialize() + self._commands[command] = True + + def execute(self): + for command, isRunning in self._commands.items(): + if not isRunning: + continue + command.execute() + if command.isFinished(): + command.end(False) + self._commands[command] = False + + def end(self, interrupted: bool): + if interrupted: + for command, isRunning in self._commands.items(): + if not isRunning: + continue + command.end(True) + self._commands[command] = False + + def isFinished(self) -> bool: + return True not in self._commands.values() + + def runsWhenDisabled(self) -> bool: + return self._runsWhenDisabled + + def getInterruptionBehavior(self) -> InterruptionBehavior: + return self._interruptBehavior diff --git a/commandsv2/src/main/python/commands2/paralleldeadlinegroup.py b/commandsv2/src/main/python/commands2/paralleldeadlinegroup.py new file mode 100644 index 0000000000..6a06cefe10 --- /dev/null +++ b/commandsv2/src/main/python/commands2/paralleldeadlinegroup.py @@ -0,0 +1,140 @@ +# validated: 2024-01-19 DS e07de37e64f2 ParallelDeadlineGroup.java +from __future__ import annotations + +from typing import Dict + +from wpiutil import SendableBuilder + +from .command import Command, InterruptionBehavior +from .commandscheduler import CommandScheduler +from .exceptions import IllegalCommandUse +from .util import flatten_args_commands + + +class ParallelDeadlineGroup(Command): + """ + A command composition that runs a set of commands in parallel, ending only when a specific + command (the "deadline") ends, interrupting all other commands that are still running at that + point. + + The rules for command compositions apply: command instances that are passed to it cannot be + added to any other composition or scheduled individually, and the composition requires all + subsystems its components require. + """ + + def __init__(self, deadline: Command, *commands: Command): + """ + Creates a new ParallelDeadlineGroup. The given commands (including the + deadline) will be executed simultaneously. The composition will finish when + the deadline finishes, interrupting all other still-running commands. If + the composition is interrupted, only the commands still running will be + interrupted. + + :param deadline: the command that determines when the composition ends + :param commands: the commands to be executed + + :raises IllegalCommandUse: if the deadline command is also in the otherCommands argument + """ + super().__init__() + self._commands: Dict[Command, bool] = {} + self._runsWhenDisabled = True + self._finished = True + self._interruptBehavior = InterruptionBehavior.kCancelIncoming + self.addCommands(*commands) + self.setDeadline(deadline) + + def setDeadline(self, deadline: Command): + """ + Sets the deadline to the given command. The deadline is added to the group if it is not already + contained. + + :param deadline: the command that determines when the group ends + + :raises IllegalCommandUse: if the deadline command is already in the composition + """ + + # use getattr here because deadline not set in constructor + isAlreadyDeadline = deadline == getattr(self, "_deadline", None) + if isAlreadyDeadline: + return + + if deadline in self._commands: + raise IllegalCommandUse( + f"The deadline command cannot also be in the other commands!", + deadline=deadline, + ) + self.addCommands(deadline) + self._deadline = deadline + + def addCommands(self, *commands: Command): + """ + Adds the given commands to the group. + + :param commands: Commands to add to the group. + + :raises IllegalCommandUse: if the deadline command is already in the composition + """ + commands = flatten_args_commands(commands) + if not self._finished: + raise IllegalCommandUse( + "Commands cannot be added to a composition while it is running" + ) + + CommandScheduler.getInstance().registerComposedCommands(commands) + + for command in commands: + in_common = command.getRequirements().intersection(self.requirements) + if in_common: + raise IllegalCommandUse( + "Multiple commands in a parallel composition cannot require the same subsystems.", + common=in_common, + ) + + self._commands[command] = False + self.requirements.update(command.getRequirements()) + self._runsWhenDisabled = ( + self._runsWhenDisabled and command.runsWhenDisabled() + ) + + if command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf: + self._interruptBehavior = InterruptionBehavior.kCancelSelf + + def initialize(self): + for command in self._commands: + command.initialize() + self._commands[command] = True + self._finished = False + + def execute(self): + for command, isRunning in self._commands.items(): + if not isRunning: + continue + command.execute() + if command.isFinished(): + command.end(False) + self._commands[command] = False + if command == self._deadline: + self._finished = True + + def end(self, interrupted: bool): + for command, isRunning in self._commands.items(): + if not isRunning: + continue + command.end(True) + self._commands[command] = False + + def isFinished(self) -> bool: + return self._finished + + def runsWhenDisabled(self) -> bool: + return self._runsWhenDisabled + + def getInterruptionBehavior(self) -> InterruptionBehavior: + return self._interruptBehavior + + def initSendable(self, builder: SendableBuilder): + super().initSendable(builder) + + builder.addStringProperty( + "deadline", lambda: self._deadline.getName(), lambda _: None + ) diff --git a/commandsv2/src/main/python/commands2/parallelracegroup.py b/commandsv2/src/main/python/commands2/parallelracegroup.py new file mode 100644 index 0000000000..032003d5cc --- /dev/null +++ b/commandsv2/src/main/python/commands2/parallelracegroup.py @@ -0,0 +1,90 @@ +# validated: 2024-01-19 DS aaea85ff1656 ParallelRaceGroup.java +from __future__ import annotations + +from typing import Set + +from .command import Command, InterruptionBehavior +from .commandscheduler import CommandScheduler +from .exceptions import IllegalCommandUse +from .util import flatten_args_commands + + +class ParallelRaceGroup(Command): + """ + A composition that runs a set of commands in parallel, ending when any one of the commands ends + and interrupting all the others. + + The rules for command compositions apply: command instances that are passed to it cannot be + added to any other composition or scheduled individually, and the composition requires all + subsystems its components require. + """ + + def __init__(self, *commands: Command): + """ + Creates a new ParallelCommandRace. The given commands will be executed simultaneously, and will + "race to the finish" - the first command to finish ends the entire command, with all other + commands being interrupted. + + :param commands: the commands to include in this composition. + """ + super().__init__() + self._commands: Set[Command] = set() + self._runsWhenDisabled = True + self._finished = True + self._interruptBehavior = InterruptionBehavior.kCancelIncoming + self.addCommands(*commands) + + def addCommands(self, *commands: Command): + """ + Adds the given commands to the group. + + :param commands: Commands to add to the group. + """ + commands = flatten_args_commands(commands) + if not self._finished: + raise IllegalCommandUse( + "Commands cannot be added to a composition while it is running" + ) + + CommandScheduler.getInstance().registerComposedCommands(commands) + + for command in commands: + in_common = command.getRequirements().intersection(self.requirements) + if in_common: + raise IllegalCommandUse( + "Multiple commands in a parallel composition cannot require the same subsystems.", + common=in_common, + ) + + self._commands.add(command) + self.requirements.update(command.getRequirements()) + self._runsWhenDisabled = ( + self._runsWhenDisabled and command.runsWhenDisabled() + ) + + if command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf: + self._interruptBehavior = InterruptionBehavior.kCancelSelf + + def initialize(self): + self._finished = False + for command in self._commands: + command.initialize() + + def execute(self): + for command in self._commands: + command.execute() + if command.isFinished(): + self._finished = True + + def end(self, interrupted: bool): + for command in self._commands: + command.end(not command.isFinished()) + + def isFinished(self) -> bool: + return self._finished + + def runsWhenDisabled(self) -> bool: + return self._runsWhenDisabled + + def getInterruptionBehavior(self) -> InterruptionBehavior: + return self._interruptBehavior diff --git a/commandsv2/src/main/python/commands2/pidcommand.py b/commandsv2/src/main/python/commands2/pidcommand.py new file mode 100644 index 0000000000..325dec52d2 --- /dev/null +++ b/commandsv2/src/main/python/commands2/pidcommand.py @@ -0,0 +1,78 @@ +# validated: 2024-01-19 DS f29a7d2e501b PIDCommand.java +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +from __future__ import annotations + +from typing import Any, Callable, Union + +from .command import Command +from .subsystem import Subsystem + +from wpimath.controller import PIDController + + +class PIDCommand(Command): + """ + A command that controls an output with a :class:`wpimath.controller.PIDController`. Runs forever by default - to add + exit conditions and/or other behavior, subclass this class. The controller calculation and output + are performed synchronously in the command's execute() method. + """ + + def __init__( + self, + controller: PIDController, + measurementSource: Callable[[], float], + setpoint: Union[Callable[[], float], float, int], + useOutput: Callable[[float], Any], + *requirements: Subsystem, + ): + """ + Creates a new PIDCommand, which controls the given output with a :class:`wpimath.controller.PIDController`. + + :param controller: the controller that controls the output. + :param measurementSource: the measurement of the process variable + :param setpoint: the controller's setpoint (either a function that returns a) + number or a number + :param useOutput: the controller's output + :param requirements: the subsystems required by this command + """ + super().__init__() + + assert callable(measurementSource) + assert callable(useOutput) + + self._controller = controller + self._useOutput = useOutput + self._measurement = measurementSource + + if isinstance(setpoint, (float, int)): + setpoint = float(setpoint) + self._setpoint = lambda: setpoint + elif callable(setpoint): + self._setpoint = setpoint + else: + raise ValueError( + f"invalid setpoint (must be callable or number; got {type(setpoint)})" + ) + + self.addRequirements(*requirements) + + def initialize(self): + self._controller.reset() + + def execute(self): + self._useOutput( + self._controller.calculate(self._measurement(), self._setpoint()) + ) + + def end(self, interrupted): + self._useOutput(0) + + def getController(self): + """ + Returns the PIDController used by the command. + + :return: The PIDController + """ + return self._controller diff --git a/commandsv2/src/main/python/commands2/pidsubsystem.py b/commandsv2/src/main/python/commands2/pidsubsystem.py new file mode 100644 index 0000000000..b875efdcfb --- /dev/null +++ b/commandsv2/src/main/python/commands2/pidsubsystem.py @@ -0,0 +1,100 @@ +# validated: 2024-01-19 DS f29a7d2e501b PIDSubsystem.java +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +from __future__ import annotations + +from wpimath.controller import PIDController + +from .subsystem import Subsystem + + +class PIDSubsystem(Subsystem): + """ + A subsystem that uses a :class:`wpimath.controller.PIDController` to control an output. The + controller is run synchronously from the subsystem's periodic() method. + """ + + def __init__(self, controller: PIDController, initial_position: float = 0.0): + """ + Creates a new PIDSubsystem. + + :param controller: The PIDController to use. + :param initial_position: The initial setpoint of the subsystem. + """ + super().__init__() + + self._controller = controller + self.setSetpoint(initial_position) + self.addChild("PID Controller", self._controller) + self._enabled = False + + def periodic(self): + """ + Executes the PID control logic during each periodic update. + + This method is called synchronously from the subsystem's periodic() method. + """ + if self._enabled: + self.useOutput( + self._controller.calculate(self.getMeasurement()), self.getSetpoint() + ) + + def getController(self) -> PIDController: + """ + Returns the PIDController used by the subsystem. + + :return: The PIDController. + """ + return self._controller + + def setSetpoint(self, setpoint: float): + """ + Sets the setpoint for the subsystem. + + :param setpoint: The setpoint for the subsystem. + """ + self._controller.setSetpoint(setpoint) + + def getSetpoint(self) -> float: + """ + Returns the current setpoint of the subsystem. + + :return: The current setpoint. + """ + return self._controller.getSetpoint() + + def useOutput(self, output: float, setpoint: float): + """ + Uses the output from the PIDController. + + :param output: The output of the PIDController. + :param setpoint: The setpoint of the PIDController (for feedforward). + """ + raise NotImplementedError(f"{self.__class__} must implement useOutput") + + def getMeasurement(self) -> float: + """ + Returns the measurement of the process variable used by the PIDController. + + :return: The measurement of the process variable. + """ + raise NotImplementedError(f"{self.__class__} must implement getMeasurement") + + def enable(self): + """Enables the PID control. Resets the controller.""" + self._enabled = True + self._controller.reset() + + def disable(self): + """Disables the PID control. Sets output to zero.""" + self._enabled = False + self.useOutput(0, 0) + + def isEnabled(self) -> bool: + """ + Returns whether the controller is enabled. + + :return: Whether the controller is enabled. + """ + return self._enabled diff --git a/commandsv2/src/main/python/commands2/printcommand.py b/commandsv2/src/main/python/commands2/printcommand.py new file mode 100644 index 0000000000..5327df73aa --- /dev/null +++ b/commandsv2/src/main/python/commands2/printcommand.py @@ -0,0 +1,21 @@ +# validated: 2024-01-19 DS 8ac45f20bb47 PrintCommand.java +from __future__ import annotations + +from .instantcommand import InstantCommand + + +class PrintCommand(InstantCommand): + """ + A command that prints a string when initialized. + """ + + def __init__(self, message: str): + """ + Creates a new a PrintCommand. + + :param message: the message to print + """ + super().__init__(lambda: print(message)) + + def runsWhenDisabled(self) -> bool: + return True diff --git a/commandsv2/src/main/python/commands2/profiledpidcommand.py b/commandsv2/src/main/python/commands2/profiledpidcommand.py new file mode 100644 index 0000000000..d7ce298b05 --- /dev/null +++ b/commandsv2/src/main/python/commands2/profiledpidcommand.py @@ -0,0 +1,81 @@ +# validated: 2024-01-24 DS f29a7d2e501b ProfiledPIDCommand.java +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +from typing import Any, Generic + +from wpimath.controller import ProfiledPIDController, ProfiledPIDControllerRadians +from wpimath.trajectory import TrapezoidProfile, TrapezoidProfileRadians + +from .command import Command +from .subsystem import Subsystem +from .typing import ( + FloatOrFloatSupplier, + FloatSupplier, + TProfiledPIDController, + UseOutputFunction, +) + + +class ProfiledPIDCommand(Command, Generic[TProfiledPIDController]): + """A command that controls an output with a :class:`.ProfiledPIDController`. Runs forever by default - + to add exit conditions and/or other behavior, subclass this class. The controller calculation and + output are performed synchronously in the command's execute() method. + """ + + _stateCls: Any + + def __init__( + self, + controller: TProfiledPIDController, + measurementSource: FloatSupplier, + goalSource: FloatOrFloatSupplier, + useOutput: UseOutputFunction, + *requirements: Subsystem, + ): + """Creates a new ProfiledPIDCommand, which controls the given output with a ProfiledPIDController. Goal + velocity is specified. + + :param controller: the controller that controls the output. + :param measurementSource: the measurement of the process variable + :param goalSource: the controller's goal + :param useOutput: the controller's output + :param requirements: the subsystems required by this command + """ + + super().__init__() + if isinstance(controller, ProfiledPIDController): + self._stateCls = TrapezoidProfile.State + elif isinstance(controller, ProfiledPIDControllerRadians): + self._stateCls = TrapezoidProfileRadians.State + else: + raise ValueError(f"unknown controller type {controller!r}") + + self._controller: TProfiledPIDController = controller + self._useOutput = useOutput + self._measurement = measurementSource + if isinstance(goalSource, (float, int)): + self._goal = lambda: float(goalSource) + else: + self._goal = goalSource + + self.addRequirements(*requirements) + + def initialize(self): + self._controller.reset(self._measurement()) + + def execute(self): + self._useOutput.accept( + self._controller.calculate(self._measurement(), self._goal()), + self._controller.getSetpoint(), + ) + + def end(self, interrupted: bool): + self._useOutput(0.0, self._stateCls()) + + def getController(self): + """Gets the controller used by the command""" + return self._controller diff --git a/commandsv2/src/main/python/commands2/profiledpidsubsystem.py b/commandsv2/src/main/python/commands2/profiledpidsubsystem.py new file mode 100644 index 0000000000..4a05baa77b --- /dev/null +++ b/commandsv2/src/main/python/commands2/profiledpidsubsystem.py @@ -0,0 +1,81 @@ +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. + +from typing import Generic + +from wpimath.trajectory import TrapezoidProfile + +from .subsystem import Subsystem +from .typing import TProfiledPIDController, TTrapezoidProfileState + + +class ProfiledPIDSubsystem( + Subsystem, Generic[TProfiledPIDController, TTrapezoidProfileState] +): + """ + A subsystem that uses a :class:`wpimath.controller.ProfiledPIDController` + or :class:`wpimath.controller.ProfiledPIDControllerRadians` to + control an output. The controller is run synchronously from the subsystem's + :meth:`.periodic` method. + """ + + def __init__( + self, + controller: TProfiledPIDController, + initial_position: float = 0, + ): + """ + Creates a new Profiled PID Subsystem using the provided PID Controller + + :param controller: the controller that controls the output + :param initial_position: the initial value of the process variable + + """ + super().__init__() + self._controller: TProfiledPIDController = controller + self._enabled = False + self.setGoal(initial_position) + + def periodic(self): + """Updates the output of the controller.""" + if self._enabled: + self.useOutput( + self._controller.calculate(self.getMeasurement()), + self._controller.getSetpoint(), + ) + + def getController( + self, + ) -> TProfiledPIDController: + """Returns the controller""" + return self._controller + + def setGoal(self, goal): + """Sets the goal state for the subsystem.""" + self._controller.setGoal(goal) + + def useOutput(self, output: float, setpoint: TTrapezoidProfileState): + """Uses the output from the controller object.""" + raise NotImplementedError(f"{self.__class__} must implement useOutput") + + def getMeasurement(self) -> float: + """ + Returns the measurement of the process variable used by the + controller object. + """ + raise NotImplementedError(f"{self.__class__} must implement getMeasurement") + + def enable(self): + """Enables the PID control. Resets the controller.""" + self._enabled = True + self._controller.reset(self.getMeasurement()) + + def disable(self): + """Disables the PID control. Sets output to zero.""" + self._enabled = False + self.useOutput(0, TrapezoidProfile.State()) + + def isEnabled(self) -> bool: + """Returns whether the controller is enabled.""" + return self._enabled diff --git a/commandsv2/src/main/python/commands2/proxycommand.py b/commandsv2/src/main/python/commands2/proxycommand.py new file mode 100644 index 0000000000..21d1cb1442 --- /dev/null +++ b/commandsv2/src/main/python/commands2/proxycommand.py @@ -0,0 +1,126 @@ +# validated: 2024-01-19 DS 192a28af4731 ProxyCommand.java +from __future__ import annotations + +from typing import Callable, overload + +from wpiutil import SendableBuilder + +from .command import Command +from .util import format_args_kwargs +import warnings + + +class ProxyCommand(Command): + """ + Schedules a given command when this command is initialized and ends when it ends, but does not + directly run it. Use this for including a command in a composition without adding its + requirements, but only if you know what you are doing. If you are unsure, see + `the WPILib docs `_ + for a complete explanation of proxy semantics. Do not proxy a command from a subsystem already + required by the composition, or else the composition will cancel itself when the proxy is reached. + If this command is interrupted, it will cancel the command. + """ + + _supplier: Callable[[], Command] + + @overload + def __init__(self, supplier: Callable[[], Command]): + """ + Creates a new ProxyCommand that schedules the supplied command when initialized, and ends when + it is no longer scheduled. Use this for lazily creating **proxied** commands at + runtime. Proxying should only be done to escape from composition requirement semantics, so if + only initialization time command construction is needed, use DeferredCommand instead. + + :param supplier: the command supplier + This constructor's similarity to DeferredCommand is confusing and opens + potential footguns for users who do not fully understand the semantics and implications of + proxying, but who simply want runtime construction. Users who do know what they are doing + and need a supplier-constructed proxied command should instead proxy a DeferredCommand + using the ``asProxy`` decorator. + """ + ... + + @overload + def __init__(self, command: Command): + """ + Creates a new ProxyCommand that schedules the given command when initialized, and ends when it + is no longer scheduled. + + :param command: the command to run by proxy + """ + ... + + def __init__(self, *args, **kwargs): + super().__init__() + + def init_supplier(supplier: Callable[[], Command]): + assert callable(supplier) + self._supplier = supplier + warnings.warn( + "The ProxyCommand supplier constructor has been deprecated", + DeprecationWarning, + stacklevel=3, + ) + + def init_command(command: Command): + self.setName(f"Proxy({command.getName()})") + self._supplier = lambda: command + + num_args = len(args) + len(kwargs) + + if num_args == 1 and len(kwargs) == 1: + if "supplier" in kwargs: + return init_supplier(kwargs["supplier"]) + elif "command" in kwargs: + return init_command(kwargs["command"]) + elif num_args == 1 and len(args) == 1: + if isinstance(args[0], Command): + return init_command(args[0]) + elif callable(args[0]): + return init_supplier(args[0]) + + raise TypeError( + f""" +TypeError: ProxyCommand(): incompatible function arguments. The following argument types are supported: + 1. (self: ProxyCommand, supplier: () -> Command) + 2. (self: ProxyCommand, command: Command) + +Invoked with: {format_args_kwargs(self, *args, **kwargs)} +""" + ) + + def initialize(self): + self._command = self._supplier() + self._command.schedule() + + def end(self, interrupted: bool): + assert self._command is not None + if interrupted: + self._command.cancel() + self._command = None + + def execute(self): + pass + + def isFinished(self) -> bool: + # because we're between `initialize` and `end`, `self._command` is necessarily not None + # but if called otherwise and m_command is None, + # it's UB, so we can do whatever we want -- like return true. + return self._command is None or not self._command.isScheduled() + + def runsWhenDisabled(self) -> bool: + """ + Whether the given command should run when the robot is disabled. Override to return true if the + command should run when disabled. + + :returns: true. Otherwise, this proxy would cancel commands that do run when disabled. + """ + return True + + def initSendable(self, builder: SendableBuilder): + super().initSendable(builder) + builder.addStringProperty( + "proxied", + lambda: "null" if self._command is None else self._command.getName(), + lambda _: None, + ) diff --git a/commandsv2/src/main/python/commands2/py.typed b/commandsv2/src/main/python/commands2/py.typed new file mode 100644 index 0000000000..e69de29bb2 diff --git a/commandsv2/src/main/python/commands2/repeatcommand.py b/commandsv2/src/main/python/commands2/repeatcommand.py new file mode 100644 index 0000000000..21cf019be7 --- /dev/null +++ b/commandsv2/src/main/python/commands2/repeatcommand.py @@ -0,0 +1,68 @@ +# validated: 2024-01-19 DS 6e58db398d63 RepeatCommand.java +from __future__ import annotations + +from wpiutil import SendableBuilder + +from .command import Command, InterruptionBehavior +from .commandscheduler import CommandScheduler + + +class RepeatCommand(Command): + """ + A command that runs another command repeatedly, restarting it when it ends, until this command is + interrupted. Command instances that are passed to it cannot be added to any other groups, or + scheduled individually. + + The rules for command compositions apply: command instances that are passed to it cannot be + added to any other composition or scheduled individually,and the composition requires all + subsystems its components require. + """ + + def __init__(self, command: Command): + """ + Creates a new RepeatCommand. Will run another command repeatedly, restarting it whenever it + ends, until this command is interrupted. + + :param command: the command to run repeatedly + """ + super().__init__() + self._command = command + CommandScheduler.getInstance().registerComposedCommands([command]) + self.requirements.update(command.getRequirements()) + self.setName(f"Repeat({command.getName()})") + + def initialize(self): + self._ended = False + self._command.initialize() + + def execute(self): + if self._ended: + self._ended = False + self._command.initialize() + + self._command.execute() + + if self._command.isFinished(): + # restart command + self._command.end(False) + self._ended = True + + def isFinished(self) -> bool: + return False + + def end(self, interrupted: bool): + # Make sure we didn't already call end() (which would happen if the command finished in the + # last call to our execute()) + if not self._ended: + self._command.end(interrupted) + self._ended = True + + def runsWhenDisabled(self) -> bool: + return self._command.runsWhenDisabled() + + def getInterruptionBehavior(self) -> InterruptionBehavior: + return self._command.getInterruptionBehavior() + + def initSendable(self, builder: SendableBuilder) -> None: + super().initSendable(builder) + builder.addStringProperty("command", self._command.getName, lambda _: None) diff --git a/commandsv2/src/main/python/commands2/runcommand.py b/commandsv2/src/main/python/commands2/runcommand.py new file mode 100644 index 0000000000..4dc1380723 --- /dev/null +++ b/commandsv2/src/main/python/commands2/runcommand.py @@ -0,0 +1,28 @@ +# validated: 2024-01-19 DS 5cf961edb973 RunCommand.java +from __future__ import annotations + +from typing import Any, Callable + +from .functionalcommand import FunctionalCommand +from .subsystem import Subsystem + + +class RunCommand(FunctionalCommand): + """ + A command that runs a Callable continuously. Has no end condition as-is; either subclass it or + use :func:`commands2.Command.withTimeout` or :func:`commands2.Command.until` to give it one. + If you only wish to execute a Callable once, use :class:`commands2.InstantCommand`. + """ + + def __init__(self, toRun: Callable[[], Any], *requirements: Subsystem): + """ + Creates a new RunCommand. The Callable will be run continuously until the command ends. Does + not run when disabled. + + :param toRun: the Callable to run + :param requirements: the subsystems to require + """ + assert callable(toRun) + super().__init__( + lambda: None, toRun, lambda interrupted: None, lambda: False, *requirements + ) diff --git a/commandsv2/src/main/python/commands2/schedulecommand.py b/commandsv2/src/main/python/commands2/schedulecommand.py new file mode 100644 index 0000000000..d6c3610874 --- /dev/null +++ b/commandsv2/src/main/python/commands2/schedulecommand.py @@ -0,0 +1,31 @@ +# validated: 2024-01-19 DS aaea85ff1656 ScheduleCommand.java +from __future__ import annotations + +from .command import Command + + +class ScheduleCommand(Command): + """ + Schedules the given commands when this command is initialized. Useful for forking off from + CommandGroups. Note that if run from a composition, the composition will not know about the + status of the scheduled commands, and will treat this command as finishing instantly. + """ + + def __init__(self, *commands: Command): + """ + Creates a new ScheduleCommand that schedules the given commands when initialized. + + :param toSchedule: the commands to schedule + """ + super().__init__() + self._toSchedule = set(commands) + + def initialize(self): + for command in self._toSchedule: + command.schedule() + + def isFinished(self) -> bool: + return True + + def runsWhenDisabled(self) -> bool: + return True diff --git a/commandsv2/src/main/python/commands2/selectcommand.py b/commandsv2/src/main/python/commands2/selectcommand.py new file mode 100644 index 0000000000..4b1a0fe328 --- /dev/null +++ b/commandsv2/src/main/python/commands2/selectcommand.py @@ -0,0 +1,86 @@ +# validated: 2024-01-19 DS a4a8ad9c753e SelectCommand.java +from __future__ import annotations + +from typing import Callable, Dict, Hashable + +from wpiutil import SendableBuilder + +from .command import Command, InterruptionBehavior +from .commandscheduler import CommandScheduler +from .printcommand import PrintCommand + + +class SelectCommand(Command): + """ + A command composition that runs one of a selection of commands, either using a selector and a key + to command mapping, or a supplier that returns the command directly at runtime. + + The rules for command compositions apply: command instances that are passed to it cannot be + added to any other composition or scheduled individually, and the composition requires all + subsystems its components require. + """ + + def __init__( + self, + commands: Dict[Hashable, Command], + selector: Callable[[], Hashable], + ): + """ + Creates a new SelectCommand. + + :param commands: the map of commands to choose from + :param selector: the selector to determine which command to run + """ + super().__init__() + + assert callable(selector) + + self._defaultCommand = PrintCommand( + "SelectCommand selector value does not correspond to any command!" + ) + + self._commands = commands + self._selector = selector + # This is slightly different than Java but avoids UB + self._selectedCommand = self._defaultCommand + self._runsWhenDisabled = True + self._interruptBehavior = InterruptionBehavior.kCancelIncoming + + scheduler = CommandScheduler.getInstance() + scheduler.registerComposedCommands([self._defaultCommand]) + scheduler.registerComposedCommands(commands.values()) + + for command in commands.values(): + self.addRequirements(*command.getRequirements()) + self._runsWhenDisabled = ( + self._runsWhenDisabled and command.runsWhenDisabled() + ) + if command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf: + self._interruptBehavior = InterruptionBehavior.kCancelSelf + + def initialize(self): + self._selectedCommand = self._commands.get( + self._selector(), self._defaultCommand + ) + self._selectedCommand.initialize() + + def execute(self): + self._selectedCommand.execute() + + def end(self, interrupted: bool): + self._selectedCommand.end(interrupted) + + def isFinished(self) -> bool: + return self._selectedCommand.isFinished() + + def runsWhenDisabled(self) -> bool: + return self._runsWhenDisabled + + def getInterruptionBehavior(self) -> InterruptionBehavior: + return self._interruptBehavior + + def initSendable(self, builder: SendableBuilder) -> None: + super().initSendable(builder) + builder.addStringProperty( + "selected", lambda: self._defaultCommand.getName(), lambda _: None + ) diff --git a/commandsv2/src/main/python/commands2/sequentialcommandgroup.py b/commandsv2/src/main/python/commands2/sequentialcommandgroup.py new file mode 100644 index 0000000000..e7ecbf7814 --- /dev/null +++ b/commandsv2/src/main/python/commands2/sequentialcommandgroup.py @@ -0,0 +1,101 @@ +# validated: 2024-01-19 DS aaea85ff1656 SequentialCommandGroup.java +from __future__ import annotations + +from typing import List + +from wpiutil import SendableBuilder + +from .command import Command, InterruptionBehavior +from .commandscheduler import CommandScheduler +from .exceptions import IllegalCommandUse +from .util import flatten_args_commands + + +class SequentialCommandGroup(Command): + """ + A command composition that runs a list of commands in sequence. + + The rules for command compositions apply: command instances that are passed to it cannot be + added to any other composition or scheduled individually, and the composition requires all + subsystems its components require. + """ + + def __init__(self, *commands: Command): + """ + Creates a new SequentialCommandGroup. The given commands will be run sequentially, with the + composition finishing when the last command finishes. + + :param commands: the commands to include in this composition. + """ + super().__init__() + self._commands: List[Command] = [] + self._currentCommandIndex = -1 + self._runsWhenDisabled = True + self._interruptBehavior = InterruptionBehavior.kCancelIncoming + self.addCommands(*commands) + + def addCommands(self, *commands: Command): + """ + Adds the given commands to the group. + + :param commands: Commands to add to the group. + """ + commands = flatten_args_commands(commands) + if self._currentCommandIndex != -1: + raise IllegalCommandUse( + "Commands cannot be added to a composition while it is running" + ) + + CommandScheduler.getInstance().registerComposedCommands(commands) + + for command in commands: + self._commands.append(command) + self.requirements.update(command.getRequirements()) + self._runsWhenDisabled = ( + self._runsWhenDisabled and command.runsWhenDisabled() + ) + if command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf: + self._interruptBehavior = InterruptionBehavior.kCancelSelf + + def initialize(self): + self._currentCommandIndex = 0 + if self._commands: + self._commands[0].initialize() + + def execute(self): + if not self._commands: + return + + currentCommand = self._commands[self._currentCommandIndex] + + currentCommand.execute() + if currentCommand.isFinished(): + currentCommand.end(False) + self._currentCommandIndex += 1 + if self._currentCommandIndex < len(self._commands): + self._commands[self._currentCommandIndex].initialize() + + def end(self, interrupted: bool): + if ( + interrupted + and self._commands + and -1 < self._currentCommandIndex < len(self._commands) + ): + self._commands[self._currentCommandIndex].end(True) + + self._currentCommandIndex = -1 + + def isFinished(self) -> bool: + return self._currentCommandIndex == len(self._commands) + + def runsWhenDisabled(self) -> bool: + return self._runsWhenDisabled + + def getInterruptionBehavior(self) -> InterruptionBehavior: + return self._interruptBehavior + + def initSendable(self, builder: SendableBuilder) -> None: + super().initSendable(builder) + builder.addIntegerProperty( + "index", lambda: self._currentCommandIndex, lambda _: None + ) diff --git a/commandsv2/src/main/python/commands2/startendcommand.py b/commandsv2/src/main/python/commands2/startendcommand.py new file mode 100644 index 0000000000..6059095bb8 --- /dev/null +++ b/commandsv2/src/main/python/commands2/startendcommand.py @@ -0,0 +1,36 @@ +# validated: 2024-01-19 DS b390cad09505 StartEndCommand.java +from __future__ import annotations + +from typing import Any, Callable + +from .functionalcommand import FunctionalCommand +from .subsystem import Subsystem + + +class StartEndCommand(FunctionalCommand): + """ + A command that runs a given Callable when it is initialized, and another Callable when it ends. + Useful for running and then stopping a motor, or extending and then retracting a solenoid. Has no + end condition as-is; either subclass it or use :func:`commands2.Command.withTimeout` + or :func:`commands2.Command.until` to give it one. + """ + + def __init__( + self, + onInit: Callable[[], Any], + onEnd: Callable[[], Any], + *requirements: Subsystem, + ): + """ + Creates a new StartEndCommand. Will run the given Callables when the command starts and when it + ends. + + :param onInit: the Callable to run on command init + :param onEnd: the Callable to run on command end + :param requirements: the subsystems required by this command + """ + assert callable(onInit) + assert callable(onEnd) + super().__init__( + onInit, lambda: None, lambda _: onEnd(), lambda: False, *requirements + ) diff --git a/commandsv2/src/main/python/commands2/subsystem.py b/commandsv2/src/main/python/commands2/subsystem.py new file mode 100644 index 0000000000..41cc5b6aff --- /dev/null +++ b/commandsv2/src/main/python/commands2/subsystem.py @@ -0,0 +1,248 @@ +# validated: 2024-01-20 DS 1144115da01f Subsystem.java +# Don't import stuff from the package here, it'll cause a circular import + +from __future__ import annotations + +from typing import TYPE_CHECKING, Callable, Optional +from typing_extensions import Self + +if TYPE_CHECKING: + from .command import Command + from .commandscheduler import CommandScheduler + +from wpiutil import Sendable, SendableBuilder, SendableRegistry + + +class Subsystem(Sendable): + """ + A robot subsystem. Subsystems are the basic unit of robot organization in the Command-based + framework; they encapsulate low-level hardware objects (motor controllers, sensors, etc.) and + provide methods through which they can be used by Commands. Subsystems are used by the + CommandScheduler's resource management system to ensure multiple robot actions are not + "fighting" over the same hardware; Commands that use a subsystem should include that subsystem in + their :func:`commands2.Command.getRequirements` method, and resources used within a subsystem should + generally remain encapsulated and not be shared by other parts of the robot. + + Subsystems must be registered with the scheduler with the :func:`commands2.CommandScheduler.registerSubsystem` + method in order for the :func:`.periodic` method to be called. It is recommended that this method be called from the + constructor of users' Subsystem implementations. + """ + + def __new__(cls, *arg, **kwargs) -> Self: + instance = super().__new__(cls) + super().__init__(instance) + SendableRegistry.add(instance, cls.__name__, cls.__name__) + # add to the scheduler + from .commandscheduler import CommandScheduler + + CommandScheduler.getInstance().registerSubsystem(instance) + return instance + + def __init__(self) -> None: + pass + + def periodic(self) -> None: + """ + This method is called periodically by the CommandScheduler. Useful for updating + subsystem-specific state that you don't want to offload to a Command. Teams should try + to be consistent within their own codebases about which responsibilities will be handled by + Commands, and which will be handled here. + """ + pass + + def simulationPeriodic(self) -> None: + """ + This method is called periodically by the CommandScheduler. Useful for updating + subsystem-specific state that needs to be maintained for simulations, such as for updating simulation classes and setting simulated sensor readings. + """ + pass + + def setDefaultCommand(self, command: Command) -> None: + """ + Sets the default Command of the subsystem. The default command will be automatically + scheduled when no other commands are scheduled that require the subsystem. Default commands + should generally not end on their own, i.e. their :func:`commands2.Command.isFinished` method should + always return false. Will automatically register this subsystem with the CommandScheduler. + + :param defaultCommand: the default command to associate with this subsystem + """ + from .commandscheduler import CommandScheduler + + CommandScheduler.getInstance().setDefaultCommand(self, command) + + def removeDefaultCommand(self) -> None: + """ + Removes the default command for the subsystem. This will not cancel the default command if it + is currently running. + """ + CommandScheduler.getInstance().removeDefaultCommand(self) + + def getDefaultCommand(self) -> Optional[Command]: + """ + Gets the default command for this subsystem. Returns None if no default command is currently + associated with the subsystem. + + :returns: the default command associated with this subsystem + """ + from .commandscheduler import CommandScheduler + + return CommandScheduler.getInstance().getDefaultCommand(self) + + def getCurrentCommand(self) -> Optional[Command]: + """ + Returns the command currently running on this subsystem. Returns None if no command is + currently scheduled that requires this subsystem. + + :returns: the scheduled command currently requiring this subsystem + """ + from .commandscheduler import CommandScheduler + + return CommandScheduler.getInstance().requiring(self) + + def register(self): + """ + Registers this subsystem with the :class:`.CommandScheduler`, allowing its + :func:`.periodic` method to be called when the scheduler runs. + """ + from .commandscheduler import CommandScheduler + + return CommandScheduler.getInstance().registerSubsystem(self) + + def runOnce(self, action: Callable[[], None]) -> Command: + """ + Constructs a command that runs an action once and finishes. Requires this subsystem. + + :param action: the action to run + :return: the command + """ + from .cmd import runOnce + + return runOnce(action, self) + + def run(self, action: Callable[[], None]) -> Command: + """ + Constructs a command that runs an action every iteration until interrupted. Requires this + subsystem. + + :param action: the action to run + :returns: the command""" + from .cmd import run + + return run(action, self) + + def startEnd(self, start: Callable[[], None], end: Callable[[], None]) -> Command: + """ + Constructs a command that runs an action once and another action when the command is + interrupted. Requires this subsystem. + + :param start: the action to run on start + :param end: the action to run on interrupt + :returns: the command + """ + from .cmd import startEnd + + return startEnd(start, end, self) + + def runEnd(self, run: Callable[[], None], end: Callable[[], None]) -> Command: + """ + Constructs a command that runs an action every iteration until interrupted, and then runs a + second action. Requires this subsystem. + + :param run: the action to run every iteration + :param end: the action to run on interrupt + :returns: the command + """ + from .cmd import runEnd + + return runEnd(run, end, self) + + def startRun(self, start: Callable[[], None], run: Callable[[], None]) -> Command: + """ + Constructs a command that runs an action once and another action every iteration until interrupted. Requires this subsystem. + + :param start: the action to run on start + :param run: the action to run every iteration + :returns: the command + """ + from .cmd import startRun + + return startRun(start, run, self) + + def idle(self) -> Command: + """ + Constructs a command that does nothing until interrupted. Requires this subsystem. + + :returns: the command + """ + from .cmd import idle + + return idle(self) + + # + # From SubsystemBase + # + + def getName(self) -> str: + """ + Gets the name of this Subsystem. + + :returns: Name + """ + return SendableRegistry.getName(self) + + def setName(self, name: str) -> None: + """ + Set the name of this Subsystem. + """ + SendableRegistry.setName(self, name) + + def getSubsystem(self) -> str: + """ + Gets the subsystem name of this Subsystem. + + :returns: Subsystem name + """ + return SendableRegistry.getSubsystem(self) + + def setSubsystem(self, subsystem: str): + """ + Sets the subsystem name of this Subsystem. + + :param subsystem: subsystem name + """ + SendableRegistry.setSubsystem(self, subsystem) + + def addChild(self, name: str, child: Sendable) -> None: + """ + Associates a :class:`wpiutil.Sendable` with this Subsystem. Also update the child's name. + + :param name: name to give child + :param child: sendable + """ + SendableRegistry.add(child, self.getSubsystem(), name) + + def initSendable(self, builder: SendableBuilder) -> None: + builder.setSmartDashboardType("Subsystem") + + builder.addBooleanProperty( + ".hasDefault", lambda: self.getDefaultCommand() is not None, lambda _: None + ) + + def get_default(): + command = self.getDefaultCommand() + if command is not None: + return command.getName() + return "none" + + builder.addStringProperty(".default", get_default, lambda _: None) + builder.addBooleanProperty( + ".hasCommand", lambda: self.getCurrentCommand() is not None, lambda _: None + ) + + def get_current(): + command = self.getCurrentCommand() + if command is not None: + return command.getName() + return "none" + + builder.addStringProperty(".command", get_current, lambda _: None) diff --git a/commandsv2/src/main/python/commands2/swervecontrollercommand.py b/commandsv2/src/main/python/commands2/swervecontrollercommand.py new file mode 100644 index 0000000000..a1e5a69705 --- /dev/null +++ b/commandsv2/src/main/python/commands2/swervecontrollercommand.py @@ -0,0 +1,111 @@ +# validated: 2024-01-20 DS 192a28af4731 SwerveControllerCommand.java +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +from __future__ import annotations +from typing import Callable, Optional, Union, Tuple, Sequence + +from wpimath.controller import ( + HolonomicDriveController, +) +from wpimath.geometry import Pose2d, Rotation2d +from wpimath.kinematics import ( + SwerveDrive2Kinematics, + SwerveDrive3Kinematics, + SwerveDrive4Kinematics, + SwerveDrive6Kinematics, + SwerveModuleState, +) +from wpimath.trajectory import Trajectory +from wpilib import Timer + +from .command import Command +from .subsystem import Subsystem + + +class SwerveControllerCommand(Command): + """ + A command that uses two PID controllers (:class:`wpimath.controller.PIDController`) + and a HolonomicDriveController (:class:`wpimath.controller.HolonomicDriveController`) + to follow a trajectory (:class:`wpimath.trajectory.Trajectory`) with a swerve drive. + + This command outputs the raw desired Swerve Module States (:class:`wpimath.kinematics.SwerveModuleState`) in an + array. The desired wheel and module rotation velocities should be taken from those and used in + velocity PIDs. + + The robot angle controller does not follow the angle given by the trajectory but rather goes + to the angle given in the final state of the trajectory. + """ + + def __init__( + self, + trajectory: Trajectory, + pose: Callable[[], Pose2d], + kinematics: Union[ + SwerveDrive2Kinematics, + SwerveDrive3Kinematics, + SwerveDrive4Kinematics, + SwerveDrive6Kinematics, + ], + controller: HolonomicDriveController, + outputModuleStates: Callable[[Sequence[SwerveModuleState]], None], + requirements: Tuple[Subsystem], + desiredRotation: Optional[Callable[[], Rotation2d]] = None, + ) -> None: + """ + Constructs a new SwerveControllerCommand that when executed will follow the + provided trajectory. This command will not return output voltages but + rather raw module states from the position controllers which need to be put + into a velocity PID. + + Note: The controllers will *not* set the outputVolts to zero upon + completion of the path- this is left to the user, since it is not + appropriate for paths with nonstationary endstates. + + :param trajectory: The trajectory to follow. + :param pose: A function that supplies the robot pose - use one of the odometry classes to + provide this. + :param kinematics: The kinematics for the robot drivetrain. Can be kinematics for 2/3/4/6 + SwerveKinematics. + :param controller: The HolonomicDriveController for the drivetrain. + If you have x, y, and theta controllers, pass them into + HolonomicPIDController. + :param outputModuleStates: The raw output module states from the position controllers. + :param requirements: The subsystems to require. + :param desiredRotation: (optional) The angle that the drivetrain should be + facing. This is sampled at each time step. If not specified, that rotation of + the final pose in the trajectory is used. + """ + super().__init__() + self._trajectory = trajectory + self._pose = pose + self._kinematics = kinematics + self._outputModuleStates = outputModuleStates + self._controller = controller + if desiredRotation is None: + self._desiredRotation = trajectory.states()[-1].pose.rotation + else: + self._desiredRotation = desiredRotation + + self._timer = Timer() + self.addRequirements(*requirements) + + def initialize(self): + self._timer.restart() + + def execute(self): + curTime = self._timer.get() + desiredState = self._trajectory.sample(curTime) + + targetChassisSpeeds = self._controller.calculate( + self._pose(), desiredState, self._desiredRotation() + ) + targetModuleStates = self._kinematics.toSwerveModuleStates(targetChassisSpeeds) + + self._outputModuleStates(targetModuleStates) + + def end(self, interrupted): + self._timer.stop() + + def isFinished(self): + return self._timer.hasElapsed(self._trajectory.totalTime()) diff --git a/commandsv2/src/main/python/commands2/sysid/__init__.py b/commandsv2/src/main/python/commands2/sysid/__init__.py new file mode 100644 index 0000000000..6a0719126c --- /dev/null +++ b/commandsv2/src/main/python/commands2/sysid/__init__.py @@ -0,0 +1,4 @@ +from .sysidroutine import SysIdRoutine + + +__all__ = ["SysIdRoutine"] diff --git a/commandsv2/src/main/python/commands2/sysid/sysidroutine.py b/commandsv2/src/main/python/commands2/sysid/sysidroutine.py new file mode 100644 index 0000000000..6b1b2b3bda --- /dev/null +++ b/commandsv2/src/main/python/commands2/sysid/sysidroutine.py @@ -0,0 +1,177 @@ +# validated: 2024-02-20 DV ee15cc172a5e sysid/SysIdRoutine.java +from dataclasses import dataclass, field +from enum import Enum + +from wpilib.sysid import SysIdRoutineLog, State +from ..command import Command +from ..subsystem import Subsystem +from wpilib import Timer + +from wpimath.units import seconds, volts + +from typing import Callable, Optional + + +volts_per_second = float + + +class SysIdRoutine(SysIdRoutineLog): + """A SysId characterization routine for a single mechanism. Mechanisms may have multiple motors. + + A single subsystem may have multiple mechanisms, but mechanisms should not share test + routines. Each complete test of a mechanism should have its own SysIdRoutine instance, since the + log name of the recorded data is determined by the mechanism name. + + The test state (e.g. "quasistatic-forward") is logged once per iteration during test + execution, and once with state "none" when a test ends. Motor frames are logged every iteration + during test execution. + + Timestamps are not coordinated across data, so motor frames and test state tags may be + recorded on different log frames. Because frame alignment is not guaranteed, SysId parses the log + by using the test state flag to determine the timestamp range for each section of the test, and + then extracts the motor frames within the valid timestamp ranges. If a given test was run + multiple times in a single logfile, the user will need to select which of the tests to use for + the fit in the analysis tool. + """ + + @dataclass + class Config: + """Hardware-independent configuration for a SysId test routine. + + :param rampRate: The voltage ramp rate used for quasistatic test routines. Defaults to 1 volt + per second if left null. + :param stepVoltage: The step voltage output used for dynamic test routines. Defaults to 7 + volts if left null. + :param timeout: Safety timeout for the test routine commands. Defaults to 10 seconds if left + null. + :param recordState: Optional handle for recording test state in a third-party logging + solution. If provided, the test routine state will be passed to this callback instead of + logged in WPILog. + """ + + rampRate: volts_per_second = 1.0 + stepVoltage: volts = 7.0 + timeout: seconds = 10.0 + recordState: Optional[Callable[[State], None]] = None + + @dataclass + class Mechanism: + """A mechanism to be characterized by a SysId routine. + + Defines callbacks needed for the SysId test routine to control + and record data from the mechanism. + + :param drive: Sends the SysId-specified drive signal to the mechanism motors during test + routines. + :param log: Returns measured data of the mechanism motors during test routines. To return + data, call `motor(string motorName)` on the supplied `SysIdRoutineLog` instance, and then + call one or more of the chainable logging handles (e.g. `voltage`) on the returned + `MotorLog`. Multiple motors can be logged in a single callback by calling `motor` + multiple times. + :param subsystem: The subsystem containing the motor(s) that is (or are) being characterized. + Will be declared as a requirement for the returned test commands. + :param name: The name of the mechanism being tested. Will be appended to the log entry title + for the routine's test state, e.g. "sysid-test-state-mechanism". Defaults to the name of + the subsystem if left null. + """ + + drive: Callable[[volts], None] + log: Callable[[SysIdRoutineLog], None] + subsystem: Subsystem + name: str = None # type: ignore[assignment] + + def __post_init__(self): + if self.name is None: + self.name = self.subsystem.getName() + + class Direction(Enum): + """Motor direction for a SysId test.""" + + kForward = 1 + kReverse = -1 + + def __init__(self, config: Config, mechanism: Mechanism): + """Create a new SysId characterization routine. + + :param config: Hardware-independent parameters for the SysId routine. + :param mechanism: Hardware interface for the SysId routine. + """ + super().__init__(mechanism.name) + self.config = config + self.mechanism = mechanism + self.outputVolts = 0.0 + self.logState = config.recordState or self.recordState + + def quasistatic(self, direction: Direction) -> Command: + """Returns a command to run a quasistatic test in the specified direction. + + The command will call the `drive` and `log` callbacks supplied at routine construction once + per iteration. Upon command end or interruption, the `drive` callback is called with a value of + 0 volts. + + :param direction: The direction in which to run the test. + + :returns: A command to run the test. + """ + + timer = Timer() + if direction == self.Direction.kForward: + state = State.kQuasistaticForward + else: + state = State.kQuasistaticReverse + + def execute(): + self.outputVolts = direction.value * timer.get() * self.config.rampRate + self.mechanism.drive(self.outputVolts) + self.mechanism.log(self) + self.logState(state) + + def end(interrupted: bool): + self.mechanism.drive(0.0) + self.logState(State.kNone) + timer.stop() + + return ( + self.mechanism.subsystem.runOnce(timer.restart) + .andThen(self.mechanism.subsystem.run(execute)) + .finallyDo(end) + .withName(f"sysid-{state}-{self.mechanism.name}") + .withTimeout(self.config.timeout) + ) + + def dynamic(self, direction: Direction) -> Command: + """Returns a command to run a dynamic test in the specified direction. + + The command will call the `drive` and `log` callbacks supplied at routine construction once + per iteration. Upon command end or interruption, the `drive` callback is called with a value of + 0 volts. + + :param direction: The direction in which to run the test. + + :returns: A command to run the test. + """ + + if direction == self.Direction.kForward: + state = State.kDynamicForward + else: + state = State.kDynamicReverse + + def command(): + self.outputVolts = direction.value * self.config.stepVoltage + + def execute(): + self.mechanism.drive(self.outputVolts) + self.mechanism.log(self) + self.logState(state) + + def end(interrupted: bool): + self.mechanism.drive(0.0) + self.logState(State.kNone) + + return ( + self.mechanism.subsystem.runOnce(command) + .andThen(self.mechanism.subsystem.run(execute)) + .finallyDo(end) + .withName(f"sysid-{state}-{self.mechanism.name}") + .withTimeout(self.config.timeout) + ) diff --git a/commandsv2/src/main/python/commands2/timedcommandrobot.py b/commandsv2/src/main/python/commands2/timedcommandrobot.py new file mode 100644 index 0000000000..a012e4f842 --- /dev/null +++ b/commandsv2/src/main/python/commands2/timedcommandrobot.py @@ -0,0 +1,16 @@ +# notrack +from wpilib import TimedRobot + +from .commandscheduler import CommandScheduler + +seconds = float + + +class TimedCommandRobot(TimedRobot): + kSchedulerOffset = 0.005 + + def __init__(self, period: seconds = TimedRobot.kDefaultPeriod / 1000) -> None: + super().__init__(period) + self.addPeriodic( + CommandScheduler.getInstance().run, period, self.kSchedulerOffset + ) diff --git a/commandsv2/src/main/python/commands2/trapezoidprofilecommand.py b/commandsv2/src/main/python/commands2/trapezoidprofilecommand.py new file mode 100644 index 0000000000..4ffff24e22 --- /dev/null +++ b/commandsv2/src/main/python/commands2/trapezoidprofilecommand.py @@ -0,0 +1,64 @@ +# validated: 2024-01-24 DS 192a28af4731 TrapezoidProfileCommand.java +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# +from __future__ import annotations + +from typing import Callable, Any + +from wpilib import Timer + +from .command import Command +from .subsystem import Subsystem + + +class TrapezoidProfileCommand(Command): + """ + A command that runs a :class:`wpimath.trajectory.TrapezoidProfile`. Useful for smoothly controlling mechanism motion. + """ + + def __init__( + self, + profile, + output: Callable[[Any], Any], + goal: Callable[[], Any], + currentState: Callable[[], Any], + *requirements: Subsystem, + ): + """Creates a new TrapezoidProfileCommand that will execute the given :class:`wpimath.trajectory.TrapezoidProfile`. + Output will be piped to the provided consumer function. + + :param profile: The motion profile to execute. + :param output: The consumer for the profile output. + :param goal: The supplier for the desired state + :param currentState: The supplier for the current state + :param requirements: The subsystems required by this command. + """ + super().__init__() + self._profile = profile + self._output = output + self._goal = goal + self._currentState = currentState + self._timer = Timer() + + self.addRequirements(*requirements) + + def initialize(self) -> None: + self._timer.restart() + + def execute(self) -> None: + self._output( + self._profile.calculate( + self._timer.get(), + self._currentState(), + self._goal(), + ) + ) + + def end(self, interrupted) -> None: + self._timer.stop() + + def isFinished(self) -> bool: + return self._timer.hasElapsed(self._profile.totalTime()) diff --git a/commandsv2/src/main/python/commands2/trapezoidprofilesubsystem.py b/commandsv2/src/main/python/commands2/trapezoidprofilesubsystem.py new file mode 100644 index 0000000000..8efb263280 --- /dev/null +++ b/commandsv2/src/main/python/commands2/trapezoidprofilesubsystem.py @@ -0,0 +1,94 @@ +# validated: 2024-02-20 DV 6cc7e52de74a TrapezoidProfileSubsystem.java +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# +from __future__ import annotations + +from typing import Any, Union + +from wpimath import units +from wpimath.trajectory import TrapezoidProfile, TrapezoidProfileRadians + + +from .subsystem import Subsystem + + +class TrapezoidProfileSubsystem(Subsystem): + """ + A subsystem that generates and runs trapezoidal motion profiles automatically. The user specifies + how to use the current state of the motion profile by overriding the `useState` method. + """ + + _profile: Any + _stateCls: Any + + def __init__( + self, + constraints: Union[ + TrapezoidProfile.Constraints, TrapezoidProfileRadians.Constraints + ], + initial_position: float = 0.0, + period: units.seconds = 0.02, + ): + """ + Creates a new TrapezoidProfileSubsystem. + + :param constraints: The constraints (maximum velocity and acceleration) for the profiles. + :param initial_position: The initial position of the controlled mechanism when the subsystem is constructed. + :param period: The period of the main robot loop, in seconds. + """ + if isinstance(constraints, TrapezoidProfile.Constraints): + self._profile = TrapezoidProfile(constraints) + self._stateCls = TrapezoidProfile.State + elif isinstance(constraints, TrapezoidProfileRadians.Constraints): + self._profile = TrapezoidProfileRadians(constraints) + self._stateCls = TrapezoidProfileRadians.State + else: + raise ValueError(f"Invalid constraints {constraints}") + + self._state = self._stateCls(initial_position, 0) + self.setGoal(initial_position) + self._period = period + self._enabled = True + + def periodic(self): + """ + Executes the TrapezoidProfileSubsystem logic during each periodic update. + + This method is called synchronously from the subsystem's periodic() method. + """ + self._state = self._profile.calculate(self._period, self._state, self._goal) + if self._enabled: + self.useState(self._state) + + def setGoal(self, goal): + """ + Sets the goal state for the subsystem. Goal velocity assumed to be zero. + + :param goal: The goal position for the subsystem's motion profile. The goal + can either be a `TrapezoidProfile.State` or `float`. If float is provided, + the assumed velocity for the goal will be 0. + """ + # If we got a float, instantiate the state + if isinstance(goal, (float, int)): + goal = self._stateCls(goal, 0) + + self._goal = goal + + def enable(self): + """Enable the TrapezoidProfileSubsystem's output.""" + self._enabled = True + + def disable(self): + """Disable the TrapezoidProfileSubsystem's output.""" + self._enabled = False + + def useState(self, state): + """ + Users should override this to consume the current state of the motion profile. + + :param state: The current state of the motion profile. + """ + raise NotImplementedError(f"{self.__class__} must implement useState") diff --git a/commandsv2/src/main/python/commands2/typing.py b/commandsv2/src/main/python/commands2/typing.py new file mode 100644 index 0000000000..616833085a --- /dev/null +++ b/commandsv2/src/main/python/commands2/typing.py @@ -0,0 +1,30 @@ +from typing import Callable, Protocol, TypeVar, Union + +from typing_extensions import TypeAlias +from wpimath.controller import ProfiledPIDController, ProfiledPIDControllerRadians +from wpimath.trajectory import TrapezoidProfile, TrapezoidProfileRadians + +# Generic Types +TProfiledPIDController = TypeVar( + "TProfiledPIDController", ProfiledPIDControllerRadians, ProfiledPIDController +) +TTrapezoidProfileState = TypeVar( + "TTrapezoidProfileState", + TrapezoidProfileRadians.State, + TrapezoidProfile.State, +) + + +# Protocols - Structural Typing +class UseOutputFunction(Protocol): + + def __init__(self, *args, **kwargs) -> None: ... + + def __call__(self, t: float, u: TTrapezoidProfileState) -> None: ... + + def accept(self, t: float, u: TTrapezoidProfileState) -> None: ... + + +# Type Aliases +FloatSupplier: TypeAlias = Callable[[], float] +FloatOrFloatSupplier: TypeAlias = Union[float, Callable[[], float]] diff --git a/commandsv2/src/main/python/commands2/util.py b/commandsv2/src/main/python/commands2/util.py new file mode 100644 index 0000000000..e0178c9416 --- /dev/null +++ b/commandsv2/src/main/python/commands2/util.py @@ -0,0 +1,25 @@ +# notrack +from __future__ import annotations + +from typing import Iterable, List, Tuple, Union + +from .command import Command + + +def flatten_args_commands( + *commands: Union[Command, Iterable[Command]] +) -> Tuple[Command, ...]: + flattened_commands: List[Command] = [] + for command in commands: + if isinstance(command, Command): + flattened_commands.append(command) + elif isinstance(command, Iterable): + flattened_commands.extend(flatten_args_commands(*command)) + return tuple(flattened_commands) + + +def format_args_kwargs(*args, **kwargs) -> str: + return ", ".join( + [repr(arg) for arg in args] + + [f"{key}={repr(value)}" for key, value in kwargs.items()] + ) diff --git a/commandsv2/src/main/python/commands2/waitcommand.py b/commandsv2/src/main/python/commands2/waitcommand.py new file mode 100644 index 0000000000..87c0337e4e --- /dev/null +++ b/commandsv2/src/main/python/commands2/waitcommand.py @@ -0,0 +1,41 @@ +# validated: 2024-01-20 DS f29a7d2e501b WaitCommand.java +from __future__ import annotations + +from wpilib import Timer +from wpimath import units +from wpiutil import SendableBuilder + +from .command import Command + + +class WaitCommand(Command): + """ + A command that does nothing but takes a specified amount of time to finish. + """ + + def __init__(self, seconds: units.seconds): + """ + Creates a new WaitCommand. This command will do nothing, and end after the specified duration. + + :param seconds: the time to wait, in seconds + """ + super().__init__() + self._duration = seconds + self._timer = Timer() + self.setName(f"{self.getName()}: {seconds}") + + def initialize(self): + self._timer.restart() + + def end(self, interrupted: bool): + self._timer.stop() + + def isFinished(self) -> bool: + return self._timer.hasElapsed(self._duration) + + def runsWhenDisabled(self) -> bool: + return True + + def initSendable(self, builder: SendableBuilder) -> None: + super().initSendable(builder) + builder.addDoubleProperty("duration", lambda: self._duration, lambda _: None) diff --git a/commandsv2/src/main/python/commands2/waituntilcommand.py b/commandsv2/src/main/python/commands2/waituntilcommand.py new file mode 100644 index 0000000000..d97526773c --- /dev/null +++ b/commandsv2/src/main/python/commands2/waituntilcommand.py @@ -0,0 +1,80 @@ +# validated: 2024-01-20 DS aaea85ff1656 WaitUntilCommand.java +from __future__ import annotations + +from typing import Callable, overload + +from wpilib import Timer +from wpimath import units + +from .command import Command +from .util import format_args_kwargs + + +class WaitUntilCommand(Command): + """ + A command that does nothing but ends after a specified match time or condition. Useful for + CommandGroups. + """ + + _condition: Callable[[], bool] + + @overload + def __init__(self, condition: Callable[[], bool]): + """ + Creates a new WaitUntilCommand that ends after a given condition becomes true. + + :param condition: the condition to determine when to end + """ + ... + + @overload + def __init__(self, time: units.seconds): + """ + Creates a new WaitUntilCommand that ends after a given match time. + + NOTE: The match timer used for this command is UNOFFICIAL. Using this command does NOT + guarantee that the time at which the action is performed will be judged to be legal by the + referees. When in doubt, add a safety factor or time the action manually. + + :param time: the match time at which to end, in seconds + """ + ... + + def __init__(self, *args, **kwargs): + super().__init__() + + def init_condition(condition: Callable[[], bool]) -> None: + assert callable(condition) + self._condition = condition + + def init_time(time: float) -> None: + self._condition = lambda: Timer.getMatchTime() - time > 0 + + num_args = len(args) + len(kwargs) + + if num_args == 1 and len(kwargs) == 1: + if "condition" in kwargs: + return init_condition(kwargs["condition"]) + elif "time" in kwargs: + return init_time(kwargs["time"]) + elif num_args == 1 and len(args) == 1: + if isinstance(args[0], float): + return init_time(args[0]) + elif callable(args[0]): + return init_condition(args[0]) + + raise TypeError( + f""" +TypeError: WaitUntilCommand(): incompatible function arguments. The following argument types are supported: + 1. (self: WaitUntilCommand, condition: () -> bool) + 2. (self: WaitUntilCommand, time: wpimath.units.seconds) + +Invoked with: {format_args_kwargs(self, *args, **kwargs)} +""" + ) + + def isFinished(self) -> bool: + return self._condition() + + def runsWhenDisabled(self) -> bool: + return True diff --git a/commandsv2/src/main/python/commands2/wrappercommand.py b/commandsv2/src/main/python/commands2/wrappercommand.py new file mode 100644 index 0000000000..45c98b75ee --- /dev/null +++ b/commandsv2/src/main/python/commands2/wrappercommand.py @@ -0,0 +1,90 @@ +# validated: 2024-01-20 DS ad0859a8c9ec WrapperCommand.java +from __future__ import annotations + +from typing import Set + +from .command import Command, InterruptionBehavior +from .commandscheduler import CommandScheduler + + +class WrapperCommand(Command): + """ + A class used internally to wrap commands while overriding a specific method; all other methods + will call through to the wrapped command. + + The rules for command compositions apply: command instances that are passed to it cannot be + added to any other composition or scheduled individually, and the composition requires all + subsystems its components require. + """ + + def __init__(self, command: Command): + """ + Wrap a command. + + :param command: the command being wrapped. Trying to directly schedule this + command or add it to a composition will throw an exception. + """ + super().__init__() + + CommandScheduler.getInstance().registerComposedCommands([command]) + self._command = command + self.setName(self._command.getName()) + + def initialize(self): + """ + The initial subroutine of a command. Called once when the command is initially scheduled. + """ + self._command.initialize() + + def execute(self): + """ + The main body of a command. Called repeatedly while the command is scheduled. + """ + self._command.execute() + + def end(self, interrupted: bool): + """ + The action to take when the command ends. Called when either the command finishes normally, or + when it interrupted/canceled. + + Do not schedule commands here that share requirements with this command. Use + :func:`commands2.Command.andThen` instead. + + :param interrupted: whether the command was interrupted/canceled + """ + self._command.end(interrupted) + + def isFinished(self) -> bool: + """ + Whether the command has finished. Once a command finishes, the scheduler will call its end() + method and un-schedule it. + + :returns: whether the command has finished. + """ + return self._command.isFinished() + + def getRequirements(self) -> Set: + """ + Specifies the set of subsystems used by this command. Two commands cannot use the same + subsystem at the same time. If the command is scheduled as interruptible and another command is + scheduled that shares a requirement, the command will be interrupted. Else, the command will + not be scheduled. If no subsystems are required, return an empty set. + + Note: it is recommended that user implementations contain the requirements as a field, and + return that field here, rather than allocating a new set every time this is called. + + :returns: the set of subsystems that are required + """ + return self._command.getRequirements() + + def runsWhenDisabled(self) -> bool: + """ + Whether the given command should run when the robot is disabled. Override to return true if the + command should run when disabled. + + :returns: whether the command should run when the robot is disabled + """ + return self._command.runsWhenDisabled() + + def getInterruptionBehavior(self) -> InterruptionBehavior: + return self._command.getInterruptionBehavior() diff --git a/commandsv2/src/test/python/compositiontestbase.py b/commandsv2/src/test/python/compositiontestbase.py new file mode 100644 index 0000000000..5739cb8160 --- /dev/null +++ b/commandsv2/src/test/python/compositiontestbase.py @@ -0,0 +1,166 @@ +from typing import Generic, TypeVar + +import commands2 +import pytest + +# T = TypeVar("T", bound=commands2.Command) +# T = commands2.Command + +from util import * + + +class SingleCompositionTestBase: + def composeSingle(self, member: commands2.Command): + raise NotImplementedError + + @pytest.mark.parametrize( + "interruptionBehavior", + [ + commands2.InterruptionBehavior.kCancelSelf, + commands2.InterruptionBehavior.kCancelIncoming, + ], + ) + def test_interruptible(self, interruptionBehavior: commands2.InterruptionBehavior): + command = self.composeSingle( + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + interruptionBehavior + ) + ) + assert command.getInterruptionBehavior() == interruptionBehavior + + @pytest.mark.parametrize("runsWhenDisabled", [True, False]) + def test_runWhenDisabled(self, runsWhenDisabled: bool): + command = self.composeSingle( + commands2.WaitUntilCommand(lambda: False).ignoringDisable(runsWhenDisabled) + ) + assert command.runsWhenDisabled() == runsWhenDisabled + + def test_command_in_other_composition(self): + command = commands2.InstantCommand() + wrapped = commands2.WrapperCommand(command) + with pytest.raises(commands2.IllegalCommandUse): + self.composeSingle(command) + + def test_command_in_multiple_compositions(self): + command = commands2.InstantCommand() + self.composeSingle(command) + with pytest.raises(commands2.IllegalCommandUse): + self.composeSingle(command) + + def test_compose_then_schedule(self, scheduler: commands2.CommandScheduler): + command = commands2.InstantCommand() + self.composeSingle(command) + with pytest.raises(commands2.IllegalCommandUse): + scheduler.schedule(command) + + def test_schedule_then_compose(self, scheduler: commands2.CommandScheduler): + command = commands2.RunCommand(lambda: None) + scheduler.schedule(command) + with pytest.raises(commands2.IllegalCommandUse): + self.composeSingle(command) + + +class MultiCompositionTestBase(SingleCompositionTestBase): + def compose(self, *members: commands2.Command): + raise NotImplementedError + + def composeSingle(self, member: commands2.Command): + return self.compose(member) + + @pytest.mark.parametrize( + "expected,command1,command2,command3", + [ + pytest.param( + commands2.InterruptionBehavior.kCancelSelf, + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelSelf + ), + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelSelf + ), + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelSelf + ), + id="AllCancelSelf", + ), + pytest.param( + commands2.InterruptionBehavior.kCancelIncoming, + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelIncoming + ), + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelIncoming + ), + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelIncoming + ), + id="AllCancelIncoming", + ), + pytest.param( + commands2.InterruptionBehavior.kCancelSelf, + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelSelf + ), + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelSelf + ), + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelIncoming + ), + id="TwoCancelSelfOneIncoming", + ), + pytest.param( + commands2.InterruptionBehavior.kCancelSelf, + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelIncoming + ), + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelIncoming + ), + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelSelf + ), + id="TwoCancelIncomingOneSelf", + ), + ], + ) + def test_interruptible(self, expected, command1, command2, command3): + command = self.compose(command1, command2, command3) + assert command.getInterruptionBehavior() == expected + + @pytest.mark.parametrize( + "expected,command1,command2,command3", + [ + pytest.param( + False, + commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), + commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), + commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), + id="AllFalse", + ), + pytest.param( + True, + commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), + commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), + commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), + id="AllTrue", + ), + pytest.param( + False, + commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), + commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), + commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), + id="TwoTrueOneFalse", + ), + pytest.param( + False, + commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), + commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), + commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), + id="TwoFalseOneTrue", + ), + ], + ) + def test_runWhenDisabled(self, expected, command1, command2, command3): + command = self.compose(command1, command2, command3) + assert command.runsWhenDisabled() == expected diff --git a/commandsv2/src/test/python/conftest.py b/commandsv2/src/test/python/conftest.py new file mode 100644 index 0000000000..96f1826393 --- /dev/null +++ b/commandsv2/src/test/python/conftest.py @@ -0,0 +1,20 @@ +import commands2 +import pytest +from ntcore import NetworkTableInstance +from wpilib.simulation import DriverStationSim + + +@pytest.fixture(autouse=True) +def scheduler(): + commands2.CommandScheduler.resetInstance() + DriverStationSim.setEnabled(True) + DriverStationSim.notifyNewData() + return commands2.CommandScheduler.getInstance() + + +@pytest.fixture() +def nt_instance(): + inst = NetworkTableInstance.create() + inst.startLocal() + yield inst + inst.stopLocal() diff --git a/commandsv2/src/test/python/test_command_decorators.py b/commandsv2/src/test/python/test_command_decorators.py new file mode 100644 index 0000000000..97f9027830 --- /dev/null +++ b/commandsv2/src/test/python/test_command_decorators.py @@ -0,0 +1,222 @@ +from typing import TYPE_CHECKING + +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import commands2 +import pytest + + +def test_timeout(scheduler: commands2.CommandScheduler): + with ManualSimTime() as sim: + command1 = commands2.WaitCommand(1) + timeout = command1.withTimeout(2) + scheduler.schedule(timeout) + scheduler.run() + assert not command1.isScheduled() + assert timeout.isScheduled() + sim.step(3) + scheduler.run() + assert not timeout.isScheduled() + + +def test_until(scheduler: commands2.CommandScheduler): + condition = OOBoolean(False) + command = commands2.WaitCommand(10).until(condition) + scheduler.schedule(command) + scheduler.run() + assert command.isScheduled() + condition.set(True) + scheduler.run() + assert not command.isScheduled() + + +def test_only_while(scheduler: commands2.CommandScheduler): + condition = OOBoolean(True) + command = commands2.WaitCommand(10).onlyWhile(condition) + scheduler.schedule(command) + scheduler.run() + assert command.isScheduled() + condition.set(False) + scheduler.run() + assert not command.isScheduled() + + +def test_ignoringDisable(scheduler: commands2.CommandScheduler): + command = commands2.RunCommand(lambda: None).ignoringDisable(True) + DriverStationSim.setEnabled(False) + DriverStationSim.notifyNewData() + scheduler.schedule(command) + scheduler.run() + assert command.isScheduled() + + +def test_beforeStarting(scheduler: commands2.CommandScheduler): + condition = OOBoolean(False) + condition.set(False) + command = commands2.InstantCommand() + scheduler.schedule( + command.beforeStarting(commands2.InstantCommand(lambda: condition.set(True))) + ) + assert condition == True + + +@pytest.mark.skip +def test_andThenLambda(scheduler: commands2.CommandScheduler): ... + + +def test_andThen(scheduler: commands2.CommandScheduler): + condition = OOBoolean(False) + condition.set(False) + command1 = commands2.InstantCommand() + command2 = commands2.InstantCommand(lambda: condition.set(True)) + scheduler.schedule(command1.andThen(command2)) + assert condition == False + scheduler.run() + assert condition == True + + +def test_deadlineWith(scheduler: commands2.CommandScheduler): + condition = OOBoolean(False) + condition.set(False) + + dictator = commands2.WaitUntilCommand(condition) + endsBefore = commands2.InstantCommand() + endsAfter = commands2.WaitUntilCommand(lambda: False) + + group = dictator.deadlineWith(endsBefore, endsAfter) + + scheduler.schedule(group) + scheduler.run() + assert group.isScheduled() + condition.set(True) + scheduler.run() + assert not group.isScheduled() + + +def test_deadlineFor(scheduler: commands2.CommandScheduler): + condition = OOBoolean(False) + condition.set(False) + + dictator = commands2.WaitUntilCommand(condition) + endsBefore = commands2.InstantCommand() + endsAfter = commands2.WaitUntilCommand(lambda: False) + + group = dictator.deadlineFor(endsBefore, endsAfter) + + scheduler.schedule(group) + scheduler.run() + assert group.isScheduled() + condition.set(True) + scheduler.run() + assert not group.isScheduled() + + +def test_alongWith(scheduler: commands2.CommandScheduler): + condition = OOBoolean() + condition.set(False) + + command1 = commands2.WaitUntilCommand(condition) + command2 = commands2.InstantCommand() + + group = command1.alongWith(command2) + + scheduler.schedule(group) + scheduler.run() + assert group.isScheduled() + condition.set(True) + scheduler.run() + assert not group.isScheduled() + + +def test_raceWith(scheduler: commands2.CommandScheduler): + command1 = commands2.WaitUntilCommand(lambda: False) + command2 = commands2.InstantCommand() + + group = command1.raceWith(command2) + + scheduler.schedule(group) + scheduler.run() + assert not group.isScheduled() + + +def test_unless(scheduler: commands2.CommandScheduler): + unlessCondition = OOBoolean(True) + hasRunCondition = OOBoolean(False) + + command = commands2.InstantCommand(lambda: hasRunCondition.set(True)).unless( + unlessCondition + ) + + scheduler.schedule(command) + scheduler.run() + assert hasRunCondition == False + unlessCondition.set(False) + scheduler.schedule(command) + scheduler.run() + assert hasRunCondition == True + + +def test_onlyIf(scheduler: commands2.CommandScheduler): + onlyIfCondition = OOBoolean(False) + hasRunCondition = OOBoolean(False) + + command = commands2.InstantCommand(lambda: hasRunCondition.set(True)).onlyIf( + onlyIfCondition + ) + + scheduler.schedule(command) + scheduler.run() + assert hasRunCondition == False + onlyIfCondition.set(True) + scheduler.schedule(command) + scheduler.run() + assert hasRunCondition == True + + +def test_finallyDo(scheduler: commands2.CommandScheduler): + first = OOInteger(0) + second = OOInteger(0) + + command = commands2.FunctionalCommand( + lambda: None, + lambda: None, + lambda interrupted: first.incrementAndGet() if not interrupted else None, + lambda: True, + ).finallyDo(lambda interrupted: second.addAndGet(1 + first())) + + scheduler.schedule(command) + assert first == 0 + assert second == 0 + scheduler.run() + assert first == 1 + assert second == 2 + + +def test_handleInterrupt(scheduler: commands2.CommandScheduler): + first = OOInteger(0) + second = OOInteger(0) + + command = commands2.FunctionalCommand( + lambda: None, + lambda: None, + lambda interrupted: first.incrementAndGet() if interrupted else None, + lambda: False, + ).handleInterrupt(lambda: second.addAndGet(1 + first())) + + scheduler.schedule(command) + scheduler.run() + assert first == 0 + assert second == 0 + scheduler.cancel(command) + assert first == 1 + assert second == 2 + + +def test_withName(scheduler: commands2.CommandScheduler): + command = commands2.InstantCommand() + name = "Named" + named = command.withName(name) + assert named.getName() == name diff --git a/commandsv2/src/test/python/test_command_requirements.py b/commandsv2/src/test/python/test_command_requirements.py new file mode 100644 index 0000000000..c25f2d20ec --- /dev/null +++ b/commandsv2/src/test/python/test_command_requirements.py @@ -0,0 +1,58 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_requirementInterrupt(scheduler: commands2.CommandScheduler): + requirement = commands2.Subsystem() + interrupted = commands2.Command() + interrupted.addRequirements(requirement) + interrupter = commands2.Command() + interrupter.addRequirements(requirement) + start_spying_on(interrupted) + start_spying_on(interrupter) + + scheduler.schedule(interrupted) + scheduler.run() + scheduler.schedule(interrupter) + scheduler.run() + + verify(interrupted).initialize() + verify(interrupted).execute() + verify(interrupted).end(True) + + verify(interrupter).initialize() + verify(interrupter).execute() + + assert not interrupted.isScheduled() + assert interrupter.isScheduled() + + +def test_requirementUninterruptible(scheduler: commands2.CommandScheduler): + requirement = commands2.Subsystem() + notInterrupted = commands2.RunCommand( + lambda: None, requirement + ).withInterruptBehavior(commands2.InterruptionBehavior.kCancelIncoming) + interrupter = commands2.Command() + interrupter.addRequirements(requirement) + start_spying_on(notInterrupted) + + scheduler.schedule(notInterrupted) + scheduler.schedule(interrupter) + + assert scheduler.isScheduled(notInterrupted) + assert not scheduler.isScheduled(interrupter) + + +def test_defaultCommandRequirementError(scheduler: commands2.CommandScheduler): + system = commands2.Subsystem() + missingRequirement = commands2.WaitUntilCommand(lambda: False) + + with pytest.raises(commands2.IllegalCommandUse): + scheduler.setDefaultCommand(system, missingRequirement) diff --git a/commandsv2/src/test/python/test_command_schedule.py b/commandsv2/src/test/python/test_command_schedule.py new file mode 100644 index 0000000000..62d218231f --- /dev/null +++ b/commandsv2/src/test/python/test_command_schedule.py @@ -0,0 +1,90 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_instantSchedule(scheduler: commands2.CommandScheduler): + command = commands2.Command() + command.isFinished = lambda: True + start_spying_on(command) + + scheduler.schedule(command) + assert scheduler.isScheduled(command) + verify(command).initialize() + + scheduler.run() + + verify(command).execute() + verify(command).end(False) + assert not scheduler.isScheduled(command) + + +def test_singleIterationSchedule(scheduler: commands2.CommandScheduler): + command = commands2.Command() + start_spying_on(command) + + scheduler.schedule(command) + assert scheduler.isScheduled(command) + + scheduler.run() + command.isFinished = lambda: True + scheduler.run() + + verify(command).initialize() + verify(command, times(2)).execute() + verify(command).end(False) + assert not scheduler.isScheduled(command) + + +def test_multiSchedule(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + command3 = commands2.Command() + + scheduler.schedule(command1, command2, command3) + assert scheduler.isScheduled(command1, command2, command3) + scheduler.run() + assert scheduler.isScheduled(command1, command2, command3) + + command1.isFinished = lambda: True + scheduler.run() + assert scheduler.isScheduled(command2, command3) + assert not scheduler.isScheduled(command1) + + command2.isFinished = lambda: True + scheduler.run() + assert scheduler.isScheduled(command3) + assert not scheduler.isScheduled(command1, command2) + + command3.isFinished = lambda: True + scheduler.run() + assert not scheduler.isScheduled(command1, command2, command3) + + +def test_schedulerCancel(scheduler: commands2.CommandScheduler): + command = commands2.Command() + start_spying_on(command) + + scheduler.schedule(command) + + scheduler.run() + scheduler.cancel(command) + scheduler.run() + + verify(command).execute() + verify(command).end(True) + verify(command, never()).end(False) + + assert not scheduler.isScheduled(command) + + +def test_notScheduledCancel(scheduler: commands2.CommandScheduler): + command = commands2.Command() + + scheduler.cancel(command) diff --git a/commandsv2/src/test/python/test_commandgroup_error.py b/commandsv2/src/test/python/test_commandgroup_error.py new file mode 100644 index 0000000000..ed4beac8cb --- /dev/null +++ b/commandsv2/src/test/python/test_commandgroup_error.py @@ -0,0 +1,37 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_commandInMultipleGroups(): + command1 = commands2.Command() + command2 = commands2.Command() + + commands2.ParallelCommandGroup(command1, command2) + with pytest.raises(commands2.IllegalCommandUse): + commands2.ParallelCommandGroup(command1, command2) + + +def test_commandInGroupExternallyScheduled(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + + commands2.ParallelCommandGroup(command1, command2) + + with pytest.raises(commands2.IllegalCommandUse): + scheduler.schedule(command1) + + +def test_redecoratedCommandError(scheduler: commands2.CommandScheduler): + command = commands2.InstantCommand() + command.withTimeout(10).until(lambda: False) + with pytest.raises(commands2.IllegalCommandUse): + command.withTimeout(10) + scheduler.removeComposedCommand(command) + command.withTimeout(10) diff --git a/commandsv2/src/test/python/test_conditional_command.py b/commandsv2/src/test/python/test_conditional_command.py new file mode 100644 index 0000000000..caeff2daa4 --- /dev/null +++ b/commandsv2/src/test/python/test_conditional_command.py @@ -0,0 +1,55 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_conditionalCommand(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command1.isFinished = lambda: True + command2 = commands2.Command() + + start_spying_on(command1) + start_spying_on(command2) + + conditionalCommand = commands2.ConditionalCommand(command1, command2, lambda: True) + + scheduler.schedule(conditionalCommand) + scheduler.run() + + verify(command1).initialize() + verify(command1).execute() + verify(command1).end(False) + + verify(command2, never()).initialize() + verify(command2, never()).execute() + verify(command2, never()).end(False) + + +def test_conditionalCommandRequirement(scheduler: commands2.CommandScheduler): + system1 = commands2.Subsystem() + system2 = commands2.Subsystem() + system3 = commands2.Subsystem() + + command1 = commands2.Command() + command1.addRequirements(system1, system2) + command2 = commands2.Command() + command2.addRequirements(system3) + + start_spying_on(command1) + start_spying_on(command2) + + conditionalCommand = commands2.ConditionalCommand(command1, command2, lambda: True) + + scheduler.schedule(conditionalCommand) + scheduler.schedule(commands2.InstantCommand(lambda: None, system3)) + + assert not scheduler.isScheduled(conditionalCommand) + + assert command1.end.called_with(True) + assert not command2.end.called_with(True) diff --git a/commandsv2/src/test/python/test_default_command.py b/commandsv2/src/test/python/test_default_command.py new file mode 100644 index 0000000000..211ecae492 --- /dev/null +++ b/commandsv2/src/test/python/test_default_command.py @@ -0,0 +1,73 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_defaultCommandSchedule(scheduler: commands2.CommandScheduler): + hasDefaultCommand = commands2.Subsystem() + + defaultCommand = commands2.Command() + defaultCommand.addRequirements(hasDefaultCommand) + + scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand) + scheduler.run() + + assert scheduler.isScheduled(defaultCommand) + + +def test_defaultCommandInterruptResume(scheduler: commands2.CommandScheduler): + hasDefaultCommand = commands2.Subsystem() + + defaultCommand = commands2.Command() + defaultCommand.addRequirements(hasDefaultCommand) + + interrupter = commands2.Command() + interrupter.addRequirements(hasDefaultCommand) + + scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand) + scheduler.run() + scheduler.schedule(interrupter) + + assert not scheduler.isScheduled(defaultCommand) + assert scheduler.isScheduled(interrupter) + + scheduler.cancel(interrupter) + scheduler.run() + + assert scheduler.isScheduled(defaultCommand) + assert not scheduler.isScheduled(interrupter) + + +def test_defaultCommandDisableResume(scheduler: commands2.CommandScheduler): + hasDefaultCommand = commands2.Subsystem() + + defaultCommand = commands2.Command() + defaultCommand.addRequirements(hasDefaultCommand) + defaultCommand.runsWhenDisabled = lambda: False + + start_spying_on(defaultCommand) + + scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand) + scheduler.run() + + assert scheduler.isScheduled(defaultCommand) + + DriverStationSim.setEnabled(False) + DriverStationSim.notifyNewData() + scheduler.run() + + assert not scheduler.isScheduled(defaultCommand) + + DriverStationSim.setEnabled(True) + DriverStationSim.notifyNewData() + scheduler.run() + + assert scheduler.isScheduled(defaultCommand) + + assert defaultCommand.end.called_with(True) diff --git a/commandsv2/src/test/python/test_deferred_command.py b/commandsv2/src/test/python/test_deferred_command.py new file mode 100644 index 0000000000..ed7357eff2 --- /dev/null +++ b/commandsv2/src/test/python/test_deferred_command.py @@ -0,0 +1,63 @@ +import commands2 + +from util import * # type: ignore + + +def test_deferred_functions(scheduler: commands2.CommandScheduler): + inner_command = commands2.Command() + command = commands2.DeferredCommand(lambda: inner_command) + + start_spying_on(inner_command) + start_spying_on(command) + + command.initialize() + verify(inner_command).initialize() + + command.execute() + verify(inner_command).execute() + + assert not command.isFinished() + verify(inner_command).isFinished() + + inner_command.isFinished = lambda: True + assert command.isFinished() + verify(inner_command, times=times(2)).isFinished() + + command.end(False) + verify(inner_command).end(False) + + +def test_deferred_supplier_only_called_during_init( + scheduler: commands2.CommandScheduler, +): + supplier_called = 0 + + def supplier() -> commands2.Command: + nonlocal supplier_called + supplier_called += 1 + return commands2.Command() + + command = commands2.DeferredCommand(supplier) + assert supplier_called == 0 + + scheduler.schedule(command) + assert supplier_called == 1 + scheduler.run() + + scheduler.schedule(command) + assert supplier_called == 1 + + +def test_deferred_requirements(scheduler: commands2.CommandScheduler): + subsystem = commands2.Subsystem() + command = commands2.DeferredCommand(lambda: commands2.Command(), subsystem) + + assert subsystem in command.getRequirements() + + +def test_deferred_null_command(scheduler: commands2.CommandScheduler): + command = commands2.DeferredCommand(lambda: None) # type: ignore + command.initialize() + command.execute() + command.isFinished() + command.end(False) diff --git a/commandsv2/src/test/python/test_functional_command.py b/commandsv2/src/test/python/test_functional_command.py new file mode 100644 index 0000000000..c6481cab07 --- /dev/null +++ b/commandsv2/src/test/python/test_functional_command.py @@ -0,0 +1,36 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_functionalCommandSchedule(scheduler: commands2.CommandScheduler): + cond1 = OOBoolean() + cond2 = OOBoolean() + cond3 = OOBoolean() + cond4 = OOBoolean() + + command = commands2.FunctionalCommand( + lambda: cond1.set(True), + lambda: cond2.set(True), + lambda _: cond3.set(True), + lambda: cond4.get(), + ) + + scheduler.schedule(command) + scheduler.run() + + assert scheduler.isScheduled(command) + + cond4.set(True) + scheduler.run() + + assert not scheduler.isScheduled(command) + assert cond1 + assert cond2 + assert cond3 diff --git a/commandsv2/src/test/python/test_instant_command.py b/commandsv2/src/test/python/test_instant_command.py new file mode 100644 index 0000000000..96d3089ee7 --- /dev/null +++ b/commandsv2/src/test/python/test_instant_command.py @@ -0,0 +1,21 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_instantCommandSchedule(scheduler: commands2.CommandScheduler): + cond = OOBoolean() + + command = commands2.InstantCommand(lambda: cond.set(True)) + + scheduler.schedule(command) + scheduler.run() + + assert cond + assert not scheduler.isScheduled(command) diff --git a/commandsv2/src/test/python/test_networkbutton.py b/commandsv2/src/test/python/test_networkbutton.py new file mode 100644 index 0000000000..d49f687d78 --- /dev/null +++ b/commandsv2/src/test/python/test_networkbutton.py @@ -0,0 +1,29 @@ +from typing import TYPE_CHECKING + +import commands2 +from ntcore import NetworkTableInstance +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + + +def test_networkbutton( + scheduler: commands2.CommandScheduler, nt_instance: NetworkTableInstance +): + # command = commands2.Command() + command = commands2.Command() + start_spying_on(command) + + pub = nt_instance.getTable("TestTable").getBooleanTopic("Test").publish() + + button = commands2.button.NetworkButton(nt_instance, "TestTable", "Test") + + pub.set(False) + button.onTrue(command) + scheduler.run() + assert command.schedule.times_called == 0 + pub.set(True) + scheduler.run() + scheduler.run() + verify(command).schedule() diff --git a/commandsv2/src/test/python/test_notifier_command.py b/commandsv2/src/test/python/test_notifier_command.py new file mode 100644 index 0000000000..5b6d56701d --- /dev/null +++ b/commandsv2/src/test/python/test_notifier_command.py @@ -0,0 +1,22 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_notifierCommandScheduler(scheduler: commands2.CommandScheduler): + with ManualSimTime() as sim: + counter = OOInteger(0) + command = commands2.NotifierCommand(counter.incrementAndGet, 0.01) + + scheduler.schedule(command) + for i in range(5): + sim.step(0.005) + scheduler.cancel(command) + + assert counter == 2 diff --git a/commandsv2/src/test/python/test_parallelcommandgroup.py b/commandsv2/src/test/python/test_parallelcommandgroup.py new file mode 100644 index 0000000000..04355af749 --- /dev/null +++ b/commandsv2/src/test/python/test_parallelcommandgroup.py @@ -0,0 +1,117 @@ +from typing import TYPE_CHECKING + +import commands2 +from compositiontestbase import MultiCompositionTestBase # type: ignore +from util import * # type: ignore + +# from tests.compositiontestbase import T + +if TYPE_CHECKING: + from .util import * + from .compositiontestbase import MultiCompositionTestBase + +import pytest + + +class TestParallelCommandGroupComposition(MultiCompositionTestBase): + def compose(self, *members: commands2.Command): + return commands2.ParallelCommandGroup(*members) + + +def test_parallelGroupSchedule(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + + start_spying_on(command1) + start_spying_on(command2) + + group = commands2.ParallelCommandGroup(command1, command2) + + scheduler.schedule(group) + + verify(command1).initialize() + verify(command2).initialize() + + command1.isFinished = lambda: True + scheduler.run() + command2.isFinished = lambda: True + scheduler.run() + + verify(command1).execute() + verify(command1).end(False) + verify(command2, times(2)).execute() + verify(command2).end(False) + + assert not scheduler.isScheduled(group) + + +def test_parallelGroupInterrupt(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + + start_spying_on(command1) + start_spying_on(command2) + + group = commands2.ParallelCommandGroup(command1, command2) + + scheduler.schedule(group) + + command1.isFinished = lambda: True + scheduler.run() + scheduler.run() + scheduler.cancel(group) + + verify(command1).execute() + verify(command1).end(False) + verify(command1, never()).end(True) + + verify(command2, times(2)).execute() + verify(command2, never()).end(False) + verify(command2).end(True) + + assert not scheduler.isScheduled(group) + + +def test_notScheduledCancel(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + + group = commands2.ParallelCommandGroup(command1, command2) + + scheduler.cancel(group) + + +def test_parallelGroupRequirement(scheduler: commands2.CommandScheduler): + system1 = commands2.Subsystem() + system2 = commands2.Subsystem() + system3 = commands2.Subsystem() + system4 = commands2.Subsystem() + + command1 = commands2.Command() + command1.addRequirements(system1, system2) + command2 = commands2.Command() + command2.addRequirements(system3) + command3 = commands2.Command() + command3.addRequirements(system3, system4) + + group = commands2.ParallelCommandGroup(command1, command2) + + scheduler.schedule(group) + scheduler.schedule(command3) + + assert not scheduler.isScheduled(group) + assert scheduler.isScheduled(command3) + + +def test_parallelGroupRequirementError(): + system1 = commands2.Subsystem() + system2 = commands2.Subsystem() + system3 = commands2.Subsystem() + + command1 = commands2.Command() + command1.addRequirements(system1, system2) + command2 = commands2.Command() + command2.addRequirements(system2, system3) + + with pytest.raises(commands2.IllegalCommandUse): + commands2.ParallelCommandGroup(command1, command2) diff --git a/commandsv2/src/test/python/test_paralleldeadlinegroup.py b/commandsv2/src/test/python/test_paralleldeadlinegroup.py new file mode 100644 index 0000000000..9136509b84 --- /dev/null +++ b/commandsv2/src/test/python/test_paralleldeadlinegroup.py @@ -0,0 +1,119 @@ +from typing import TYPE_CHECKING + +import commands2 +from compositiontestbase import MultiCompositionTestBase # type: ignore +from util import * # type: ignore + +# from tests.compositiontestbase import T + +if TYPE_CHECKING: + from .util import * + from .compositiontestbase import MultiCompositionTestBase + +import pytest + + +class TestParallelDeadlineGroupComposition(MultiCompositionTestBase): + def compose(self, *members: commands2.Command): + return commands2.ParallelDeadlineGroup(members[0], *members[1:]) + + +def test_parallelDeadlineSchedule(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + command2.isFinished = lambda: True + command3 = commands2.Command() + + start_spying_on(command1) + start_spying_on(command2) + start_spying_on(command3) + + group = commands2.ParallelDeadlineGroup(command1, command2, command3) + + scheduler.schedule(group) + scheduler.run() + + assert scheduler.isScheduled(group) + + command1.isFinished = lambda: True + scheduler.run() + + assert not scheduler.isScheduled(group) + + verify(command2).initialize() + verify(command2).execute() + verify(command2).end(False) + verify(command2, never()).end(True) + + verify(command1).initialize() + verify(command1, times(2)).execute() + verify(command1).end(False) + verify(command1, never()).end(True) + + verify(command3).initialize() + verify(command3, times(2)).execute() + verify(command3, never()).end(False) + verify(command3).end(True) + + +def test_parallelDeadlineInterrupt(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + command2.isFinished = lambda: True + + start_spying_on(command1) + start_spying_on(command2) + + group = commands2.ParallelDeadlineGroup(command1, command2) + + scheduler.schedule(group) + + scheduler.run() + scheduler.run() + scheduler.cancel(group) + + verify(command1, times(2)).execute() + verify(command1, never()).end(False) + verify(command1).end(True) + + verify(command2).execute() + verify(command2).end(False) + verify(command2, never()).end(True) + + assert not scheduler.isScheduled(group) + + +def test_parallelDeadlineRequirement(scheduler: commands2.CommandScheduler): + system1 = commands2.Subsystem() + system2 = commands2.Subsystem() + system3 = commands2.Subsystem() + system4 = commands2.Subsystem() + + command1 = commands2.Command() + command1.addRequirements(system1, system2) + command2 = commands2.Command() + command2.addRequirements(system3) + command3 = commands2.Command() + command3.addRequirements(system3, system4) + + group = commands2.ParallelDeadlineGroup(command1, command2) + + scheduler.schedule(group) + scheduler.schedule(command3) + + assert not scheduler.isScheduled(group) + assert scheduler.isScheduled(command3) + + +def test_parallelDeadlineRequirementError(scheduler: commands2.CommandScheduler): + system1 = commands2.Subsystem() + system2 = commands2.Subsystem() + system3 = commands2.Subsystem() + + command1 = commands2.Command() + command1.addRequirements(system1, system2) + command2 = commands2.Command() + command2.addRequirements(system2, system3) + + with pytest.raises(commands2.IllegalCommandUse): + commands2.ParallelDeadlineGroup(command1, command2) diff --git a/commandsv2/src/test/python/test_parallelracegroup.py b/commandsv2/src/test/python/test_parallelracegroup.py new file mode 100644 index 0000000000..58e2ace408 --- /dev/null +++ b/commandsv2/src/test/python/test_parallelracegroup.py @@ -0,0 +1,183 @@ +from typing import TYPE_CHECKING + +import commands2 +from compositiontestbase import MultiCompositionTestBase # type: ignore +from util import * # type: ignore + +# from tests.compositiontestbase import T + +if TYPE_CHECKING: + from .util import * + from .compositiontestbase import MultiCompositionTestBase + +import pytest + + +class TestParallelRaceGroupComposition(MultiCompositionTestBase): + def compose(self, *members: commands2.Command): + return commands2.ParallelRaceGroup(*members) + + +def test_parallelRaceSchedule(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + + start_spying_on(command1) + start_spying_on(command2) + + group = commands2.ParallelRaceGroup(command1, command2) + + scheduler.schedule(group) + + verify(command1).initialize() + verify(command2).initialize() + + command1.isFinished = lambda: True + scheduler.run() + command2.isFinished = lambda: True + scheduler.run() + + verify(command1).execute() + verify(command1).end(False) + verify(command2).execute() + verify(command2).end(True) + verify(command2, never()).end(False) + + assert not scheduler.isScheduled(group) + + +def test_parallelRaceInterrupt(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + + start_spying_on(command1) + start_spying_on(command2) + + group = commands2.ParallelRaceGroup(command1, command2) + + scheduler.schedule(group) + + scheduler.run() + scheduler.run() + scheduler.cancel(group) + + verify(command1, times(2)).execute() + verify(command1, never()).end(False) + verify(command1).end(True) + + verify(command2, times(2)).execute() + verify(command2, never()).end(False) + verify(command2).end(True) + + assert not scheduler.isScheduled(group) + + +def test_notScheduledCancel(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + + group = commands2.ParallelRaceGroup(command1, command2) + + scheduler.cancel(group) + + +def test_parallelRaceRequirement(scheduler: commands2.CommandScheduler): + system1 = commands2.Subsystem() + system2 = commands2.Subsystem() + system3 = commands2.Subsystem() + system4 = commands2.Subsystem() + + command1 = commands2.Command() + command1.addRequirements(system1, system2) + command2 = commands2.Command() + command2.addRequirements(system3) + command3 = commands2.Command() + command3.addRequirements(system3, system4) + + group = commands2.ParallelRaceGroup(command1, command2) + + scheduler.schedule(group) + scheduler.schedule(command3) + + assert not scheduler.isScheduled(group) + assert scheduler.isScheduled(command3) + + +def test_parallelRaceRequirementError(): + system1 = commands2.Subsystem() + system2 = commands2.Subsystem() + system3 = commands2.Subsystem() + + command1 = commands2.Command() + command1.addRequirements(system1, system2) + command2 = commands2.Command() + command2.addRequirements(system2, system3) + + with pytest.raises(commands2.IllegalCommandUse): + commands2.ParallelRaceGroup(command1, command2) + + +def test_parallelRaceOnlyCallsEndOnce(scheduler: commands2.CommandScheduler): + system1 = commands2.Subsystem() + system2 = commands2.Subsystem() + + command1 = commands2.Command() + command1.addRequirements(system1) + command2 = commands2.Command() + command2.addRequirements(system2) + command3 = commands2.Command() + + group1 = commands2.SequentialCommandGroup(command1, command2) + group2 = commands2.ParallelRaceGroup(group1, command3) + + scheduler.schedule(group2) + scheduler.run() + command1.isFinished = lambda: True + scheduler.run() + command2.isFinished = lambda: True + scheduler.run() + assert not scheduler.isScheduled(group2) + + +def test_parallelRaceScheduleTwiceTest(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + + start_spying_on(command1) + start_spying_on(command2) + + group = commands2.ParallelRaceGroup(command1, command2) + + scheduler.schedule(group) + + verify(command1).initialize() + verify(command2).initialize() + + command1.isFinished = lambda: True + scheduler.run() + command2.isFinished = lambda: True + scheduler.run() + + verify(command1).execute() + verify(command1).end(False) + verify(command2).execute() + verify(command2).end(True) + verify(command2, never()).end(False) + + assert not scheduler.isScheduled(group) + + reset(command1) + reset(command2) + + scheduler.schedule(group) + + verify(command1).initialize() + verify(command2).initialize() + + scheduler.run() + scheduler.run() + assert scheduler.isScheduled(group) + command2.isFinished = lambda: True + scheduler.run() + + assert not scheduler.isScheduled(group) diff --git a/commandsv2/src/test/python/test_pidcommand.py b/commandsv2/src/test/python/test_pidcommand.py new file mode 100644 index 0000000000..5ba51650b3 --- /dev/null +++ b/commandsv2/src/test/python/test_pidcommand.py @@ -0,0 +1,114 @@ +from typing import TYPE_CHECKING + +from util import * # type: ignore +import wpimath.controller as controller +import commands2 + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_pidCommandSupplier(scheduler: commands2.CommandScheduler): + with ManualSimTime() as sim: + output_float = OOFloat(0.0) + measurement_source = OOFloat(5.0) + setpoint_source = OOFloat(2.0) + pid_controller = controller.PIDController(0.1, 0.01, 0.001) + system = commands2.Subsystem() + pidCommand = commands2.PIDCommand( + pid_controller, + measurement_source, + setpoint_source, + output_float.set, + system, + ) + start_spying_on(pidCommand) + scheduler.schedule(pidCommand) + scheduler.run() + sim.step(1) + scheduler.run() + + assert scheduler.isScheduled(pidCommand) + + assert not pidCommand._controller.atSetpoint() + + # Tell the pid command we're at our setpoint through the controller + measurement_source.set(setpoint_source()) + + sim.step(2) + + scheduler.run() + + # Should be measuring error of 0 now + assert pidCommand._controller.atSetpoint() + + +def test_pidCommandScalar(scheduler: commands2.CommandScheduler): + with ManualSimTime() as sim: + output_float = OOFloat(0.0) + measurement_source = OOFloat(5.0) + setpoint_source = 2.0 + pid_controller = controller.PIDController(0.1, 0.01, 0.001) + system = commands2.Subsystem() + pidCommand = commands2.PIDCommand( + pid_controller, + measurement_source, + setpoint_source, + output_float.set, + system, + ) + start_spying_on(pidCommand) + scheduler.schedule(pidCommand) + scheduler.run() + sim.step(1) + scheduler.run() + + assert scheduler.isScheduled(pidCommand) + + assert not pidCommand._controller.atSetpoint() + + # Tell the pid command we're at our setpoint through the controller + measurement_source.set(setpoint_source) + + sim.step(2) + + scheduler.run() + + # Should be measuring error of 0 now + assert pidCommand._controller.atSetpoint() + + +def test_withTimeout(scheduler: commands2.CommandScheduler): + with ManualSimTime() as sim: + output_float = OOFloat(0.0) + measurement_source = OOFloat(5.0) + setpoint_source = OOFloat(2.0) + pid_controller = controller.PIDController(0.1, 0.01, 0.001) + system = commands2.Subsystem() + command1 = commands2.PIDCommand( + pid_controller, + measurement_source, + setpoint_source, + output_float.set, + system, + ) + start_spying_on(command1) + + timeout = command1.withTimeout(2) + + scheduler.schedule(timeout) + scheduler.run() + + verify(command1).initialize() + verify(command1).execute() + assert not scheduler.isScheduled(command1) + assert scheduler.isScheduled(timeout) + + sim.step(3) + scheduler.run() + + verify(command1).end(True) + verify(command1, never()).end(False) + assert not scheduler.isScheduled(timeout) diff --git a/commandsv2/src/test/python/test_printcommand.py b/commandsv2/src/test/python/test_printcommand.py new file mode 100644 index 0000000000..6d28cad67c --- /dev/null +++ b/commandsv2/src/test/python/test_printcommand.py @@ -0,0 +1,17 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_printCommandSchedule(capsys, scheduler: commands2.CommandScheduler): + command = commands2.PrintCommand("Test!") + scheduler.schedule(command) + scheduler.run() + assert not scheduler.isScheduled(command) + assert capsys.readouterr().out == "Test!\n" diff --git a/commandsv2/src/test/python/test_profiledpidsubsystem.py b/commandsv2/src/test/python/test_profiledpidsubsystem.py new file mode 100644 index 0000000000..8b896f3c1d --- /dev/null +++ b/commandsv2/src/test/python/test_profiledpidsubsystem.py @@ -0,0 +1,119 @@ +from types import MethodType +from typing import Any + +import pytest +from wpimath.controller import ProfiledPIDController, ProfiledPIDControllerRadians +from wpimath.trajectory import TrapezoidProfile, TrapezoidProfileRadians + +from commands2 import ProfiledPIDSubsystem + +MAX_VELOCITY = 30 # Radians per second +MAX_ACCELERATION = 500 # Radians per sec squared +PID_KP = 50 + + +class EvalSubsystem(ProfiledPIDSubsystem): + def __init__(self, controller, state_factory): + self._state_factory = state_factory + super().__init__(controller, 0) + + +def simple_use_output(self, output: float, setpoint: Any): + """A simple useOutput method that saves the current state of the controller.""" + self._output = output + self._setpoint = setpoint + + +def simple_get_measurement(self) -> float: + """A simple getMeasurement method that returns zero (frozen or stuck plant).""" + return 0.0 + + +controller_types = [ + ( + ProfiledPIDControllerRadians, + TrapezoidProfileRadians.Constraints, + TrapezoidProfileRadians.State, + ), + (ProfiledPIDController, TrapezoidProfile.Constraints, TrapezoidProfile.State), +] +controller_ids = ["radians", "dimensionless"] + + +@pytest.fixture(params=controller_types, ids=controller_ids) +def subsystem(request): + """ + Fixture that returns an EvalSubsystem object for each type of controller. + """ + controller, profile_factory, state_factory = request.param + profile = profile_factory(MAX_VELOCITY, MAX_ACCELERATION) + pid = controller(PID_KP, 0, 0, profile) + return EvalSubsystem(pid, state_factory) + + +def test_profiled_pid_subsystem_init(subsystem): + """ + Verify that the ProfiledPIDSubsystem can be initialized using + all supported profiled PID controller / trapezoid profile types. + """ + assert isinstance(subsystem, EvalSubsystem) + + +def test_profiled_pid_subsystem_not_implemented_get_measurement(subsystem): + """ + Verify that the ProfiledPIDSubsystem.getMeasurement method + raises NotImplementedError. + """ + with pytest.raises(NotImplementedError): + subsystem.getMeasurement() + + +def test_profiled_pid_subsystem_not_implemented_use_output(subsystem): + """ + Verify that the ProfiledPIDSubsystem.useOutput method raises + NotImplementedError. + """ + with pytest.raises(NotImplementedError): + subsystem.useOutput(0, subsystem._state_factory()) + + +@pytest.mark.parametrize("use_float", [True, False]) +def test_profiled_pid_subsystem_set_goal(subsystem, use_float): + """ + Verify that the ProfiledPIDSubsystem.setGoal method sets the goal. + """ + if use_float: + subsystem.setGoal(1.0) + assert subsystem.getController().getGoal().position == 1.0 + assert subsystem.getController().getGoal().velocity == 0.0 + else: + subsystem.setGoal(subsystem._state_factory(1.0, 2.0)) + assert subsystem.getController().getGoal().position == 1.0 + assert subsystem.getController().getGoal().velocity == 2.0 + + +def test_profiled_pid_subsystem_enable_subsystem(subsystem): + """ + Verify the subsystem can be enabled. + """ + # Dynamically add useOutput and getMeasurement methods so the + # system can be enabled + setattr(subsystem, "useOutput", MethodType(simple_use_output, subsystem)) + setattr(subsystem, "getMeasurement", MethodType(simple_get_measurement, subsystem)) + # Enable the subsystem + subsystem.enable() + assert subsystem.isEnabled() + + +def test_profiled_pid_subsystem_disable_subsystem(subsystem): + """ + Verify the subsystem can be disabled. + """ + # Dynamically add useOutput and getMeasurement methods so the + # system can be enabled + setattr(subsystem, "useOutput", MethodType(simple_use_output, subsystem)) + setattr(subsystem, "getMeasurement", MethodType(simple_get_measurement, subsystem)) + # Enable and then disable the subsystem + subsystem.enable() + subsystem.disable() + assert not subsystem.isEnabled() diff --git a/commandsv2/src/test/python/test_proxycommand.py b/commandsv2/src/test/python/test_proxycommand.py new file mode 100644 index 0000000000..496ffe9a5d --- /dev/null +++ b/commandsv2/src/test/python/test_proxycommand.py @@ -0,0 +1,38 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_proxyCommandSchedule(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + start_spying_on(command1) + + scheduleCommand = commands2.ProxyCommand(command1) + + scheduler.schedule(scheduleCommand) + + verify(command1).schedule() + + +def test_proxyCommandEnd(scheduler: commands2.CommandScheduler): + cond = OOBoolean() + + command = commands2.WaitUntilCommand(cond) + + scheduleCommand = commands2.ProxyCommand(command) + + scheduler.schedule(scheduleCommand) + scheduler.run() + + assert scheduler.isScheduled(scheduleCommand) + + cond.set(True) + scheduler.run() + scheduler.run() + assert not scheduler.isScheduled(scheduleCommand) diff --git a/commandsv2/src/test/python/test_repeatcommand.py b/commandsv2/src/test/python/test_repeatcommand.py new file mode 100644 index 0000000000..e2ff3be927 --- /dev/null +++ b/commandsv2/src/test/python/test_repeatcommand.py @@ -0,0 +1,69 @@ +from typing import TYPE_CHECKING + +import commands2 +from compositiontestbase import SingleCompositionTestBase # type: ignore +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + from .compositiontestbase import SingleCompositionTestBase + +import pytest + + +class RepeatCommandCompositionTest(SingleCompositionTestBase): + def composeSingle(self, member): + return member.repeatedly() + + +def test_callsMethodsCorrectly(scheduler: commands2.CommandScheduler): + command = commands2.Command() + repeated = command.repeatedly() + + start_spying_on(command) + + assert command.initialize.times_called == 0 + assert command.execute.times_called == 0 + assert command.isFinished.times_called == 0 + assert command.end.times_called == 0 + + scheduler.schedule(repeated) + assert command.initialize.times_called == 1 + assert command.execute.times_called == 0 + assert command.isFinished.times_called == 0 + assert command.end.times_called == 0 + + command.isFinished = lambda: False + + scheduler.run() + assert command.initialize.times_called == 1 + assert command.execute.times_called == 1 + assert command.isFinished.times_called == 1 + assert command.end.times_called == 0 + + command.isFinished = lambda: True + scheduler.run() + assert command.initialize.times_called == 1 + assert command.execute.times_called == 2 + assert command.isFinished.times_called == 2 + assert command.end.times_called == 1 + + command.isFinished = lambda: False + scheduler.run() + assert command.initialize.times_called == 2 + assert command.execute.times_called == 3 + assert command.isFinished.times_called == 3 + assert command.end.times_called == 1 + + command.isFinished = lambda: True + scheduler.run() + assert command.initialize.times_called == 2 + assert command.execute.times_called == 4 + assert command.isFinished.times_called == 4 + assert command.end.times_called == 2 + + scheduler.cancel(repeated) + assert command.initialize.times_called == 2 + assert command.execute.times_called == 4 + assert command.isFinished.times_called == 4 + assert command.end.times_called == 2 diff --git a/commandsv2/src/test/python/test_robotdisabledcommand.py b/commandsv2/src/test/python/test_robotdisabledcommand.py new file mode 100644 index 0000000000..120dc07018 --- /dev/null +++ b/commandsv2/src/test/python/test_robotdisabledcommand.py @@ -0,0 +1,154 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest +from wpilib import RobotState + + +def test_robotDisabledCommandCancel(scheduler: commands2.CommandScheduler): + command = commands2.Command() + scheduler.schedule(command) + assert scheduler.isScheduled(command) + DriverStationSim.setEnabled(False) + DriverStationSim.notifyNewData() + scheduler.run() + assert not scheduler.isScheduled(command) + DriverStationSim.setEnabled(True) + DriverStationSim.notifyNewData() + + +def test_runWhenDisabled(scheduler: commands2.CommandScheduler): + command = commands2.Command() + command.runsWhenDisabled = lambda: True + + scheduler.schedule(command) + + assert scheduler.isScheduled(command) + + DriverStationSim.setEnabled(False) + DriverStationSim.notifyNewData() + + scheduler.run() + + assert scheduler.isScheduled(command) + + +def test_sequentialGroupRunWhenDisabled(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command1.runsWhenDisabled = lambda: True + command2 = commands2.Command() + command2.runsWhenDisabled = lambda: True + command3 = commands2.Command() + command3.runsWhenDisabled = lambda: True + command4 = commands2.Command() + command4.runsWhenDisabled = lambda: False + + runWhenDisabled = commands2.SequentialCommandGroup(command1, command2) + dontRunWhenDisabled = commands2.SequentialCommandGroup(command3, command4) + + scheduler.schedule(runWhenDisabled) + scheduler.schedule(dontRunWhenDisabled) + + DriverStationSim.setEnabled(False) + DriverStationSim.notifyNewData() + + scheduler.run() + + assert scheduler.isScheduled(runWhenDisabled) + assert not scheduler.isScheduled(dontRunWhenDisabled) + + +def test_parallelGroupRunWhenDisabled(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command1.runsWhenDisabled = lambda: True + command2 = commands2.Command() + command2.runsWhenDisabled = lambda: True + command3 = commands2.Command() + command3.runsWhenDisabled = lambda: True + command4 = commands2.Command() + command4.runsWhenDisabled = lambda: False + + runWhenDisabled = commands2.ParallelCommandGroup(command1, command2) + dontRunWhenDisabled = commands2.ParallelCommandGroup(command3, command4) + + scheduler.schedule(runWhenDisabled) + scheduler.schedule(dontRunWhenDisabled) + + DriverStationSim.setEnabled(False) + DriverStationSim.notifyNewData() + + scheduler.run() + + assert scheduler.isScheduled(runWhenDisabled) + assert not scheduler.isScheduled(dontRunWhenDisabled) + + +def test_conditionalRunWhenDisabled(scheduler: commands2.CommandScheduler): + DriverStationSim.setEnabled(False) + DriverStationSim.notifyNewData() + + command1 = commands2.Command() + command1.runsWhenDisabled = lambda: True + command2 = commands2.Command() + command2.runsWhenDisabled = lambda: True + command3 = commands2.Command() + command3.runsWhenDisabled = lambda: True + command4 = commands2.Command() + command4.runsWhenDisabled = lambda: False + + runWhenDisabled = commands2.ConditionalCommand(command1, command2, lambda: True) + dontRunWhenDisabled = commands2.ConditionalCommand(command3, command4, lambda: True) + + scheduler.schedule(runWhenDisabled, dontRunWhenDisabled) + + assert scheduler.isScheduled(runWhenDisabled) + assert not scheduler.isScheduled(dontRunWhenDisabled) + + +def test_selectRunWhenDisabled(scheduler: commands2.CommandScheduler): + DriverStationSim.setEnabled(False) + DriverStationSim.notifyNewData() + + command1 = commands2.Command() + command1.runsWhenDisabled = lambda: True + command2 = commands2.Command() + command2.runsWhenDisabled = lambda: True + command3 = commands2.Command() + command3.runsWhenDisabled = lambda: True + command4 = commands2.Command() + command4.runsWhenDisabled = lambda: False + + runWhenDisabled = commands2.SelectCommand({1: command1, 2: command2}, lambda: 1) + dontRunWhenDisabled = commands2.SelectCommand({1: command3, 2: command4}, lambda: 1) + + scheduler.schedule(runWhenDisabled, dontRunWhenDisabled) + assert scheduler.isScheduled(runWhenDisabled) + assert not scheduler.isScheduled(dontRunWhenDisabled) + + +def test_parallelConditionalRunWhenDisabledTest(scheduler: commands2.CommandScheduler): + DriverStationSim.setEnabled(False) + DriverStationSim.notifyNewData() + + command1 = commands2.Command() + command1.runsWhenDisabled = lambda: True + command2 = commands2.Command() + command2.runsWhenDisabled = lambda: True + command3 = commands2.Command() + command3.runsWhenDisabled = lambda: True + command4 = commands2.Command() + command4.runsWhenDisabled = lambda: False + + runWhenDisabled = commands2.ConditionalCommand(command1, command2, lambda: True) + dontRunWhenDisabled = commands2.ConditionalCommand(command3, command4, lambda: True) + + parallel = commands2.cmd.parallel(runWhenDisabled, dontRunWhenDisabled) + + scheduler.schedule(parallel) + + assert not scheduler.isScheduled(runWhenDisabled) diff --git a/commandsv2/src/test/python/test_runcommand.py b/commandsv2/src/test/python/test_runcommand.py new file mode 100644 index 0000000000..6bd4fcffa7 --- /dev/null +++ b/commandsv2/src/test/python/test_runcommand.py @@ -0,0 +1,22 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_runCommandSchedule(scheduler: commands2.CommandScheduler): + counter = OOInteger(0) + + command = commands2.RunCommand(counter.incrementAndGet) + + scheduler.schedule(command) + scheduler.run() + scheduler.run() + scheduler.run() + + assert counter == 3 diff --git a/commandsv2/src/test/python/test_schedulecommand.py b/commandsv2/src/test/python/test_schedulecommand.py new file mode 100644 index 0000000000..401f4e0d8b --- /dev/null +++ b/commandsv2/src/test/python/test_schedulecommand.py @@ -0,0 +1,36 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_scheduleCommandSchedule(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + + start_spying_on(command1) + start_spying_on(command2) + + scheduleCommand = commands2.ScheduleCommand(command1, command2) + + scheduler.schedule(scheduleCommand) + + verify(command1).schedule() + verify(command2).schedule() + + +def test_scheduleCommandDruingRun(scheduler: commands2.CommandScheduler): + toSchedule = commands2.InstantCommand() + scheduleCommand = commands2.ScheduleCommand(toSchedule) + group = commands2.SequentialCommandGroup( + commands2.InstantCommand(), scheduleCommand + ) + + scheduler.schedule(group) + scheduler.schedule(commands2.RunCommand(lambda: None)) + scheduler.run() diff --git a/commandsv2/src/test/python/test_scheduler.py b/commandsv2/src/test/python/test_scheduler.py new file mode 100644 index 0000000000..de8fdae6ec --- /dev/null +++ b/commandsv2/src/test/python/test_scheduler.py @@ -0,0 +1,134 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_schedulerLambdaTestNoInterrupt(scheduler: commands2.CommandScheduler): + counter = OOInteger() + + scheduler.onCommandInitialize(lambda _: counter.incrementAndGet()) + scheduler.onCommandExecute(lambda _: counter.incrementAndGet()) + scheduler.onCommandFinish(lambda _: counter.incrementAndGet()) + + scheduler.schedule(commands2.InstantCommand()) + scheduler.run() + + assert counter == 3 + + +def test_schedulerInterruptLambda(scheduler: commands2.CommandScheduler): + counter = OOInteger() + + scheduler.onCommandInterrupt(lambda _: counter.incrementAndGet()) + + command = commands2.WaitCommand(10) + + scheduler.schedule(command) + scheduler.cancel(command) + + assert counter == 1 + + +def test_scheduler_interrupt_no_cause_lambda(scheduler: commands2.CommandScheduler): + counter = OOInteger() + + def on_interrupt(interrupted, cause): + assert cause is None + counter.incrementAndGet() + + scheduler.onCommandInterruptWithCause(on_interrupt) + + command = commands2.cmd.run(lambda: {}) + + scheduler.schedule(command) + scheduler.cancel(command) + + assert counter.get() == 1 + + +def test_scheduler_interrupt_cause_lambda(scheduler: commands2.CommandScheduler): + counter = OOInteger() + + subsystem = commands2.Subsystem() + command = subsystem.run(lambda: None) + interruptor = subsystem.runOnce(lambda: None) + + def on_interrupt(interrupted, cause): + assert cause is interruptor + counter.incrementAndGet() + + scheduler.onCommandInterruptWithCause(on_interrupt) + + scheduler.schedule(command) + scheduler.schedule(interruptor) + + assert counter.get() == 1 + + +def test_scheduler_interrupt_cause_lambda_in_run_loop( + scheduler: commands2.CommandScheduler, +): + counter = OOInteger() + + subsystem = commands2.Subsystem() + command = subsystem.run(lambda: None) + interruptor = subsystem.runOnce(lambda: None) + # This command will schedule interruptor in execute() inside the run loop + interruptor_scheduler = commands2.cmd.runOnce( + lambda: scheduler.schedule(interruptor) + ) + + def on_interrupt(interrupted, cause): + assert cause is interruptor + counter.incrementAndGet() + + scheduler.onCommandInterruptWithCause(on_interrupt) + + scheduler.schedule(command) + scheduler.schedule(interruptor_scheduler) + + scheduler.run() + + assert counter.get() == 1 + + +def test_unregisterSubsystem(scheduler: commands2.CommandScheduler): + system = commands2.Subsystem() + scheduler.registerSubsystem(system) + scheduler.unregisterSubsystem(system) + + +def test_schedulerCancelAll(scheduler: commands2.CommandScheduler): + counter = OOInteger() + + def on_interrupt(command, interruptor): + assert interruptor is None + + scheduler.onCommandInterrupt(lambda _: counter.incrementAndGet()) + scheduler.onCommandInterruptWithCause(on_interrupt) + + command = commands2.WaitCommand(10) + command2 = commands2.WaitCommand(10) + + scheduler.schedule(command) + scheduler.schedule(command2) + scheduler.cancelAll() + + assert counter == 2 + + +def test_scheduleScheduledNoOp(scheduler: commands2.CommandScheduler): + counter = OOInteger() + + command = commands2.cmd.startEnd(counter.incrementAndGet, lambda: None) + + scheduler.schedule(command) + scheduler.schedule(command) + + assert counter == 1 diff --git a/commandsv2/src/test/python/test_schedulingrecursion.py b/commandsv2/src/test/python/test_schedulingrecursion.py new file mode 100644 index 0000000000..ff6230dcc3 --- /dev/null +++ b/commandsv2/src/test/python/test_schedulingrecursion.py @@ -0,0 +1,165 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +@pytest.mark.parametrize( + "interruptionBehavior", + [ + commands2.InterruptionBehavior.kCancelIncoming, + commands2.InterruptionBehavior.kCancelSelf, + ], +) +def test_cancelFromInitialize( + interruptionBehavior: commands2.InterruptionBehavior, + scheduler: commands2.CommandScheduler, +): + hasOtherRun = OOBoolean() + requirement = commands2.Subsystem() + + selfCancels = commands2.Command() + selfCancels.addRequirements(requirement) + selfCancels.getInterruptionBehavior = lambda: interruptionBehavior + selfCancels.initialize = lambda: scheduler.cancel(selfCancels) + + other = commands2.RunCommand(lambda: hasOtherRun.set(True), requirement) + + scheduler.schedule(selfCancels) + scheduler.run() + scheduler.schedule(other) + + assert not scheduler.isScheduled(selfCancels) + assert scheduler.isScheduled(other) + scheduler.run() + assert hasOtherRun == True + + +@pytest.mark.parametrize( + "interruptionBehavior", + [ + commands2.InterruptionBehavior.kCancelIncoming, + commands2.InterruptionBehavior.kCancelSelf, + ], +) +def test_defaultCommandGetsRescheduledAfterSelfCanceling( + interruptionBehavior: commands2.InterruptionBehavior, + scheduler: commands2.CommandScheduler, +): + hasOtherRun = OOBoolean() + requirement = commands2.Subsystem() + + selfCancels = commands2.Command() + selfCancels.addRequirements(requirement) + selfCancels.getInterruptionBehavior = lambda: interruptionBehavior + selfCancels.initialize = lambda: scheduler.cancel(selfCancels) + + other = commands2.RunCommand(lambda: hasOtherRun.set(True), requirement) + scheduler.setDefaultCommand(requirement, other) + + scheduler.schedule(selfCancels) + scheduler.run() + + scheduler.run() + assert not scheduler.isScheduled(selfCancels) + assert scheduler.isScheduled(other) + scheduler.run() + assert hasOtherRun == True + + +def test_cancelFromEnd(scheduler: commands2.CommandScheduler): + counter = OOInteger() + selfCancels = commands2.Command() + + @patch_via_decorator(selfCancels) + def end(self, interrupted): + counter.incrementAndGet() + scheduler.cancel(self) + + scheduler.schedule(selfCancels) + + scheduler.cancel(selfCancels) + assert counter == 1 + assert not scheduler.isScheduled(selfCancels) + + +def test_scheduleFromEnd(scheduler: commands2.CommandScheduler): + counter = OOInteger() + requirement = commands2.Subsystem() + other = commands2.InstantCommand(lambda: None, requirement) + + selfCancels = commands2.Command() + selfCancels.addRequirements(requirement) + + @patch_via_decorator(selfCancels) + def end(self, interrupted): + counter.incrementAndGet() + scheduler.schedule(other) + + scheduler.schedule(selfCancels) + + scheduler.cancel(selfCancels) + assert counter == 1 + assert not scheduler.isScheduled(selfCancels) + + +def test_scheduleFromEndInterrupt(scheduler: commands2.CommandScheduler): + counter = OOInteger() + requirement = commands2.Subsystem() + other = commands2.InstantCommand(lambda: None, requirement) + + selfCancels = commands2.Command() + selfCancels.addRequirements(requirement) + + @patch_via_decorator(selfCancels) + def end(self, interrupted): + counter.incrementAndGet() + scheduler.schedule(other) + + scheduler.schedule(selfCancels) + + scheduler.schedule(other) + assert counter == 1 + assert not scheduler.isScheduled(selfCancels) + assert scheduler.isScheduled(other) + + +@pytest.mark.parametrize( + "interruptionBehavior", + [ + commands2.InterruptionBehavior.kCancelIncoming, + commands2.InterruptionBehavior.kCancelSelf, + ], +) +def test_scheduleInitializeFromDefaultCommand( + interruptionBehavior: commands2.InterruptionBehavior, + scheduler: commands2.CommandScheduler, +): + counter = OOInteger() + requirement = commands2.Subsystem() + other = commands2.InstantCommand(lambda: None, requirement).withInterruptBehavior( + interruptionBehavior + ) + + defaultCommand = commands2.Command() + defaultCommand.addRequirements(requirement) + + @patch_via_decorator(defaultCommand) + def initialize(self): + counter.incrementAndGet() + scheduler.schedule(other) + + scheduler.setDefaultCommand(requirement, defaultCommand) + + scheduler.run() + scheduler.run() + scheduler.run() + + assert counter == 3 + assert not scheduler.isScheduled(defaultCommand) + assert scheduler.isScheduled(other) diff --git a/commandsv2/src/test/python/test_selectcommand.py b/commandsv2/src/test/python/test_selectcommand.py new file mode 100644 index 0000000000..3ceb2a72e1 --- /dev/null +++ b/commandsv2/src/test/python/test_selectcommand.py @@ -0,0 +1,94 @@ +from typing import TYPE_CHECKING + +import commands2 +from compositiontestbase import MultiCompositionTestBase # type: ignore +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + from .compositiontestbase import MultiCompositionTestBase + +import pytest + + +class TestSelectCommandComposition(MultiCompositionTestBase): + def compose(self, *members: commands2.Command): + return commands2.SelectCommand(dict(enumerate(members)), lambda: 0) + + +def test_selectCommand(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + command3 = commands2.Command() + + start_spying_on(command1) + start_spying_on(command2) + start_spying_on(command3) + + command1.isFinished = lambda: True + + selectCommand = commands2.SelectCommand( + {"one": command1, "two": command2, "three": command3}, lambda: "one" + ) + + scheduler.schedule(selectCommand) + scheduler.run() + + verify(command1).initialize() + verify(command1).execute() + verify(command1).end(False) + + verify(command2, never()).initialize() + verify(command2, never()).execute() + verify(command2, never()).end(False) + + verify(command3, never()).initialize() + verify(command3, never()).execute() + verify(command3, never()).end(False) + + +def test_selectCommandInvalidKey(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + command3 = commands2.Command() + + start_spying_on(command1) + start_spying_on(command2) + start_spying_on(command3) + + command1.isFinished = lambda: True + + selectCommand = commands2.SelectCommand( + {"one": command1, "two": command2, "three": command3}, lambda: "four" + ) + + scheduler.schedule(selectCommand) + + +def test_selectCommandRequirement(scheduler: commands2.CommandScheduler): + system1 = commands2.Subsystem() + system2 = commands2.Subsystem() + system3 = commands2.Subsystem() + system4 = commands2.Subsystem() + + command1 = commands2.Command() + command1.addRequirements(system1, system2) + command2 = commands2.Command() + command2.addRequirements(system3) + command3 = commands2.Command() + command3.addRequirements(system3, system4) + + start_spying_on(command1) + start_spying_on(command2) + start_spying_on(command3) + + selectCommand = commands2.SelectCommand( + {"one": command1, "two": command2, "three": command3}, lambda: "one" + ) + + scheduler.schedule(selectCommand) + scheduler.schedule(commands2.InstantCommand(lambda: None, system3)) + + verify(command1).end(interrupted=True) + verify(command2, never()).end(interrupted=True) + verify(command3, never()).end(interrupted=True) diff --git a/commandsv2/src/test/python/test_sequentialcommandgroup.py b/commandsv2/src/test/python/test_sequentialcommandgroup.py new file mode 100644 index 0000000000..a6ecfb9699 --- /dev/null +++ b/commandsv2/src/test/python/test_sequentialcommandgroup.py @@ -0,0 +1,114 @@ +from typing import TYPE_CHECKING + +import commands2 +from compositiontestbase import MultiCompositionTestBase # type: ignore +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + from .compositiontestbase import MultiCompositionTestBase + +import pytest + + +class TestSequentialCommandGroupComposition(MultiCompositionTestBase): + def compose(self, *members: commands2.Command): + return commands2.SequentialCommandGroup(*members) + + +def test_sequntialGroupSchedule(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + + start_spying_on(command1) + start_spying_on(command2) + + group = commands2.SequentialCommandGroup(command1, command2) + + scheduler.schedule(group) + + verify(command1).initialize() + verify(command2, never()).initialize() + + command1.isFinished = lambda: True + scheduler.run() + + verify(command1).execute() + verify(command1).end(False) + verify(command2).initialize() + verify(command2, never()).execute() + verify(command2, never()).end(False) + + command2.isFinished = lambda: True + scheduler.run() + + verify(command1).execute() + verify(command1).end(False) + verify(command2).execute() + verify(command2).end(False) + + assert not scheduler.isScheduled(group) + + +def test_sequentialGroupInterrupt(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + command3 = commands2.Command() + + start_spying_on(command1) + start_spying_on(command2) + start_spying_on(command3) + + group = commands2.SequentialCommandGroup(command1, command2, command3) + + scheduler.schedule(group) + + command1.isFinished = lambda: True + scheduler.run() + scheduler.cancel(group) + scheduler.run() + + verify(command1).execute() + verify(command1, never()).end(True) + verify(command1).end(False) + verify(command2, never()).execute() + verify(command2).end(True) + verify(command3, never()).initialize() + verify(command3, never()).execute() + + # assert command3.end.times_called == 0 + verify(command3, never()).end(True) + verify(command3, never()).end(False) + + assert not scheduler.isScheduled(group) + + +def test_notScheduledCancel(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + + group = commands2.SequentialCommandGroup(command1, command2) + + scheduler.cancel(group) + + +def test_sequentialGroupRequirement(scheduler: commands2.CommandScheduler): + system1 = commands2.Subsystem() + system2 = commands2.Subsystem() + system3 = commands2.Subsystem() + system4 = commands2.Subsystem() + + command1 = commands2.InstantCommand() + command1.addRequirements(system1, system2) + command2 = commands2.InstantCommand() + command2.addRequirements(system3) + command3 = commands2.InstantCommand() + command3.addRequirements(system3, system4) + + group = commands2.SequentialCommandGroup(command1, command2) + + scheduler.schedule(group) + scheduler.schedule(command3) + + assert not scheduler.isScheduled(group) + assert scheduler.isScheduled(command3) diff --git a/commandsv2/src/test/python/test_startendcommand.py b/commandsv2/src/test/python/test_startendcommand.py new file mode 100644 index 0000000000..700412f710 --- /dev/null +++ b/commandsv2/src/test/python/test_startendcommand.py @@ -0,0 +1,30 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_startEndCommandSchedule(scheduler: commands2.CommandScheduler): + cond1 = OOBoolean(False) + cond2 = OOBoolean(False) + + command = commands2.StartEndCommand( + lambda: cond1.set(True), + lambda: cond2.set(True), + ) + + scheduler.schedule(command) + scheduler.run() + + assert scheduler.isScheduled(command) + + scheduler.cancel(command) + + assert not scheduler.isScheduled(command) + assert cond1 == True + assert cond2 == True diff --git a/commandsv2/src/test/python/test_swervecontrollercommand.py b/commandsv2/src/test/python/test_swervecontrollercommand.py new file mode 100644 index 0000000000..317461d433 --- /dev/null +++ b/commandsv2/src/test/python/test_swervecontrollercommand.py @@ -0,0 +1,142 @@ +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. + +from typing import TYPE_CHECKING, List, Tuple +import math + +from wpilib import Timer + +from wpimath.geometry import Pose2d, Rotation2d, Translation2d +from wpimath.kinematics import ( + SwerveModuleState, + SwerveModulePosition, + SwerveDrive4Kinematics, + SwerveDrive4Odometry, +) +from wpimath.controller import ( + ProfiledPIDControllerRadians, + PIDController, + HolonomicDriveController, +) +from wpimath.trajectory import ( + TrapezoidProfileRadians, + Trajectory, + TrajectoryConfig, + TrajectoryGenerator, +) + + +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + +import commands2 + + +def test_swervecontrollercommand(): + timer = Timer() + angle = Rotation2d(0) + + swerve_module_states = ( + SwerveModuleState(0, Rotation2d(0)), + SwerveModuleState(0, Rotation2d(0)), + SwerveModuleState(0, Rotation2d(0)), + SwerveModuleState(0, Rotation2d(0)), + ) + + swerve_module_positions = ( + SwerveModulePosition(0, Rotation2d(0)), + SwerveModulePosition(0, Rotation2d(0)), + SwerveModulePosition(0, Rotation2d(0)), + SwerveModulePosition(0, Rotation2d(0)), + ) + + rot_controller = ProfiledPIDControllerRadians( + 1, + 0, + 0, + TrapezoidProfileRadians.Constraints(3 * math.pi, math.pi), + ) + + x_tolerance = 1 / 12.0 + y_tolerance = 1 / 12.0 + angular_tolerance = 1 / 12.0 + + wheel_base = 0.5 + track_width = 0.5 + + kinematics = SwerveDrive4Kinematics( + Translation2d(wheel_base / 2, track_width / 2), + Translation2d(wheel_base / 2, -track_width / 2), + Translation2d(-wheel_base / 2, track_width / 2), + Translation2d(-wheel_base / 2, -track_width / 2), + ) + + odometry = SwerveDrive4Odometry( + kinematics, + Rotation2d(0), + swerve_module_positions, + Pose2d(0, 0, Rotation2d(0)), + ) + + def set_module_states(states): + nonlocal swerve_module_states + swerve_module_states = states + + def get_robot_pose() -> Pose2d: + odometry.update(angle, swerve_module_positions) + return odometry.getPose() + + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[Pose2d] = [] + waypoints.append(Pose2d(0, 0, Rotation2d(0))) + waypoints.append(Pose2d(1, 5, Rotation2d(3))) + config = TrajectoryConfig(8.8, 0.1) + trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config) + + end_state = trajectory.sample(trajectory.totalTime()) + + command = commands2.SwerveControllerCommand( + trajectory=trajectory, + pose=get_robot_pose, + kinematics=kinematics, + controller=HolonomicDriveController( + PIDController(0.6, 0, 0), + PIDController(0.6, 0, 0), + rot_controller, + ), + outputModuleStates=set_module_states, + requirements=(subsystem,), + ) + + timer.restart() + + command.initialize() + while not command.isFinished(): + command.execute() + angle = trajectory.sample(timer.get()).pose.rotation() + + for i in range(0, len(swerve_module_positions)): + swerve_module_positions[i].distance += ( + swerve_module_states[i].speed * 0.005 + ) + swerve_module_positions[i].angle = swerve_module_states[i].angle + + sim.step(0.005) + + timer.stop() + command.end(True) + + assert end_state.pose.X() == pytest.approx(get_robot_pose().X(), x_tolerance) + + assert end_state.pose.Y() == pytest.approx(get_robot_pose().Y(), y_tolerance) + + assert end_state.pose.rotation().radians() == pytest.approx( + get_robot_pose().rotation().radians(), + angular_tolerance, + ) diff --git a/commandsv2/src/test/python/test_sysidroutine.py b/commandsv2/src/test/python/test_sysidroutine.py new file mode 100644 index 0000000000..99ac445632 --- /dev/null +++ b/commandsv2/src/test/python/test_sysidroutine.py @@ -0,0 +1,168 @@ +import pytest +from unittest.mock import Mock, call, ANY +from wpilib.simulation import stepTiming, pauseTiming, resumeTiming +from wpimath.units import volts +from commands2 import Command, Subsystem +from commands2.sysid import SysIdRoutine +from wpilib.sysid import SysIdRoutineLog, State + + +class Mechanism(Subsystem): + def recordState(self, state: State): + pass + + def drive(self, voltage: volts): + pass + + def log(self, log: SysIdRoutineLog): + pass + + +@pytest.fixture +def mechanism(): + return Mock(spec=Mechanism) + + +@pytest.fixture +def sysid_routine(mechanism): + return SysIdRoutine( + SysIdRoutine.Config(recordState=mechanism.recordState), + SysIdRoutine.Mechanism(mechanism.drive, mechanism.log, Subsystem()), + ) + + +@pytest.fixture +def quasistatic_forward(sysid_routine): + return sysid_routine.quasistatic(SysIdRoutine.Direction.kForward) + + +@pytest.fixture +def quasistatic_reverse(sysid_routine): + return sysid_routine.quasistatic(SysIdRoutine.Direction.kReverse) + + +@pytest.fixture +def dynamic_forward(sysid_routine): + return sysid_routine.dynamic(SysIdRoutine.Direction.kForward) + + +@pytest.fixture +def dynamic_reverse(sysid_routine): + return sysid_routine.dynamic(SysIdRoutine.Direction.kReverse) + + +@pytest.fixture(autouse=True) +def timing(): + pauseTiming() + yield + resumeTiming() + + +def run_command(command: Command): + command.initialize() + command.execute() + stepTiming(1) + command.execute() + command.end(True) + + +def test_record_state_bookends_motor_logging( + mechanism, quasistatic_forward, dynamic_forward +): + run_command(quasistatic_forward) + + mechanism.assert_has_calls( + [ + call.drive(ANY), + call.log(ANY), + call.recordState(State.kQuasistaticForward), + call.drive(ANY), + call.recordState(State.kNone), + ], + any_order=False, + ) + + mechanism.reset_mock() + run_command(dynamic_forward) + + mechanism.assert_has_calls( + [ + call.drive(ANY), + call.log(ANY), + call.recordState(State.kDynamicForward), + call.drive(ANY), + call.recordState(State.kNone), + ], + any_order=False, + ) + + +def test_tests_declare_correct_state( + mechanism, + quasistatic_forward, + quasistatic_reverse, + dynamic_forward, + dynamic_reverse, +): + run_command(quasistatic_forward) + mechanism.recordState.assert_any_call(State.kQuasistaticForward) + + run_command(quasistatic_reverse) + mechanism.recordState.assert_any_call(State.kQuasistaticReverse) + + run_command(dynamic_forward) + mechanism.recordState.assert_any_call(State.kDynamicForward) + + run_command(dynamic_reverse) + mechanism.recordState.assert_any_call(State.kDynamicReverse) + + +def test_tests_output_correct_voltage( + mechanism, + quasistatic_forward, + quasistatic_reverse, + dynamic_forward, + dynamic_reverse, +): + run_command(quasistatic_forward) + + mechanism.drive.assert_has_calls( + [ + call(pytest.approx(1.0)), + call(pytest.approx(0.0)), + ], + any_order=False, + ) + + mechanism.reset_mock() + run_command(quasistatic_reverse) + + mechanism.drive.assert_has_calls( + [ + call(pytest.approx(-1.0)), + call(pytest.approx(0.0)), + ], + any_order=False, + ) + + mechanism.reset_mock() + run_command(dynamic_forward) + + mechanism.drive.assert_has_calls( + [ + call(pytest.approx(7.0)), + call(pytest.approx(0.0)), + ], + any_order=False, + ) + + mechanism.reset_mock() + run_command(dynamic_reverse) + + mechanism.drive.assert_has_calls( + [ + call(pytest.approx(-7.0)), + call(pytest.approx(0.0)), + ], + any_order=False, + ) diff --git a/commandsv2/src/test/python/test_trapezoidprofilecommand.py b/commandsv2/src/test/python/test_trapezoidprofilecommand.py new file mode 100644 index 0000000000..730fc821c3 --- /dev/null +++ b/commandsv2/src/test/python/test_trapezoidprofilecommand.py @@ -0,0 +1,142 @@ +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. + +from typing import TYPE_CHECKING, List, Tuple +import math + +import wpimath.controller as controller +import wpimath.trajectory as trajectory +import wpimath.geometry as geometry +import wpimath.kinematics as kinematics +from wpimath.trajectory import TrapezoidProfile as DimensionlessProfile +from wpimath.trajectory import TrapezoidProfileRadians as RadiansProfile + +from wpilib import Timer + +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + +import commands2 + + +class TrapezoidProfileRadiansFixture: + def __init__(self): + constraints: RadiansProfile.Constraints = RadiansProfile.Constraints( + 3 * math.pi, math.pi + ) + self._profile: RadiansProfile = RadiansProfile(constraints) + self._goal_state = RadiansProfile.State(3, 0) + + self._state = self._profile.calculate( + 0, self._goal_state, RadiansProfile.State(0, 0) + ) + + self._timer = Timer() + + def profileOutput(self, state: RadiansProfile.State) -> None: + self._state = state + + def currentState(self) -> RadiansProfile.State: + return self._state + + def getGoal(self) -> RadiansProfile.State: + return self._goal_state + + +@pytest.fixture() +def get_trapezoid_profile_radians() -> TrapezoidProfileRadiansFixture: + return TrapezoidProfileRadiansFixture() + + +class TrapezoidProfileFixture: + def __init__(self): + constraints: DimensionlessProfile.Constraints = ( + DimensionlessProfile.Constraints(3 * math.pi, math.pi) + ) + self._profile: DimensionlessProfile = DimensionlessProfile(constraints) + self._goal_state = DimensionlessProfile.State(3, 0) + + self._state = self._profile.calculate( + 0, self._goal_state, DimensionlessProfile.State(0, 0) + ) + + self._timer = Timer() + + def profileOutput(self, state: DimensionlessProfile.State) -> None: + self._state = state + + def currentState(self) -> DimensionlessProfile.State: + return self._state + + def getGoal(self) -> DimensionlessProfile.State: + return self._goal_state + + +@pytest.fixture() +def get_trapezoid_profile_dimensionless() -> TrapezoidProfileFixture: + return TrapezoidProfileFixture() + + +def test_trapezoidProfileDimensionless( + scheduler: commands2.CommandScheduler, get_trapezoid_profile_dimensionless +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + + fixture_data = get_trapezoid_profile_dimensionless + + command = commands2.TrapezoidProfileCommand( + fixture_data._profile, + fixture_data.profileOutput, + fixture_data.getGoal, + fixture_data.currentState, + subsystem, + ) + + fixture_data._timer.restart() + + command.initialize() + + count = 0 + while not command.isFinished(): + command.execute() + count += 1 + sim.step(0.005) + + fixture_data._timer.stop() + command.end(True) + + +def test_trapezoidProfileRadians( + scheduler: commands2.CommandScheduler, get_trapezoid_profile_radians +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + + fixture_data = get_trapezoid_profile_radians + + command = commands2.TrapezoidProfileCommand( + fixture_data._profile, + fixture_data.profileOutput, + fixture_data.getGoal, + fixture_data.currentState, + subsystem, + ) + + fixture_data._timer.restart() + + command.initialize() + + count = 0 + while not command.isFinished(): + command.execute() + count += 1 + sim.step(0.005) + + fixture_data._timer.stop() + command.end(True) diff --git a/commandsv2/src/test/python/test_trigger.py b/commandsv2/src/test/python/test_trigger.py new file mode 100644 index 0000000000..79ef9f4c79 --- /dev/null +++ b/commandsv2/src/test/python/test_trigger.py @@ -0,0 +1,236 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore +from wpilib.simulation import stepTiming + +if TYPE_CHECKING: + from .util import * + + +def test_onTrue(scheduler: commands2.CommandScheduler): + finished = OOBoolean(False) + command1 = commands2.WaitUntilCommand(finished) + + button = InternalButton() + button.setPressed(False) + button.onTrue(command1) + scheduler.run() + assert not command1.isScheduled() + button.setPressed(True) + scheduler.run() + assert command1.isScheduled() + finished.set(True) + scheduler.run() + assert not command1.isScheduled() + + +def test_onFalse(scheduler: commands2.CommandScheduler): + finished = OOBoolean(False) + command1 = commands2.WaitUntilCommand(finished) + + button = InternalButton() + button.setPressed(True) + button.onFalse(command1) + scheduler.run() + assert not command1.isScheduled() + button.setPressed(False) + scheduler.run() + assert command1.isScheduled() + finished.set(True) + scheduler.run() + assert not command1.isScheduled() + + +def test_onChange(scheduler: commands2.CommandScheduler): + finished = OOBoolean(False) + command1 = commands2.WaitUntilCommand(finished) + + button = InternalButton() + button.setPressed(True) + button.onChange(command1) + scheduler.run() + assert not command1.isScheduled() + button.setPressed(False) + scheduler.run() + assert command1.isScheduled() + finished.set(True) + scheduler.run() + assert not command1.isScheduled() + + +def test_whileTrueRepeatedly(scheduler: commands2.CommandScheduler): + inits = OOInteger(0) + counter = OOInteger(0) + + command1 = commands2.FunctionalCommand( + inits.incrementAndGet, + lambda: None, + lambda _: None, + lambda: counter.incrementAndGet() % 2 == 0, + ).repeatedly() + + button = InternalButton() + button.setPressed(False) + button.whileTrue(command1) + scheduler.run() + assert inits == 0 + button.setPressed(True) + scheduler.run() + assert inits == 1 + scheduler.run() + assert inits == 1 + scheduler.run() + assert inits == 2 + button.setPressed(False) + scheduler.run() + assert inits == 2 + + +def test_whileTrueLambdaRunCommand(scheduler: commands2.CommandScheduler): + counter = OOInteger(0) + + command1 = commands2.RunCommand(counter.incrementAndGet) + + button = InternalButton() + button.setPressed(False) + button.whileTrue(command1) + scheduler.run() + assert counter == 0 + button.setPressed(True) + scheduler.run() + assert counter == 1 + scheduler.run() + assert counter == 2 + button.setPressed(False) + scheduler.run() + assert counter == 2 + + +def test_whileTrueOnce(scheduler: commands2.CommandScheduler): + startCounter = OOInteger(0) + endCounter = OOInteger(0) + + command1 = commands2.StartEndCommand( + startCounter.incrementAndGet, endCounter.incrementAndGet + ) + + button = InternalButton() + button.setPressed(False) + button.whileTrue(command1) + scheduler.run() + assert startCounter == 0 + assert endCounter == 0 + button.setPressed(True) + scheduler.run() + scheduler.run() + assert startCounter == 1 + assert endCounter == 0 + button.setPressed(False) + scheduler.run() + assert startCounter == 1 + assert endCounter == 1 + + +def test_toggleOnTrue(scheduler: commands2.CommandScheduler): + startCounter = OOInteger(0) + endCounter = OOInteger(0) + + command1 = commands2.StartEndCommand( + startCounter.incrementAndGet, endCounter.incrementAndGet + ) + + button = InternalButton() + button.setPressed(False) + button.toggleOnTrue(command1) + scheduler.run() + assert startCounter == 0 + assert endCounter == 0 + button.setPressed(True) + scheduler.run() + scheduler.run() + assert startCounter == 1 + assert endCounter == 0 + button.setPressed(False) + scheduler.run() + assert startCounter == 1 + assert endCounter == 0 + button.setPressed(True) + scheduler.run() + assert startCounter == 1 + assert endCounter == 1 + + +def test_cancelWhenActive(scheduler: commands2.CommandScheduler): + startCounter = OOInteger(0) + endCounter = OOInteger(0) + + button = InternalButton() + command1 = commands2.StartEndCommand( + startCounter.incrementAndGet, endCounter.incrementAndGet + ).until(button) + + button.setPressed(False) + command1.schedule() + scheduler.run() + assert startCounter == 1 + assert endCounter == 0 + button.setPressed(True) + scheduler.run() + assert startCounter == 1 + assert endCounter == 1 + scheduler.run() + assert startCounter == 1 + assert endCounter == 1 + + +def test_triggerComposition(): + button1 = InternalButton() + button2 = InternalButton() + + button1.setPressed(True) + button2.setPressed(False) + + assert button1.and_(button2).getAsBoolean() == False + assert button1.or_(button2)() == True + assert bool(button1.negate()) == False + assert (button1 & ~button2)() == True + + +def test_triggerCompositionSupplier(): + button1 = InternalButton() + supplier = lambda: False + + button1.setPressed(True) + + assert button1.and_(supplier)() == False + assert button1.or_(supplier)() == True + + +def test_debounce(scheduler: commands2.CommandScheduler): + command = commands2.Command() + start_spying_on(command) + + button = InternalButton() + debounced = button.debounce(0.1) + + debounced.onTrue(command) + + button.setPressed(True) + scheduler.run() + + verify(command, never()).schedule() + + stepTiming(0.3) + + button.setPressed(True) + scheduler.run() + verify(command).schedule() + + +def test_booleanSupplier(): + button = InternalButton() + + assert button() == False + button.setPressed(True) + assert button() == True diff --git a/commandsv2/src/test/python/test_waitcommand.py b/commandsv2/src/test/python/test_waitcommand.py new file mode 100644 index 0000000000..125a2e310a --- /dev/null +++ b/commandsv2/src/test/python/test_waitcommand.py @@ -0,0 +1,50 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_waitCommand(scheduler: commands2.CommandScheduler): + with ManualSimTime() as sim: + waitCommand = commands2.WaitCommand(2) + + scheduler.schedule(waitCommand) + scheduler.run() + sim.step(1) + scheduler.run() + + assert scheduler.isScheduled(waitCommand) + + sim.step(2) + + scheduler.run() + + assert not scheduler.isScheduled(waitCommand) + + +def test_withTimeout(scheduler: commands2.CommandScheduler): + with ManualSimTime() as sim: + command1 = commands2.Command() + start_spying_on(command1) + + timeout = command1.withTimeout(2) + + scheduler.schedule(timeout) + scheduler.run() + + verify(command1).initialize() + verify(command1).execute() + assert not scheduler.isScheduled(command1) + assert scheduler.isScheduled(timeout) + + sim.step(3) + scheduler.run() + + verify(command1).end(True) + verify(command1, never()).end(False) + assert not scheduler.isScheduled(timeout) diff --git a/commandsv2/src/test/python/test_waituntilcommand.py b/commandsv2/src/test/python/test_waituntilcommand.py new file mode 100644 index 0000000000..2762a1f6c8 --- /dev/null +++ b/commandsv2/src/test/python/test_waituntilcommand.py @@ -0,0 +1,22 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_waitUntil(scheduler: commands2.CommandScheduler): + condition = OOBoolean() + + command = commands2.WaitUntilCommand(condition) + + scheduler.schedule(command) + scheduler.run() + assert scheduler.isScheduled(command) + condition.set(True) + scheduler.run() + assert not scheduler.isScheduled(command) diff --git a/commandsv2/src/test/python/util.py b/commandsv2/src/test/python/util.py new file mode 100644 index 0000000000..0331840394 --- /dev/null +++ b/commandsv2/src/test/python/util.py @@ -0,0 +1,294 @@ +from typing import Any, Dict, TypeVar, Type + +import inspect + +import commands2 +from wpilib.simulation import DriverStationSim, pauseTiming, resumeTiming, stepTiming + + +Y = TypeVar("Y") + + +def full_subclass_of(cls: Type[Y]) -> Type[Y]: + # Pybind classes can't be monkeypatched. + # This generates a subclass with every method filled out + # so that it can be monkeypatched. + retlist = [] + clsname = cls.__name__ # + "_Subclass" + classdef = f"class {clsname}(cls):\n" + for name in dir(cls): + # for name in set(dir(cls)): + value = getattr(cls, name) + if callable(value) and not inspect.isclass(value) and not name.startswith("_"): + classdef += f" def {name}(self, *args, **kwargs):\n" + classdef += f" return super().{name}(*args, **kwargs)\n" + classdef += " ...\n" + classdef += f"retlist.append({clsname})\n" + print(classdef) + exec(classdef, globals(), locals()) + return retlist[0] + + +class ManualSimTime: + def __enter__(self) -> "ManualSimTime": + pauseTiming() + return self + + def __exit__(self, *args): + resumeTiming() + + def step(self, delta: float): + stepTiming(delta) + + +class OOInteger: + def __init__(self, value: int = 0) -> None: + self.value = value + + def get(self) -> int: + return self.value + + def set(self, value: int): + self.value = value + + def incrementAndGet(self) -> int: + self.value += 1 + return self.value + + def addAndGet(self, value: int) -> int: + self.value += value + return self.value + + def __eq__(self, value: float) -> bool: + return self.value == value + + def __lt__(self, value: float) -> bool: + return self.value < value + + def __call__(self) -> int: + return self.value + + +class OOBoolean: + def __init__(self, value: bool = False) -> None: + self.value = value + + def get(self) -> bool: + return self.value + + def set(self, value: bool): + self.value = value + + def __eq__(self, value: object) -> bool: + return self.value == value + + def __bool__(self) -> bool: + return self.value + + def __call__(self) -> bool: + return self.value + + +class InternalButton(commands2.button.Trigger): + def __init__(self): + super().__init__(self.isPressed) + self.pressed = False + + def isPressed(self) -> bool: + return self.pressed + + def setPressed(self, value: bool): + self.pressed = value + + def __call__(self) -> bool: + return self.pressed + + +class OOFloat: + def __init__(self, value: float = 0.0) -> None: + self.value = value + + def get(self) -> float: + return self.value + + def set(self, value: float): + self.value = value + + def incrementAndGet(self) -> float: + self.value += 1 + return self.value + + def addAndGet(self, value: float) -> float: + self.value += value + return self.value + + def __eq__(self, value: float) -> bool: + return self.value == value + + def __lt__(self, value: float) -> bool: + return self.value < value + + def __call__(self) -> float: + return self.value + + def __name__(self) -> str: + return "OOFloat" + + +########################################## +# Fakito Framework + + +def _get_all_args_as_kwargs(method, *args, **kwargs) -> Dict[str, Any]: + try: + import inspect + + method_args = inspect.getcallargs(method, *args, **kwargs) + + method_arg_names = list(inspect.signature(method).parameters.keys()) + + for idx, arg in enumerate(args): + method_args[method_arg_names[idx]] = arg + + try: + del method_args["self"] + except KeyError: + pass + return method_args + except TypeError: + # Pybind methods can't be inspected + # The exact args/kwargs that are passed in are checked instead + r = {} + for idx, arg in enumerate(args): + r[idx] = arg + r.update(kwargs) + return r + + +class MethodWrapper: + def __init__(self, method): + self.method = method + self.og_method = method + self.times_called = 0 + self.call_log = [] + + def __call__(self, *args, **kwargs): + self.times_called += 1 + method_args = _get_all_args_as_kwargs(self.method, *args, **kwargs) + self.call_log.append(method_args) + return self.method(*args, **kwargs) + + def called_with(self, *args, **kwargs): + return _get_all_args_as_kwargs(self.method, *args, **kwargs) in self.call_log + + def times_called_with(self, *args, **kwargs): + return self.call_log.count( + _get_all_args_as_kwargs(self.method, *args, **kwargs) + ) + + +def start_spying_on(obj: Any) -> None: + """ + Mocks all methods on an object, so that that call info can be used in asserts. + + Example: + ``` + obj = SomeClass() + start_spying_on(obj) + obj.method() + obj.method = lambda: None # supports monkeypatching + assert obj.method.times_called == 2 + assert obj.method.called_with(arg1=1, arg2=2) + assert obj.method.times_called_with(arg1=1, arg2=2) == 2 + ``` + """ + + for name in dir(obj): + value = getattr(obj, name) + if callable(value) and not inspect.isclass(value) and not name.startswith("_"): + setattr(obj, name, MethodWrapper(value)) + + if not hasattr(obj.__class__, "_is_being_spied_on"): + try: + old_setattr = obj.__class__.__setattr__ + except AttributeError: + old_setattr = object.__setattr__ + + def _setattr(self, name, value): + if name in dir(self): + existing_value = getattr(self, name) + if isinstance(existing_value, MethodWrapper): + existing_value.method = value + return + old_setattr(self, name, value) + + obj.__class__.__setattr__ = _setattr + obj.__class__._is_being_spied_on = True + + +# fakito verify +def reset(obj: Any) -> None: + """ + Resets the call log of all mocked methods on an object. + Also restores all monkeypatched methods. + """ + for name in dir(obj): + value = getattr(obj, name) + if isinstance(value, MethodWrapper): + value.method = value.og_method + value.times_called = 0 + value.call_log = [] + + +class times: + def __init__(self, times: int) -> None: + self.times = times + + +def never() -> times: + return times(0) + + +class _verify: + def __init__(self, obj: Any, times: times = times(1)): + self.obj = obj + self.times = times.times + + def __getattribute__(self, name: str) -> Any: + def self_dot(name: str): + return super(_verify, self).__getattribute__(name) + + def times_string(times: int) -> str: + if times == 1: + return "1 time" + else: + return f"{times} times" + + def check(*args, **kwargs): + __tracebackhide__ = True + # import code + # code.interact(local={**globals(), **locals()}) + method = getattr(self_dot("obj"), name) + # method = getattr(self1.obj, name) + assert method.times_called_with(*args, **kwargs) == self_dot( + "times" + ), f"Expected {name} to be called {times_string(self_dot('times'))} with {args} {kwargs}, but was called {times_string(method.times_called_with(*args, **kwargs))}" + + return check + + +T = TypeVar("T") + + +def verify(obj: T, times: times = times(1)) -> T: + # import code + # code.interact(local={**globals(), **locals()}) + return _verify(obj, times) # type: ignore + + +def patch_via_decorator(obj: Any): + def decorator(method): + setattr(obj, method.__name__, method.__get__(obj, obj.__class__)) + return method + + return decorator