diff --git a/.github/workflows/dist.yml b/.github/workflows/dist.yml index a0fa3047..fc3d05b1 100644 --- a/.github/workflows/dist.yml +++ b/.github/workflows/dist.yml @@ -11,14 +11,9 @@ on: jobs: ci: - uses: robotpy/build-actions/.github/workflows/package-ci.yml@v2023 + uses: robotpy/build-actions/.github/workflows/package-pure.yml@v2024-alpha with: enable_sphinx_check: false secrets: - SSH_USER: ${{ secrets.SSH_USER }} - SSH_KEY: ${{ secrets.SSH_KEY }} - SSH_PASSPHRASE: ${{ secrets.SSH_PASSPHRASE }} META_REPO_ACCESS_TOKEN: ${{ secrets.REPO_ACCESS_TOKEN }} - RTD_TOKEN: ${{ secrets.RTD_TOKEN }} - RTD_WEBHOOK: ${{ secrets.RTD_WEBHOOK }} PYPI_API_TOKEN: ${{ secrets.PYPI_PASSWORD }} diff --git a/commands2/__init__.py b/commands2/__init__.py index d4c26b32..225a4d98 100644 --- a/commands2/__init__.py +++ b/commands2/__init__.py @@ -1,96 +1,107 @@ -from . import _init_impl +from .button import Trigger +from .command import Command, InterruptionBehavior -from .version import version as __version__ +from . import cmd -# autogenerated by 'robotpy-build create-imports commands2 commands2._impl' -from ._impl import ( - Command, - CommandBase, - CommandGroupBase, - CommandScheduler, - ConditionalCommand, - FunctionalCommand, - InstantCommand, - MecanumControllerCommand, - NotifierCommand, - PIDCommand, - PIDSubsystem, - ParallelCommandGroup, - ParallelDeadlineGroup, - ParallelRaceGroup, - PerpetualCommand, - PrintCommand, - ProfiledPIDCommand, - ProfiledPIDSubsystem, - ProxyScheduleCommand, - RamseteCommand, - RepeatCommand, - RunCommand, - ScheduleCommand, - SelectCommand, - SequentialCommandGroup, - StartEndCommand, - Subsystem, - SubsystemBase, - Swerve2ControllerCommand, - Swerve3ControllerCommand, - Swerve4ControllerCommand, - Swerve6ControllerCommand, - TimedCommandRobot, - TrapezoidProfileCommand, - TrapezoidProfileCommandRadians, - TrapezoidProfileSubsystem, - TrapezoidProfileSubsystemRadians, - Trigger, - WaitCommand, - WaitUntilCommand, - # button, - # cmd, - requirementsDisjoint, -) +# from .cmd import ( +# deadline, +# either, +# none, +# parallel, +# print_, +# race, +# repeatingSequence, +# run, +# runEnd, +# runOnce, +# select, +# sequence, +# startEnd, +# waitSeconds, +# waitUntil, +# ) +from .commandgroup import CommandGroup, IllegalCommandUse +from .commandscheduler import CommandScheduler +from .conditionalcommand import ConditionalCommand +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 .perpetualcommand import PerpetualCommand +from .printcommand import PrintCommand +from .proxycommand import ProxyCommand +from .proxyschedulecommand import ProxyScheduleCommand +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 .timedcommandrobot import TimedCommandRobot +from .waitcommand import WaitCommand +from .waituntilcommand import WaitUntilCommand +from .wrappercommand import WrapperCommand __all__ = [ + "cmd", "Command", - "CommandBase", - "CommandGroupBase", + "CommandGroup", "CommandScheduler", "ConditionalCommand", - "SelectCommand", "FunctionalCommand", + "IllegalCommandUse", "InstantCommand", - "MecanumControllerCommand", + "InterruptionBehavior", "NotifierCommand", - "PIDCommand", - "PIDSubsystem", "ParallelCommandGroup", "ParallelDeadlineGroup", "ParallelRaceGroup", "PerpetualCommand", "PrintCommand", - "ProfiledPIDCommand", - "ProfiledPIDSubsystem", + "ProxyCommand", "ProxyScheduleCommand", - "RamseteCommand", "RepeatCommand", "RunCommand", "ScheduleCommand", + "SelectCommand", "SequentialCommandGroup", "StartEndCommand", "Subsystem", - "SubsystemBase", - "Swerve2ControllerCommand", - "Swerve3ControllerCommand", - "Swerve4ControllerCommand", - "Swerve6ControllerCommand", "TimedCommandRobot", - "TrapezoidProfileCommand", - "TrapezoidProfileCommandRadians", - "TrapezoidProfileSubsystem", - "TrapezoidProfileSubsystemRadians", - "Trigger", "WaitCommand", "WaitUntilCommand", - # "button", - # "cmd", - "requirementsDisjoint", + "WrapperCommand", + # "none", + # "runOnce", + # "run", + # "startEnd", + # "runEnd", + # "print_", + # "waitSeconds", + # "waitUntil", + # "either", + # "select", + # "sequence", + # "repeatingSequence", + # "parallel", + # "race", + # "deadline", + "Trigger", # was here in 2023 ] + +def __getattr__(attr): + if attr == "SubsystemBase": + import warnings + warnings.warn("SubsystemBase is deprecated", DeprecationWarning, stacklevel=2) + return Subsystem + + if attr == "CommandBase": + import warnings + warnings.warn("CommandBase is deprecated", DeprecationWarning, stacklevel=2) + return Command + + raise AttributeError("module {!r} has no attribute " + "{!r}".format(__name__, attr)) \ No newline at end of file diff --git a/commands2/button/__init__.py b/commands2/button/__init__.py index 651dc05b..f6947397 100644 --- a/commands2/button/__init__.py +++ b/commands2/button/__init__.py @@ -1,17 +1,14 @@ -# autogenerated by 'robotpy-build create-imports commands2.button commands2._impl.button' -from .._impl.button import ( - Button, - CommandGenericHID, - CommandJoystick, - CommandPS4Controller, - CommandXboxController, - JoystickButton, - NetworkButton, - POVButton, -) +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__ = [ - "Button", + "Trigger", "CommandGenericHID", "CommandJoystick", "CommandPS4Controller", diff --git a/commands2/button/commandgenerichid.py b/commands2/button/commandgenerichid.py new file mode 100644 index 00000000..1512fd15 --- /dev/null +++ b/commands2/button/commandgenerichid.py @@ -0,0 +1,190 @@ +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 GenericHID with 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 attache the event to. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getRawButtonPressed(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 {@link + CommandScheduler#getDefaultButtonLoop() the default command scheduler button loop}. + :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 {@link CommandScheduler#getDefaultButtonLoop() the default command + scheduler button loop}. + + :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 {@link CommandScheduler#getDefaultButtonLoop() the default + command scheduler button loop}. + + :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 {@link CommandScheduler#getDefaultButtonLoop() the default command + scheduler button loop}. + + :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 {@link CommandScheduler#getDefaultButtonLoop() the + default command scheduler button loop}. + + :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 {@link CommandScheduler#getDefaultButtonLoop() the default command + scheduler button loop}. + + :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 {@link CommandScheduler#getDefaultButtonLoop() the + default command scheduler button loop}. + + :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 {@link CommandScheduler#getDefaultButtonLoop() the default command + scheduler button loop}. + + :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 {@link CommandScheduler#getDefaultButtonLoop() the default + command scheduler button loop}. + + :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 {@link CommandScheduler#getDefaultButtonLoop() the + default command scheduler button loop}. + + :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 {@code 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 {@code + 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 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) diff --git a/commands2/button/commandjoystick.py b/commands2/button/commandjoystick.py new file mode 100644 index 00000000..cdb84340 --- /dev/null +++ b/commands2/button/commandjoystick.py @@ -0,0 +1,203 @@ +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 Joystick with Trigger factories for command-based. + """ + + 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): + """ + 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. + :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. + :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 getThrottleChannel(self) -> int: + """ + Get the channel currently associated with the throttle axis. + + :returns: The channel for the axis. + """ + return self._hid.getThrottleChannel() + + def getTwistChannel(self) -> int: + """ + Get the channel currently associated with the twist axis. + + :returns: The channel for the axis. + """ + return self._hid.getTwistChannel() + + def getX(self) -> float: + """ + Get the x position of the HID. + + :returns: the x position + """ + return self._hid.getX() + + def getY(self) -> float: + """ + Get the y position of the HID. + + :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 direction 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. + + :returns: The direction of the vector in radians + """ + return self._hid.getDirectionRadians() + + def getDirectionDegrees(self) -> float: + """ + Get the direction of the vector formed by the joystick and its origin in degrees. + + :returns: The direction of the vector in degrees + """ + return self._hid.getDirectionDegrees() diff --git a/commands2/button/commandps4controller.py b/commands2/button/commandps4controller.py new file mode 100644 index 00000000..567ecf33 --- /dev/null +++ b/commands2/button/commandps4controller.py @@ -0,0 +1,249 @@ +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. + """ + + 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): + """ + 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. + :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. + :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. + :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. + :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. + :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. + :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. + :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. + :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. + :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. + :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. + :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. + :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. + :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. + :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. + + :returns: the axis value. + """ + return self._hid.getLeftX() + + def getRightX(self) -> float: + """ + Get the X axis value of right side of the controller. + + :returns: the axis value. + """ + return self._hid.getRightX() + + def getLeftY(self) -> float: + """ + Get the Y axis value of left side of the controller. + + :returns: the axis value. + """ + return self._hid.getLeftY() + + def getRightY(self) -> float: + """ + Get the Y axis value of right side of the controller. + + :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/commands2/button/commandxboxcontroller.py b/commands2/button/commandxboxcontroller.py new file mode 100644 index 00000000..50428d2d --- /dev/null +++ b/commands2/button/commandxboxcontroller.py @@ -0,0 +1,235 @@ +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. + """ + + 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): + """ + 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. + :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. + :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. + :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. + :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. + :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. + :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. + :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. + :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. + :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. + :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. + :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. + :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. + + :returns: The axis value. + """ + return self._hid.getLeftX() + + def getRightX(self) -> float: + """ + Get the X axis value of right side of the controller. + + :returns: The axis value. + """ + return self._hid.getRightX() + + def getLeftY(self) -> float: + """ + Get the Y axis value of left side of the controller. + + :returns: The axis value. + """ + return self._hid.getLeftY() + + def getRightY(self) -> float: + """ + Get the Y axis value of right side of the controller. + + :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/commands2/button/joystickbutton.py b/commands2/button/joystickbutton.py new file mode 100644 index 00000000..2e0e107c --- /dev/null +++ b/commands2/button/joystickbutton.py @@ -0,0 +1,20 @@ +from wpilib.interfaces import GenericHID + +from .trigger import Trigger + + +class JoystickButton(Trigger): + """ + A Button that gets its state from a GenericHID. + + This class is provided by the NewCommands VendorDep + """ + + 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 GenericHID#getRawButton(int) + """ + super().__init__(lambda: joystick.getRawButton(buttonNumber)) diff --git a/commands2/button/networkbutton.py b/commands2/button/networkbutton.py new file mode 100644 index 00000000..c6c6602c --- /dev/null +++ b/commands2/button/networkbutton.py @@ -0,0 +1,127 @@ +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 NetworkTable boolean field. + + This class is provided by the NewCommands VendorDep + """ + + @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/commands2/button/povbutton.py b/commands2/button/povbutton.py new file mode 100644 index 00000000..8e0c06cd --- /dev/null +++ b/commands2/button/povbutton.py @@ -0,0 +1,21 @@ +from wpilib.interfaces import GenericHID + +from .trigger import Trigger + + +class POVButton(Trigger): + """ + A Button that gets its state from a POV on a GenericHID. + + This class is provided by the NewCommands VendorDep + """ + + 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 GenericHID#getPOV(int)) + """ + super().__init__(lambda: joystick.getPOV(povNumber) == angle) diff --git a/commands2/button/trigger.py b/commands2/button/trigger.py new file mode 100644 index 00000000..ce6ffa78 --- /dev/null +++ b/commands2/button/trigger.py @@ -0,0 +1,265 @@ +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]): + 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 + """ + + @self._loop.bind + def _(state=SimpleNamespace(pressed_last=self._condition())): + 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 + """ + + @self._loop.bind + def _(state=SimpleNamespace(pressed_last=self._condition())): + pressed = self._condition() + if state.pressed_last and not 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 RepeatCommand. + + :param command: the command to start + :returns: this trigger, so calls can be chained + """ + + @self._loop.bind + def _(state=SimpleNamespace(pressed_last=self._condition())): + 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 RepeatCommand. + + :param command: the command to start + :returns: this trigger, so calls can be chained + """ + + @self._loop.bind + def _(state=SimpleNamespace(pressed_last=self._condition())): + 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 + """ + + @self._loop.bind + def _(state=SimpleNamespace(pressed_last=self._condition())): + 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 + """ + + @self._loop.bind + def _(state=SimpleNamespace(pressed_last=self._condition())): + 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": + return Trigger(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": + return Trigger(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(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(lambda: debouncer.calculate(self())) diff --git a/commands2/cmd.py b/commands2/cmd.py new file mode 100644 index 00000000..62a2e24c --- /dev/null +++ b/commands2/cmd.py @@ -0,0 +1,191 @@ +from typing import Any, Callable, Dict, Hashable + +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 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, lambda: 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 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: float) -> 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) diff --git a/commands2/cmd/__init__.py b/commands2/cmd/__init__.py deleted file mode 100644 index 1e63dfe1..00000000 --- a/commands2/cmd/__init__.py +++ /dev/null @@ -1,36 +0,0 @@ -# autogenerated by 'robotpy-build create-imports commands2.cmd commands2._impl.cmd' -from .._impl.cmd import ( - deadline, - either, - nothing, - parallel, - print, - race, - repeatingSequence, - run, - runEnd, - runOnce, - select, - sequence, - startEnd, - wait, - waitUntil, -) - -__all__ = [ - "deadline", - "either", - "nothing", - "parallel", - "print", - "race", - "repeatingSequence", - "run", - "runEnd", - "runOnce", - "select", - "sequence", - "startEnd", - "wait", - "waitUntil", -] diff --git a/commands2/command.py b/commands2/command.py new file mode 100644 index 00000000..39afad12 --- /dev/null +++ b/commands2/command.py @@ -0,0 +1,555 @@ +# 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 + +from typing_extensions import Self + +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 .perpetualcommand import PerpetualCommand + 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, #end(boolean) end(true) 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 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. + + This class is provided by the NewCommands VendorDep + """ + + InterruptionBehavior = InterruptionBehavior # type alias for 2023 location + + requirements: Set[Subsystem] + + def __new__(cls, *args, **kwargs) -> Self: + instance = super().__new__( + cls, + ) + instance.requirements = set() + SendableRegistry.add(instance, cls.__name__) + super().__init__(instance) + 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 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 False + + 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 {@link + #andThen(Command...)} instead. + + :param interrupted: whether the command was interrupted/canceled + """ + pass + + 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, {@link + #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 that 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 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 schedule(self, interruptible: bool = True) -> None: + """Schedules this command.""" + from .commandscheduler import CommandScheduler + + CommandScheduler.getInstance().schedule(self) + + def cancel(self) -> None: + """ + Cancels this command. Will call #end(boolean) 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 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 CommandScheduler#removeComposedCommand(Command). 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 CommandScheduler#removeComposedCommand(Command). 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 CommandScheduler#removeComposedCommand(Command). 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 + """ + return self.until(lambda: not condition()) + + def withInterrupt(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 + that this 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 CommandScheduler#removeComposedCommand(Command). 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 + @deprecated Replace with #until(BooleanSupplier) + """ + return self.until(condition) + + def beforeStarting(self, before: Command) -> SequentialCommandGroup: + """ + Decorates this command with another 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 CommandScheduler#removeComposedCommand(Command). 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 .sequentialcommandgroup import SequentialCommandGroup + + 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 CommandScheduler#removeComposedCommand(Command). 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 CommandScheduler#removeComposedCommand(Command). 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 CommandScheduler#removeComposedCommand(Command). 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 CommandScheduler#removeComposedCommand(Command). 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 perpetually(self) -> PerpetualCommand: + """ + Decorates this command to run perpetually, ignoring its ordinary end conditions. The decorated + command can still be interrupted or 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 CommandScheduler#removeComposedCommand(Command). The command composition + returned from this method can be further decorated without issue. + + :returns: the decorated command + @deprecated PerpetualCommand violates the assumption that execute() doesn't get called after + isFinished() returns true -- an assumption that should be valid. This was unsafe/undefined + behavior from the start, and RepeatCommand provides an easy way to achieve similar end + results with slightly different (and safe) semantics. + """ + from .perpetualcommand import PerpetualCommand + + return PerpetualCommand(self) + + 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 CommandScheduler#removeComposedCommand(Command). 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. This is + useful for "forking off" from command compositions when the user does not wish to extend the + command's requirements to the entire command composition. + + :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 CommandScheduler#removeComposedCommand(Command). 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 CommandScheduler#removeComposedCommand(Command). 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 #end(boolean) 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 + #end(boolean) 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 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 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/commands2/commandgroup.py b/commands2/commandgroup.py new file mode 100644 index 00000000..8a00f242 --- /dev/null +++ b/commands2/commandgroup.py @@ -0,0 +1,21 @@ +from __future__ import annotations + +from .command import Command + + +class IllegalCommandUse(Exception): + pass + + +class CommandGroup(Command): + """ + A base for CommandGroups. + """ + + def addCommands(self, *commands: Command): + """ + Adds the given commands to the command group. + + :param commands: The commands to add. + """ + raise NotImplementedError diff --git a/commands2/commandscheduler.py b/commands2/commandscheduler.py new file mode 100644 index 00000000..fa7b41e7 --- /dev/null +++ b/commands2/commandscheduler.py @@ -0,0 +1,475 @@ +from __future__ import annotations + +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 +from wpilib.event import EventLoop + +from .command import Command, InterruptionBehavior +from .commandgroup import * +from .subsystem import Subsystem + + +class CommandScheduler: + """ + The scheduler responsible for running Commands. A Command-based robot should call {@link + CommandScheduler#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 {@link + CommandScheduler#registerSubsystem(Subsystem...)} in order for their Subsystem#periodic() + methods to be called and for their default commands to be scheduled. + """ + + _instance: Optional[Self] = None + + def __new__(cls) -> Self: + 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() + CommandScheduler._instance = None + + def __init__(self) -> None: + """ + Gets the scheduler instance. + """ + if CommandScheduler._instance is not None: + return + CommandScheduler._instance = self + self._composedCommands: Set[Command] = set() + self._scheduledCommands: Dict[Command, None] = {} + self._requirements: Dict[Subsystem, Command] = {} + self._subsystems: Dict[Subsystem, Optional[Command]] = {} + + self._defaultButtonLoop = EventLoop() + self.setActiveButtonLoop(self._defaultButtonLoop) + + self._disabled = False + + self._initActions: List[Callable[[Command], None]] = [] + self._executeActions: List[Callable[[Command], None]] = [] + self._interruptActions: List[Callable[[Command], None]] = [] + self._finishActions: List[Callable[[Command], None]] = [] + + self._inRunLoop = False + self._toSchedule: Dict[Command, None] = {} + self._toCancel: Dict[Command, None] = {} + + self._watchdog = Watchdog(TimedRobot.kDefaultPeriod, lambda: None) + + hal.report( + hal.tResourceType.kResourceType_Command.value, + hal.tInstances.kCommand2_Scheduler.value, + ) + + 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() + + def schedule(self, *commands) -> 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. + + :param commands: the commands to schedule. + """ + if len(commands) > 1: + for command in commands: + self.schedule(command) + return + + command = commands[0] + + if command is None: + # DriverStation.reportWarning("CommandScheduler tried to schedule a null command!", True) + return + + if self._inRunLoop: + self._toSchedule[command] = None + return + + if command in self.getComposedCommands(): + raise IllegalCommandUse( + "A command that is part of a CommandGroup cannot be independently scheduled" + ) + + if self._disabled: + return + + if RobotState.isDisabled() and not command.runsWhenDisabled(): + return + + if self.isScheduled(command): + return + + requirements = command.getRequirements() + + if self._requirements.keys().isdisjoint(requirements): + self.initCommand(command, *requirements) + else: + 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) + + self.initCommand(command, *requirements) + + def run(self): + """ + 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() + + for subsystem in self._subsystems: + subsystem.periodic() + if RobotBase.isSimulation(): + subsystem.simulationPeriodic() + # self._watchdog.addEpoch() + + loopCache = self._activeButtonLoop + loopCache.poll() + self._watchdog.addEpoch("buttons.run()") + + self._inRunLoop = True + + for command in self._scheduledCommands.copy(): + if not command.runsWhenDisabled() and RobotState.isDisabled(): + command.end(True) + for action in self._interruptActions: + action(command) + for requirement in command.getRequirements(): + self._requirements.pop(requirement) + self._scheduledCommands.pop(command) + continue + + command.execute() + for action in self._executeActions: + action(command) + # self._watchdog.addEpoch() + if command.isFinished(): + command.end(False) + for action in self._finishActions: + action(command) + self._scheduledCommands.pop(command) + for requirement in command.getRequirements(): + self._requirements.pop(requirement) + + self._inRunLoop = False + + for command in self._toSchedule: + self.schedule(command) + + for command in self._toCancel: + self.cancel(command) + + self._toSchedule.clear() + self._toCancel.clear() + + for subsystem, command in self._subsystems.items(): + if subsystem not in self._requirements and command is not None: + self.schedule(command) + + 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 in self._subsystems: + # DriverStation.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) + + 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 {@link + 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 + """ + self.requireNotComposed([defaultCommand]) + if subsystem not in defaultCommand.getRequirements(): + raise IllegalCommandUse("Default commands must require their subsystem!") + if ( + defaultCommand.getInterruptionBehavior() + != InterruptionBehavior.kCancelIncoming + ): + # DriverStation.reportWarning("Registering a non-interruptible default command\nThis will likely prevent any other commands from requiring this subsystem.", True) + pass + 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 + """ + 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 Command#end(boolean) method of the + canceled command with {@code 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 + """ + if self._inRunLoop: + for command in commands: + self._toCancel[command] = None + return + + for command in commands: + if not self.isScheduled(command): + continue + + self._scheduledCommands.pop(command) + for requirement in command.getRequirements(): + del self._requirements[requirement] + command.end(True) + for action in self._interruptActions: + action(command) + + 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 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 + """ + 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 + """ + 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 + """ + 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 + """ + 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 + @throws IllegalArgumentException if the given commands have already been composed. + """ + self.requireNotComposed(commands) + self._composedCommands.update(commands) + + 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 removeComposedCommands(self, commands: Iterable[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.difference_update(commands) + + def requireNotComposed(self, commands: Iterable[Command]) -> None: + """ + Requires that the specified command hasn't been already added to a composition. + + :param commands: The commands to check + @throws IllegalArgumentException if the given commands have already been composed. + """ + if not self._composedCommands.isdisjoint(commands): + raise IllegalCommandUse( + "Commands that have been composed may not be added to another composition or scheduled individually" + ) + + 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.getComposedCommands() + + def getComposedCommands(self) -> Set[Command]: + return self._composedCommands diff --git a/commands2/conditionalcommand.py b/commands2/conditionalcommand.py new file mode 100644 index 00000000..b26760de --- /dev/null +++ b/commands2/conditionalcommand.py @@ -0,0 +1,61 @@ +from __future__ import annotations + +from typing import Callable + +from .command import Command +from .commandgroup import * +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. + + This class is provided by the NewCommands VendorDep""" + + selectedCommand: 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): + self.selectedCommand.execute() + + def isFinished(self) -> bool: + return self.selectedCommand.isFinished() + + def end(self, interrupted: bool): + self.selectedCommand.end(interrupted) + + def runsWhenDisabled(self) -> bool: + return self.onTrue.runsWhenDisabled() and self.onFalse.runsWhenDisabled() diff --git a/commands2/functionalcommand.py b/commands2/functionalcommand.py new file mode 100644 index 00000000..9498568e --- /dev/null +++ b/commands2/functionalcommand.py @@ -0,0 +1,52 @@ +from __future__ import annotations + +from typing import Any, Callable + +from .command import Command +from .commandgroup import * +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__() + + 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/commands2/instantcommand.py b/commands2/instantcommand.py new file mode 100644 index 00000000..6d20a2e6 --- /dev/null +++ b/commands2/instantcommand.py @@ -0,0 +1,29 @@ +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 Runnable 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 Runnable with the given requirements. + + :param toRun: the Runnable to run + :param requirements: the subsystems required by this command""" + super().__init__( + toRun or (lambda: None), + lambda: None, + lambda _: None, + lambda: True, + *requirements, + ) diff --git a/commands2/notifiercommand.py b/commands2/notifiercommand.py new file mode 100644 index 00000000..0a6e57e8 --- /dev/null +++ b/commands2/notifiercommand.py @@ -0,0 +1,41 @@ +from __future__ import annotations + +from typing import Any, Callable + +from wpilib import Notifier + +from .command import Command +from .commandgroup import * +from .subsystem import Subsystem + + +class NotifierCommand(Command): + """ + A command that starts a notifier to run the given runnable periodically in a separate thread. Has + no end condition as-is; either subclass it or use Command#withTimeout(double) or {@link + Command#until(java.util.function.BooleanSupplier)} 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: float, *requirements: Subsystem + ): + """ + Creates a new NotifierCommand. + + :param toRun: the runnable 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__() + + 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/commands2/parallelcommandgroup.py b/commands2/parallelcommandgroup.py new file mode 100644 index 00000000..e3ad01dd --- /dev/null +++ b/commands2/parallelcommandgroup.py @@ -0,0 +1,87 @@ +from __future__ import annotations + +from typing import Dict + +from commands2.command import Command, InterruptionBehavior + +from .command import Command, InterruptionBehavior +from .commandgroup import * +from .commandscheduler import CommandScheduler +from .util import flatten_args_commands + + +class ParallelCommandGroup(CommandGroup): + """ + 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): + 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: + if not command.getRequirements().isdisjoint(self.requirements): + raise IllegalCommandUse( + "Multiple comands in a parallel composition cannot require the same subsystems." + ) + + 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/commands2/paralleldeadlinegroup.py b/commands2/paralleldeadlinegroup.py new file mode 100644 index 00000000..eb064f49 --- /dev/null +++ b/commands2/paralleldeadlinegroup.py @@ -0,0 +1,98 @@ +from __future__ import annotations + +from typing import Dict + +from commands2.command import Command, InterruptionBehavior + +from .command import Command, InterruptionBehavior +from .commandgroup import * +from .commandscheduler import CommandScheduler +from .util import flatten_args_commands + + +class ParallelDeadlineGroup(CommandGroup): + """ + 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, deadline: Command, *commands: Command): + """ + 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__() + self._commands: Dict[Command, bool] = {} + self._runsWhenDisabled = True + self._finished = True + self._deadline = deadline + self._interruptBehavior = InterruptionBehavior.kCancelIncoming + self.addCommands(*commands) + if deadline not in self._commands: + self.addCommands(deadline) + + def setDeadline(self, deadline: Command): + if deadline not in self._commands: + self.addCommands(deadline) + self._deadline = deadline + + def addCommands(self, *commands: Command): + 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: + if not command.getRequirements().isdisjoint(self.requirements): + raise IllegalCommandUse( + "Multiple comands in a parallel composition cannot require the same subsystems." + ) + + 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 diff --git a/commands2/parallelracegroup.py b/commands2/parallelracegroup.py new file mode 100644 index 00000000..3b66c14a --- /dev/null +++ b/commands2/parallelracegroup.py @@ -0,0 +1,82 @@ +from __future__ import annotations + +from typing import Set + +from commands2.command import Command, InterruptionBehavior + +from .command import Command, InterruptionBehavior +from .commandgroup import * +from .commandscheduler import CommandScheduler +from .util import flatten_args_commands + + +class ParallelRaceGroup(CommandGroup): + """ + 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._interruptBehavior = InterruptionBehavior.kCancelIncoming + self._finished = True + self.addCommands(*commands) + + def addCommands(self, *commands: Command): + 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: + if not command.getRequirements().isdisjoint(self.requirements): + raise IllegalCommandUse( + "Multiple comands in a parallel composition cannot require the same subsystems." + ) + + 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/commands2/perpetualcommand.py b/commands2/perpetualcommand.py new file mode 100644 index 00000000..1e535e34 --- /dev/null +++ b/commands2/perpetualcommand.py @@ -0,0 +1,46 @@ +from __future__ import annotations + +from .command import Command +from .commandgroup import * +from .commandscheduler import CommandScheduler + + +class PerpetualCommand(Command): + """ + A command that runs another command in perpetuity, ignoring that command's end conditions. While + this class does not extend CommandGroupBase, it is still considered a composition, as it + allows one to compose another command within it; the command instances that are passed to it + cannot be added to any other groups, or scheduled individually. + + As a rule, CommandGroups require the union of the requirements of their component commands. + + This class is provided by the NewCommands VendorDep + + @deprecated PerpetualCommand violates the assumption that execute() doesn't get called after + isFinished() returns true -- an assumption that should be valid. This was unsafe/undefined + behavior from the start, and RepeatCommand provides an easy way to achieve similar end + results with slightly different (and safe) semantics.""" + + def __init__(self, command: Command): + """ + Creates a new PerpetualCommand. Will run another command in perpetuity, ignoring that command's + end conditions, unless this command itself is interrupted. + + :param command: the command to run perpetually""" + super().__init__() + + CommandScheduler.getInstance().registerComposedCommands([command]) + self._command = command + self.addRequirements(*command.getRequirements()) + + def initialize(self): + self._command.initialize() + + def execute(self): + self._command.execute + + def end(self, interrupted: bool): + self._command.end(interrupted) + + def runsWhenDisabled(self) -> bool: + return self._command.runsWhenDisabled() diff --git a/commands2/printcommand.py b/commands2/printcommand.py new file mode 100644 index 00000000..082a8c7f --- /dev/null +++ b/commands2/printcommand.py @@ -0,0 +1,18 @@ +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/commands2/proxycommand.py b/commands2/proxycommand.py new file mode 100644 index 00000000..630e7b52 --- /dev/null +++ b/commands2/proxycommand.py @@ -0,0 +1,90 @@ +from __future__ import annotations + +from typing import Callable, overload + +from .command import Command +from .commandgroup import * +from .util import format_args_kwargs + + +class ProxyCommand(Command): + """ + Schedules the given command when this command is initialized, and ends when it ends. Useful for + forking off from CommandGroups. 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. Useful for lazily creating commands at runtime. + + :param supplier: the command supplier""" + ... + + @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]): + self._supplier = supplier + + def init_command(command: Command): + 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): + if interrupted and self._command is not None: + self._command.cancel() + self._command = None + + def execute(self): + pass + + def isFinished(self) -> bool: + 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 diff --git a/commands2/proxyschedulecommand.py b/commands2/proxyschedulecommand.py new file mode 100644 index 00000000..23b9d8a4 --- /dev/null +++ b/commands2/proxyschedulecommand.py @@ -0,0 +1,41 @@ +from __future__ import annotations + +from .command import Command +from .commandgroup import * + + +class ProxyScheduleCommand(Command): + """ + Schedules the given commands when this command is initialized, and ends when all the commands are + no longer scheduled. Useful for forking off from CommandGroups. If this command is interrupted, + it will cancel all the commands. + """ + + def __init__(self, *toSchedule: Command): + """ + Creates a new ProxyScheduleCommand that schedules the given commands when initialized, and ends + when they are all no longer scheduled. + + :param toSchedule: the commands to schedule + @deprecated Replace with ProxyCommand, composing multiple of them in a {@link + ParallelRaceGroup} if needed.""" + super().__init__() + self.toSchedule = set(toSchedule) + self._finished = False + + def initialize(self): + for command in self.toSchedule: + command.schedule() + + def end(self, interrupted: bool): + if interrupted: + for command in self.toSchedule: + command.cancel() + + def execute(self): + self._finished = True + for command in self.toSchedule: + self._finished = self._finished and not command.isScheduled() + + def isFinished(self) -> bool: + return self._finished diff --git a/commands2/repeatcommand.py b/commands2/repeatcommand.py new file mode 100644 index 00000000..dc7dc991 --- /dev/null +++ b/commands2/repeatcommand.py @@ -0,0 +1,55 @@ +from __future__ import annotations + +from commands2.command import InterruptionBehavior + +from .command import Command +from .commandgroup import * + + +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 + + 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(): + self._command.end(False) + self._ended = True + + def isFinished(self) -> bool: + return False + + def end(self, interrupted: bool): + 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() diff --git a/commands2/runcommand.py b/commands2/runcommand.py new file mode 100644 index 00000000..231567f5 --- /dev/null +++ b/commands2/runcommand.py @@ -0,0 +1,25 @@ +from __future__ import annotations + +from typing import Any, Callable + +from .commandgroup import * +from .functionalcommand import FunctionalCommand +from .subsystem import Subsystem + + +class RunCommand(FunctionalCommand): + """ + A command that runs a Runnable continuously. Has no end condition as-is; either subclass it or + use Command#withTimeout(double) or Command#until(BooleanSupplier) to give it one. + If you only wish to execute a Runnable once, use InstantCommand.""" + + def __init__(self, toRun: Callable[[], Any], *requirements: Subsystem): + """ + Creates a new RunCommand. The Runnable will be run continuously until the command ends. Does + not run when disabled. + + :param toRun: the Runnable to run + :param requirements: the subsystems to require""" + super().__init__( + lambda: None, toRun, lambda interrupted: None, lambda: False, *requirements + ) diff --git a/commands2/schedulecommand.py b/commands2/schedulecommand.py new file mode 100644 index 00000000..1d987e6d --- /dev/null +++ b/commands2/schedulecommand.py @@ -0,0 +1,30 @@ +from __future__ import annotations + +from .command import Command +from .commandgroup import * + + +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/commands2/selectcommand.py b/commands2/selectcommand.py new file mode 100644 index 00000000..56710dcd --- /dev/null +++ b/commands2/selectcommand.py @@ -0,0 +1,71 @@ +from __future__ import annotations + +from typing import Callable, Dict, Hashable + +from commands2.command import InterruptionBehavior + +from .command import Command, InterruptionBehavior +from .commandgroup import * +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__() + + self._commands = commands + self._selector = selector + + CommandScheduler.getInstance().registerComposedCommands(commands.values()) + + self._runsWhenDisabled = True + self._interruptBehavior = InterruptionBehavior.kCancelIncoming + 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): + if self._selector() not in self._commands: + self._selectedCommand = PrintCommand( + "SelectCommand selector value does not correspond to any command!" + ) + return + self._selectedCommand = self._commands[self._selector()] + 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 diff --git a/commands2/sequentialcommandgroup.py b/commands2/sequentialcommandgroup.py new file mode 100644 index 00000000..ef6984d4 --- /dev/null +++ b/commands2/sequentialcommandgroup.py @@ -0,0 +1,90 @@ +from __future__ import annotations + +from typing import List + +from commands2.command import Command, InterruptionBehavior + +from .command import Command, InterruptionBehavior +from .commandgroup import * +from .commandscheduler import CommandScheduler +from .util import flatten_args_commands + + +class SequentialCommandGroup(CommandGroup): + """ + 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._runsWhenDisabled = True + self._interruptBehavior = InterruptionBehavior.kCancelIncoming + self._currentCommandIndex = -1 + self.addCommands(*commands) + + def addCommands(self, *commands: Command): + 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 not interrupted: + return + if not self._commands: + return + if not self._currentCommandIndex > -1: + return + if not self._currentCommandIndex < len(self._commands): + return + + 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 diff --git a/commands2/src/Command.cpp.inl b/commands2/src/Command.cpp.inl deleted file mode 100644 index 8bc1ab1c..00000000 --- a/commands2/src/Command.cpp.inl +++ /dev/null @@ -1,163 +0,0 @@ - -// Decorators taken from Java - -#define DECORATOR_NOTE \ - "\n" \ - "Note: This decorator works by composing this command within a CommandGroup. The command\n" \ - "cannot be used independently after being decorated, or be re-decorated with a different\n" \ - "decorator, unless it is manually cleared from the list of grouped commands with Command.setGrouped(False)\n" \ - "The decorated command can, however, be further decorated without issue\n" - - -cls_Command - .def("andThen", - [](std::shared_ptr self, std::function toRun, std::span> requirements) { - std::vector> temp; - temp.emplace_back(self); - temp.emplace_back( - std::make_shared(std::move(toRun), requirements)); - return SequentialCommandGroup(std::move(temp)); - }, - py::arg("toRun"), py::arg("requirements") = std::span>{}, - "Decorates this command with a runnable to run after the command finishes.\n" - DECORATOR_NOTE) - .def("andThen", - [](std::shared_ptr self, py::args cmds) { - std::vector> commands; - commands.emplace_back(self); - for (auto cmd : cmds) { - auto cmdptr = py::cast>(cmd); - commands.emplace_back(cmdptr); - } - return std::make_shared(std::move(commands)); - }, - "Decorates this command with a set of commands to run after it in sequence. Often more\n" - "convenient/less-verbose than constructing a new :class:`.SequentialCommandGroup` explicitly.\n" - DECORATOR_NOTE) - .def("alongWith", - [](std::shared_ptr self, py::args cmds) { - std::vector> commands; - commands.emplace_back(self); - for (auto cmd : cmds) { - auto cmdptr = py::cast>(cmd); - commands.emplace_back(cmdptr); - } - return std::make_shared(std::move(commands)); - }, - "Decorates this command with a set of commands to run parallel to it, " - "ending when the last\n" - "command ends. Often more convenient/less-verbose than constructing a new\n" - "ParallelCommandGroup explicitly.\n" - DECORATOR_NOTE) - .def("asProxy", - [](std::shared_ptr self) { - return std::make_shared(self); - }, - "Decorates this command to run \"by proxy\" by wrapping it in a\n" - "ProxyScheduleCommand. This is useful for \"forking off\" from command groups\n" - "when the user does not wish to extend the command's requirements to the\n" - "entire command group.\n" - "\n" - ":returns: the decorated command\n" - DECORATOR_NOTE - ) - .def("beforeStarting", - [](std::shared_ptr self, std::function toRun, std::span> requirements) { - std::vector> temp; - temp.emplace_back(std::make_shared(std::move(toRun), requirements)); - temp.emplace_back(self); - return std::make_shared(std::move(temp)); - }, - py::arg("toRun"), py::arg("requirements")=std::span >{}, - "Decorates this command with a runnable to run before this command starts.\n" - "\n" - ":param toRun: the Runnable to run\n" - ":param requirements: the required subsystems\n" - "\n" - ":returns: the decorated command\n" - DECORATOR_NOTE) - .def("deadlineWith", - [](std::shared_ptr self, py::args cmds) { - return std::make_shared( - self, std::move(pyargs2cmdList(cmds))); - }, - "Decorates this command with a set of commands to run parallel to it, ending when the calling\n" - "command ends and interrupting all the others. Often more convenient/less-verbose than\n" - "constructing a new :class:`.ParallelDeadlineGroup` explicitly.\n" - DECORATOR_NOTE) - .def("perpetually", - [](std::shared_ptr self) { - return std::make_shared(self); - }, - "Decorates this command to run perpetually, ignoring its ordinary end\n" - "conditions. The decorated command can still be interrupted or canceled.\n" - "\n" - ":returns: the decorated command\n" - DECORATOR_NOTE) - .def("raceWith", - [](std::shared_ptr self, py::args cmds) { - std::vector> commands; - commands.emplace_back(self); - for (auto cmd : cmds) { - auto cmdptr = py::cast>(cmd); - commands.emplace_back(cmdptr); - } - return std::make_shared(std::move(commands)); - }, - "Decorates this command with a set of commands to run parallel to it, ending when the first\n" - "command ends. Often more convenient/less-verbose than constructing a new\n" - "ParallelRaceGroup explicitly.\n" - DECORATOR_NOTE) - .def("until", - [](std::shared_ptr self, std::function condition) { - std::vector> temp; - temp.emplace_back(std::make_shared(std::move(condition))); - temp.emplace_back(self); - return std::make_shared(std::move(temp)); - }, - py::arg("condition"), - "Decorates this command with an interrupt condition. If the specified\n" - "condition becomes true before the command finishes normally, the command\n" - "will be interrupted and un-scheduled. Note that this only applies to the\n" - "command returned by this method; the calling command is not itself changed.\n" - "\n" - ":param condition: the interrupt condition\n" - "\n" - ":returns: the command with the interrupt condition added\n" - DECORATOR_NOTE) - .def("withInterrupt", - [](std::shared_ptr self, std::function condition) { - std::vector> temp; - temp.emplace_back(std::make_shared(std::move(condition))); - temp.emplace_back(self); - return std::make_shared(std::move(temp)); - }, - py::arg("condition"), - "Decorates this command with an interrupt condition. If the specified\n" - "condition becomes true before the command finishes normally, the command\n" - "will be interrupted and un-scheduled. Note that this only applies to the\n" - "command returned by this method; the calling command is not itself changed.\n" - "\n" - ":param condition: the interrupt condition\n" - "\n" - ":returns: the command with the interrupt condition added\n" - DECORATOR_NOTE) - .def("withTimeout", - [](std::shared_ptr self, units::second_t duration) { - std::vector> temp; - temp.emplace_back(std::make_shared(duration)); - temp.emplace_back(self); - return std::make_shared(std::move(temp)); - }, - py::arg("duration"), - "Decorates this command with a timeout. If the specified timeout is\n" - "exceeded before the command finishes normally, the command will be\n" - "interrupted and un-scheduled. Note that the timeout only applies to the\n" - "command returned by this method; the calling command is not itself changed.\n" - "\n" - ":param duration: the timeout duration\n" - "\n" - ":returns: the command with the timeout added\n" - DECORATOR_NOTE) - ; - diff --git a/commands2/src/SelectCommandKey.h b/commands2/src/SelectCommandKey.h deleted file mode 100644 index fd6ee30e..00000000 --- a/commands2/src/SelectCommandKey.h +++ /dev/null @@ -1,68 +0,0 @@ -#pragma once - -#include - -// this assumes that the __hash__ of the python object isn't going to change -// once added to the map of the SelectCommand. This is... probably reasonable? -struct SelectCommandKey { - - SelectCommandKey() = default; - ~SelectCommandKey() { - py::gil_scoped_acquire gil; - m_v.release().dec_ref(); - } - - SelectCommandKey(const SelectCommandKey& other) { - py::gil_scoped_acquire gil; - m_v = other.m_v; - m_hash = py::hash(m_v); - } - - SelectCommandKey &operator=(const SelectCommandKey& other) { - py::gil_scoped_acquire gil; - m_v = other.m_v; - m_hash = py::hash(m_v); - return *this; - } - - SelectCommandKey &operator=(const py::handle src) { - py::gil_scoped_acquire gil; - m_v = py::reinterpret_borrow(src); - m_hash = py::hash(m_v); - return *this; - } - - operator py::object() const { return m_v; } - - py::object m_v; - std::size_t m_hash; -}; - -inline bool operator==(const SelectCommandKey &lhs, - const SelectCommandKey &rhs) { - py::gil_scoped_acquire gil; - return lhs.m_v == rhs.m_v; -} - -template <> struct std::hash { - std::size_t operator()(const SelectCommandKey &s) const noexcept { - return s.m_hash; - } -}; - -namespace pybind11 { -namespace detail { -template <> struct type_caster { - PYBIND11_TYPE_CASTER(SelectCommandKey, const_name("object")); - bool load(handle src, bool) { - value = src; - return true; - } - - static handle cast(const SelectCommandKey &src, - return_value_policy /* policy */, handle /* parent */) { - return src.m_v; - } -}; -} // namespace detail -} // namespace pybind11 diff --git a/commands2/src/TimedCommandRobot.h b/commands2/src/TimedCommandRobot.h deleted file mode 100644 index 0b2d756b..00000000 --- a/commands2/src/TimedCommandRobot.h +++ /dev/null @@ -1,27 +0,0 @@ - -#pragma once - -#include -#include - -namespace frc2 { - -/** - * TimedCommandRobot implements the IterativeRobotBase robot program framework. - * - * The TimedCommandRobot class is intended to be subclassed by a user creating a - * command-based robot program. This python-specific class calls the - * CommandScheduler run method in robotPeriodic for you. - */ -class TimedCommandRobot : public frc::TimedRobot { -public: - - TimedCommandRobot(units::second_t period = frc::TimedRobot::kDefaultPeriod) : - TimedRobot(period) - {} - - /** Ensures commands are run */ - void RobotPeriodic() override { CommandScheduler::GetInstance().Run(); } -}; - -} // namespace frc2 \ No newline at end of file diff --git a/commands2/src/cpp/frc2/command/Command.cpp b/commands2/src/cpp/frc2/command/Command.cpp deleted file mode 100644 index 77a8b4fa..00000000 --- a/commands2/src/cpp/frc2/command/Command.cpp +++ /dev/null @@ -1,197 +0,0 @@ -// 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. - -#include "frc2/command/Command.h" - -#include "frc2/command/CommandHelper.h" -#include "frc2/command/CommandScheduler.h" -#include "frc2/command/ConditionalCommand.h" -#include "frc2/command/InstantCommand.h" -#include "frc2/command/ParallelCommandGroup.h" -#include "frc2/command/ParallelDeadlineGroup.h" -#include "frc2/command/ParallelRaceGroup.h" -#include "frc2/command/PerpetualCommand.h" -#include "frc2/command/RepeatCommand.h" -#include "frc2/command/SequentialCommandGroup.h" -#include "frc2/command/WaitCommand.h" -#include "frc2/command/WaitUntilCommand.h" -#include "frc2/command/WrapperCommand.h" - -#include - -using namespace frc2; - -Command::~Command() { - // CommandScheduler::GetInstance().Cancel(this); -} - -// Command& Command::operator=(const Command& rhs) { -// m_isComposed = false; -// return *this; -// } - -void Command::Initialize() {} -void Command::Execute() {} -void Command::End(bool interrupted) {} - -/* -CommandPtr Command::WithTimeout(units::second_t duration) && { - return std::move(*this).ToPtr().WithTimeout(duration); -} -*/ - -/* -CommandPtr Command::Until(std::function condition) && { - return std::move(*this).ToPtr().Until(std::move(condition)); -} -*/ - -/* -CommandPtr Command::IgnoringDisable(bool doesRunWhenDisabled) && { - return std::move(*this).ToPtr().IgnoringDisable(doesRunWhenDisabled); -} -*/ - -/* -CommandPtr Command::WithInterruptBehavior( - InterruptionBehavior interruptBehavior) && { - return std::move(*this).ToPtr().WithInterruptBehavior(interruptBehavior); -} -*/ - -/* -CommandPtr Command::WithInterrupt(std::function condition) && { - return std::move(*this).ToPtr().Until(std::move(condition)); -} -*/ - -/* -CommandPtr Command::BeforeStarting( - std::function toRun, - std::initializer_list requirements) && { - return std::move(*this).BeforeStarting( - std::move(toRun), {requirements.begin(), requirements.end()}); -} -*/ - -/* -CommandPtr Command::BeforeStarting( - std::function toRun, std::span requirements) && { - return std::move(*this).ToPtr().BeforeStarting(std::move(toRun), - requirements); -} -*/ - -/* -CommandPtr Command::AndThen(std::function toRun, - std::initializer_list requirements) && { - return std::move(*this).AndThen(std::move(toRun), - {requirements.begin(), requirements.end()}); -} -*/ - -/* -CommandPtr Command::AndThen(std::function toRun, - std::span requirements) && { - return std::move(*this).ToPtr().AndThen(std::move(toRun), requirements); -} -*/ - -/* -PerpetualCommand Command::Perpetually() && { - WPI_IGNORE_DEPRECATED - return PerpetualCommand(std::move(*this).TransferOwnership()); - WPI_UNIGNORE_DEPRECATED -} -*/ - -/* -CommandPtr Command::Repeatedly() && { - return std::move(*this).ToPtr().Repeatedly(); -} -*/ - -/* -CommandPtr Command::AsProxy() && { - return std::move(*this).ToPtr().AsProxy(); -} -*/ - -/* -CommandPtr Command::Unless(std::function condition) && { - return std::move(*this).ToPtr().Unless(std::move(condition)); -} -*/ - -/* -CommandPtr Command::FinallyDo(std::function end) && { - return std::move(*this).ToPtr().FinallyDo(std::move(end)); -} -*/ - -/* -CommandPtr Command::HandleInterrupt(std::function handler) && { - return std::move(*this).ToPtr().HandleInterrupt(std::move(handler)); -} -*/ - -/* -CommandPtr Command::WithName(std::string_view name) && { - return std::move(*this).ToPtr().WithName(name); -} -*/ - -void frc2::Command_Schedule(std::shared_ptr self) { - CommandScheduler::GetInstance().Schedule(self); -} - -void Command::Cancel() { - CommandScheduler::GetInstance().Cancel(this); -} - -bool Command::IsScheduled() { - return CommandScheduler::GetInstance().IsScheduled(this); -} - -bool Command::HasRequirement(Subsystem* requirement) const { - bool hasRequirement = false; - for (auto&& subsystem : GetRequirements()) { - hasRequirement |= requirement == subsystem.get(); - } - return hasRequirement; -} - -std::string Command::GetName() const { - return GetTypeName(this); -} - -void Command::SetName(std::string_view name) {} - -bool Command::IsComposed() const { - return m_isComposed; -} - -void Command::SetComposed(bool isComposed) { - m_isComposed = isComposed; -} - -bool Command::IsGrouped() const { - return IsComposed(); -} - -void Command::SetGrouped(bool grouped) { - SetComposed(grouped); -} - -namespace frc2 { -bool RequirementsDisjoint(Command* first, Command* second) { - bool disjoint = true; - auto&& requirements = second->GetRequirements(); - for (auto&& requirement : first->GetRequirements()) { - // disjoint &= requirements.count(requirement) == requirements.end(); - disjoint &= requirements.count(requirement) == 0; - } - return disjoint; -} -} // namespace frc2 diff --git a/commands2/src/cpp/frc2/command/CommandBase.cpp b/commands2/src/cpp/frc2/command/CommandBase.cpp deleted file mode 100644 index f88db82e..00000000 --- a/commands2/src/cpp/frc2/command/CommandBase.cpp +++ /dev/null @@ -1,91 +0,0 @@ -// 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. - -#include "frc2/command/CommandBase.h" - -#include -#include - -#include - -using namespace frc2; - -CommandBase::CommandBase() { - wpi::SendableRegistry::Add(this, GetTypeName(*this)); -} - -void CommandBase::AddRequirements( - std::initializer_list> requirements) { - m_requirements.insert(requirements.begin(), requirements.end()); -} - -void CommandBase::AddRequirements(std::span> requirements) { - m_requirements.insert(requirements.begin(), requirements.end()); -} - -void CommandBase::AddRequirements(wpi::SmallSet, 4> requirements) { - m_requirements.insert(requirements.begin(), requirements.end()); -} - -void CommandBase::AddRequirements(std::shared_ptr requirement) { - m_requirements.insert(requirement); -} - -wpi::SmallSet, 4> CommandBase::GetRequirements() const { - return m_requirements; -} - -void CommandBase::SetName(std::string_view name) { - wpi::SendableRegistry::SetName(this, name); -} - -std::string CommandBase::GetName() const { - return wpi::SendableRegistry::GetName(this); -} - -std::string CommandBase::GetSubsystem() const { - return wpi::SendableRegistry::GetSubsystem(this); -} - -void CommandBase::SetSubsystem(std::string_view subsystem) { - wpi::SendableRegistry::SetSubsystem(this, subsystem); -} - -void CommandBase::InitSendable(wpi::SendableBuilder& builder) { - builder.SetSmartDashboardType("Command"); - builder.AddStringProperty( - ".name", [this] { return GetName(); }, nullptr); - builder.AddBooleanProperty( - "running", [this] { return IsScheduled(); }, - [this](bool value) { - std::shared_ptr hack_ptr = convertToSharedPtrHack(this); - if (!hack_ptr) { - return; - } - - bool isScheduled = IsScheduled(); - if (value && !isScheduled) { - Command_Schedule(hack_ptr); - } else if (!value && isScheduled) { - Cancel(); - } - }); - builder.AddBooleanProperty( - ".isParented", [this] { return IsComposed(); }, nullptr); - builder.AddStringProperty( - "interruptBehavior", - [this] { - switch (GetInterruptionBehavior()) { - case Command::InterruptionBehavior::kCancelIncoming: - return "kCancelIncoming"; - case Command::InterruptionBehavior::kCancelSelf: - return "kCancelSelf"; - default: - return "Invalid"; - } - }, - nullptr); - builder.AddBooleanProperty( - "runsWhenDisabled", [this] { return RunsWhenDisabled(); }, nullptr); -} diff --git a/commands2/src/cpp/frc2/command/CommandGroupBase.cpp b/commands2/src/cpp/frc2/command/CommandGroupBase.cpp deleted file mode 100644 index eb9c293e..00000000 --- a/commands2/src/cpp/frc2/command/CommandGroupBase.cpp +++ /dev/null @@ -1,7 +0,0 @@ -// 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. - -#include "frc2/command/CommandGroupBase.h" - -using namespace frc2; diff --git a/commands2/src/cpp/frc2/command/CommandPtr.cpp b/commands2/src/cpp/frc2/command/CommandPtr.cpp deleted file mode 100644 index 1bb8bf08..00000000 --- a/commands2/src/cpp/frc2/command/CommandPtr.cpp +++ /dev/null @@ -1,270 +0,0 @@ -// 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. - -#include "frc2/command/CommandPtr.h" - -#include - -#include "frc2/command/CommandScheduler.h" -#include "frc2/command/ConditionalCommand.h" -#include "frc2/command/InstantCommand.h" -#include "frc2/command/ParallelCommandGroup.h" -#include "frc2/command/ParallelDeadlineGroup.h" -#include "frc2/command/ParallelRaceGroup.h" -#include "frc2/command/PrintCommand.h" -#include "frc2/command/ProxyCommand.h" -#include "frc2/command/RepeatCommand.h" -#include "frc2/command/SequentialCommandGroup.h" -#include "frc2/command/WaitCommand.h" -#include "frc2/command/WaitUntilCommand.h" -#include "frc2/command/WrapperCommand.h" - -using namespace frc2; - -void CommandPtr::AssertValid() const { - if (!m_ptr) { - throw FRC_MakeError(frc::err::CommandIllegalUse, - "Moved-from CommandPtr object used!"); - } -} - -CommandPtr CommandPtr::Repeatedly() && { - AssertValid(); - m_ptr = std::make_unique(std::move(m_ptr)); - return std::move(*this); -} - -CommandPtr CommandPtr::AsProxy() && { - AssertValid(); - m_ptr = std::make_unique(std::move(m_ptr)); - return std::move(*this); -} - -class RunsWhenDisabledCommand : public WrapperCommand { - public: - RunsWhenDisabledCommand(std::unique_ptr&& command, - bool doesRunWhenDisabled) - : WrapperCommand(std::move(command)), - m_runsWhenDisabled(doesRunWhenDisabled) {} - - bool RunsWhenDisabled() const override { return m_runsWhenDisabled; } - - private: - bool m_runsWhenDisabled; -}; - -CommandPtr CommandPtr::IgnoringDisable(bool doesRunWhenDisabled) && { - AssertValid(); - m_ptr = std::make_unique(std::move(m_ptr), - doesRunWhenDisabled); - return std::move(*this); -} - -using InterruptionBehavior = Command::InterruptionBehavior; -class InterruptBehaviorCommand : public WrapperCommand { - public: - InterruptBehaviorCommand(std::unique_ptr&& command, - InterruptionBehavior interruptBehavior) - : WrapperCommand(std::move(command)), - m_interruptBehavior(interruptBehavior) {} - - InterruptionBehavior GetInterruptionBehavior() const override { - return m_interruptBehavior; - } - - private: - InterruptionBehavior m_interruptBehavior; -}; - -CommandPtr CommandPtr::WithInterruptBehavior( - InterruptionBehavior interruptBehavior) && { - AssertValid(); - m_ptr = std::make_unique(std::move(m_ptr), - interruptBehavior); - return std::move(*this); -} - -CommandPtr CommandPtr::AndThen(std::function toRun, - std::span requirements) && { - AssertValid(); - return std::move(*this).AndThen(CommandPtr( - std::make_unique(std::move(toRun), requirements))); -} - -CommandPtr CommandPtr::AndThen( - std::function toRun, - std::initializer_list requirements) && { - AssertValid(); - return std::move(*this).AndThen(CommandPtr( - std::make_unique(std::move(toRun), requirements))); -} - -CommandPtr CommandPtr::AndThen(CommandPtr&& next) && { - AssertValid(); - std::vector> temp; - temp.emplace_back(std::move(m_ptr)); - temp.emplace_back(std::move(next).Unwrap()); - m_ptr = std::make_unique(std::move(temp)); - return std::move(*this); -} - -CommandPtr CommandPtr::BeforeStarting( - std::function toRun, std::span requirements) && { - AssertValid(); - return std::move(*this).BeforeStarting(CommandPtr( - std::make_unique(std::move(toRun), requirements))); -} - -CommandPtr CommandPtr::BeforeStarting( - std::function toRun, - std::initializer_list requirements) && { - AssertValid(); - return std::move(*this).BeforeStarting(CommandPtr( - std::make_unique(std::move(toRun), requirements))); -} - -CommandPtr CommandPtr::BeforeStarting(CommandPtr&& before) && { - AssertValid(); - std::vector> temp; - temp.emplace_back(std::move(before).Unwrap()); - temp.emplace_back(std::move(m_ptr)); - m_ptr = std::make_unique(std::move(temp)); - return std::move(*this); -} - -CommandPtr CommandPtr::WithTimeout(units::second_t duration) && { - AssertValid(); - std::vector> temp; - temp.emplace_back(std::make_unique(duration)); - temp.emplace_back(std::move(m_ptr)); - m_ptr = std::make_unique(std::move(temp)); - return std::move(*this); -} - -CommandPtr CommandPtr::Until(std::function condition) && { - AssertValid(); - std::vector> temp; - temp.emplace_back(std::make_unique(std::move(condition))); - temp.emplace_back(std::move(m_ptr)); - m_ptr = std::make_unique(std::move(temp)); - return std::move(*this); -} - -CommandPtr CommandPtr::Unless(std::function condition) && { - AssertValid(); - m_ptr = std::make_unique( - std::make_unique(), std::move(m_ptr), - std::move(condition)); - return std::move(*this); -} - -CommandPtr CommandPtr::DeadlineWith(CommandPtr&& parallel) && { - AssertValid(); - std::vector> vec; - vec.emplace_back(std::move(parallel).Unwrap()); - m_ptr = - std::make_unique(std::move(m_ptr), std::move(vec)); - return std::move(*this); -} - -CommandPtr CommandPtr::AlongWith(CommandPtr&& parallel) && { - AssertValid(); - std::vector> vec; - vec.emplace_back(std::move(m_ptr)); - vec.emplace_back(std::move(parallel).Unwrap()); - m_ptr = std::make_unique(std::move(vec)); - return std::move(*this); -} - -CommandPtr CommandPtr::RaceWith(CommandPtr&& parallel) && { - AssertValid(); - std::vector> vec; - vec.emplace_back(std::move(m_ptr)); - vec.emplace_back(std::move(parallel).Unwrap()); - m_ptr = std::make_unique(std::move(vec)); - return std::move(*this); -} - -namespace { -class FinallyCommand : public WrapperCommand { - public: - FinallyCommand(std::unique_ptr&& command, - std::function end) - : WrapperCommand(std::move(command)), m_end(std::move(end)) {} - - void End(bool interrupted) override { - WrapperCommand::End(interrupted); - m_end(interrupted); - } - - private: - std::function m_end; -}; -} // namespace - -CommandPtr CommandPtr::FinallyDo(std::function end) && { - AssertValid(); - m_ptr = std::make_unique(std::move(m_ptr), std::move(end)); - return std::move(*this); -} - -CommandPtr CommandPtr::HandleInterrupt(std::function handler) && { - AssertValid(); - return std::move(*this).FinallyDo( - [handler = std::move(handler)](bool interrupted) { - if (interrupted) { - handler(); - } - }); -} - -CommandPtr CommandPtr::WithName(std::string_view name) && { - AssertValid(); - WrapperCommand wrapper{std::move(m_ptr)}; - wrapper.SetName(name); - return std::move(wrapper).ToPtr(); -} - -CommandBase* CommandPtr::get() const& { - AssertValid(); - return m_ptr.get(); -} - -std::unique_ptr CommandPtr::Unwrap() && { - AssertValid(); - return std::move(m_ptr); -} - -void CommandPtr::Schedule() const& { - AssertValid(); - CommandScheduler::GetInstance().Schedule(*this); -} - -void CommandPtr::Cancel() const& { - AssertValid(); - CommandScheduler::GetInstance().Cancel(*this); -} - -bool CommandPtr::IsScheduled() const& { - AssertValid(); - return CommandScheduler::GetInstance().IsScheduled(*this); -} - -bool CommandPtr::HasRequirement(Subsystem* requirement) const& { - AssertValid(); - return m_ptr->HasRequirement(requirement); -} - -CommandPtr::operator bool() const& { - return m_ptr.operator bool(); -} - -std::vector> CommandPtr::UnwrapVector( - std::vector&& vec) { - std::vector> ptrs; - for (auto&& ptr : vec) { - ptrs.emplace_back(std::move(ptr).Unwrap()); - } - return ptrs; -} diff --git a/commands2/src/cpp/frc2/command/CommandScheduler.cpp b/commands2/src/cpp/frc2/command/CommandScheduler.cpp deleted file mode 100644 index 628f4da5..00000000 --- a/commands2/src/cpp/frc2/command/CommandScheduler.cpp +++ /dev/null @@ -1,544 +0,0 @@ -// 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. - -#include "frc2/command/CommandScheduler.h" - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "frc2/command/DenseMapInfo.h" - -#include "frc2/command/CommandGroupBase.h" -#include "frc2/command/CommandPtr.h" -#include "frc2/command/Subsystem.h" - -using namespace frc2; - -class CommandScheduler::Impl { - public: - // A set of the currently-running commands. - wpi::DenseMap, bool> scheduledCommands; - - // A map from required subsystems to their requiring commands. Also used as a - // set of the currently-required subsystems. - wpi::DenseMap, std::shared_ptr> requirements; - - // A map from subsystems registered with the scheduler to their default - // commands. Also used as a list of currently-registered subsystems. - wpi::DenseMap> subsystems; - - frc::EventLoop defaultButtonLoop; - // The set of currently-registered buttons that will be polled every - // iteration. - frc::EventLoop* activeButtonLoop{&defaultButtonLoop}; - - bool disabled{false}; - - // Lists of user-supplied actions to be executed on scheduling events for - // every command. - wpi::SmallVector initActions; - wpi::SmallVector executeActions; - wpi::SmallVector interruptActions; - wpi::SmallVector finishActions; - - // Flag and queues for avoiding concurrent modification if commands are - // scheduled/canceled during run - - bool inRunLoop = false; - wpi::SmallVector, 4> toSchedule; - wpi::SmallVector, 4> toCancel; -}; - -template -static bool ContainsKey(const TMap& map, TKey keyToCheck) { - return map.find(keyToCheck) != map.end(); -} - -CommandScheduler::CommandScheduler() - : m_impl(new Impl), m_watchdog(frc::TimedRobot::kDefaultPeriod, [] { - std::puts("CommandScheduler loop time overrun."); - }) { - HAL_Report(HALUsageReporting::kResourceType_Command, - HALUsageReporting::kCommand2_Scheduler); - wpi::SendableRegistry::AddLW(this, "Scheduler"); - frc::LiveWindow::SetEnabledCallback([this] { - this->Disable(); - this->CancelAll(); - }); - frc::LiveWindow::SetDisabledCallback([this] { this->Enable(); }); -} - -CommandScheduler::~CommandScheduler() { - wpi::SendableRegistry::Remove(this); - frc::LiveWindow::SetEnabledCallback(nullptr); - frc::LiveWindow::SetDisabledCallback(nullptr); - - std::unique_ptr().swap(m_impl); -} - -CommandScheduler& CommandScheduler::GetInstance() { - static CommandScheduler scheduler; - return scheduler; -} - -void CommandScheduler::ResetInstance() { - CommandScheduler& instance = GetInstance(); - std::make_unique().swap(instance.m_impl); -} - -void CommandScheduler::SetPeriod(units::second_t period) { - m_watchdog.SetTimeout(period); -} - -frc::EventLoop* CommandScheduler::GetActiveButtonLoop() const { - return m_impl->activeButtonLoop; -} - -void CommandScheduler::SetActiveButtonLoop(frc::EventLoop* loop) { - m_impl->activeButtonLoop = loop; -} - -frc::EventLoop* CommandScheduler::GetDefaultButtonLoop() const { - return &(m_impl->defaultButtonLoop); -} - -void CommandScheduler::ClearButtons() { - m_impl->activeButtonLoop->Clear(); -} - -void CommandScheduler::Schedule(std::shared_ptr command) { - if (m_impl->inRunLoop) { - m_impl->toSchedule.emplace_back(command); - return; - } - - RequireUngrouped(command); - - if (m_impl->disabled || ContainsKey(m_impl->scheduledCommands, command)|| - (frc::RobotState::IsDisabled() && !command->RunsWhenDisabled())) { - return; - } - - const auto& requirements = command->GetRequirements(); - - wpi::SmallVector, 8> intersection; - - bool isDisjoint = true; - bool allInterruptible = true; - for (auto&& i1 : m_impl->requirements) { - // if (requirements.find(i1.first) != requirements.end()) { - if (requirements.count(i1.first) != 0) { - isDisjoint = false; - allInterruptible &= (i1.second->GetInterruptionBehavior() == - Command::InterruptionBehavior::kCancelSelf); - intersection.emplace_back(i1.second); - } - } - - if (isDisjoint || allInterruptible) { - if (allInterruptible) { - for (auto&& cmdToCancel : intersection) { - Cancel(cmdToCancel); - } - } - m_impl->scheduledCommands[command] = true; - for (auto&& requirement : requirements) { - m_impl->requirements[requirement] = command; - } - command->Initialize(); - for (auto&& action : m_impl->initActions) { - action(command); - } - m_watchdog.AddEpoch(command->GetName() + ".Initialize()"); - } -} - -void CommandScheduler::Schedule(std::span> commands) { - for (auto command : commands) { - Schedule(command); - } -} - -void CommandScheduler::Schedule(std::initializer_list> commands) { - for (auto command : commands) { - Schedule(command); - } -} - -/* -void CommandScheduler::Schedule(const CommandPtr& command) { - Schedule(command.get()); -} -*/ - -void CommandScheduler::Run() { - if (m_impl->disabled) { - return; - } - - m_watchdog.Reset(); - - // Run the periodic method of all registered subsystems. - for (auto&& subsystem : m_impl->subsystems) { - subsystem.getFirst()->Periodic(); - if constexpr (frc::RobotBase::IsSimulation()) { - subsystem.getFirst()->SimulationPeriodic(); - } - m_watchdog.AddEpoch("Subsystem Periodic()"); - } - - // Cache the active instance to avoid concurrency problems if SetActiveLoop() - // is called from inside the button bindings. - frc::EventLoop* loopCache = m_impl->activeButtonLoop; - // Poll buttons for new commands to add. - loopCache->Poll(); - m_watchdog.AddEpoch("buttons.Run()"); - - m_impl->inRunLoop = true; - // Run scheduled commands, remove finished commands. - for (auto&& iter : m_impl->scheduledCommands) { - auto command = iter.first; - if (!command->RunsWhenDisabled() && frc::RobotState::IsDisabled()) { - Cancel(command); - continue; - } - - command->Execute(); - for (auto&& action : m_impl->executeActions) { - action(command); - } - m_watchdog.AddEpoch(command->GetName() + ".Execute()"); - - if (command->IsFinished()) { - command->End(false); - for (auto&& action : m_impl->finishActions) { - action(command); - } - - for (auto&& requirement : command->GetRequirements()) { - m_impl->requirements.erase(requirement); - } - - m_impl->scheduledCommands.erase(command); - m_watchdog.AddEpoch(command->GetName() + ".End(false)"); - } - } - m_impl->inRunLoop = false; - - for (auto&& command : m_impl->toSchedule) { - Schedule(command); - } - - for (auto&& command : m_impl->toCancel) { - Cancel(command); - } - - m_impl->toSchedule.clear(); - m_impl->toCancel.clear(); - - // Add default commands for un-required registered subsystems. - for (auto&& subsystem : m_impl->subsystems) { - auto s = m_impl->requirements.find_as(subsystem.getFirst()); - if (s == m_impl->requirements.end() && subsystem.getSecond()) { - Schedule(subsystem.getSecond()); - } - } - - m_watchdog.Disable(); - if (m_watchdog.IsExpired()) { - m_watchdog.PrintEpochs(); - } -} - -void CommandScheduler::RegisterSubsystem(Subsystem* subsystem) { - if (m_impl->subsystems.find(subsystem) != m_impl->subsystems.end()) { - std::puts("Tried to register an already-registered subsystem"); - return; - } - - m_impl->subsystems[subsystem] = nullptr; -} - -void CommandScheduler::UnregisterSubsystem(Subsystem* subsystem) { - auto s = m_impl->subsystems.find(subsystem); - if (s != m_impl->subsystems.end()) { - m_impl->subsystems.erase(s); - } -} - -void CommandScheduler::RegisterSubsystem( - std::initializer_list subsystems) { - for (auto* subsystem : subsystems) { - RegisterSubsystem(subsystem); - } -} - -void CommandScheduler::RegisterSubsystem( - std::span subsystems) { - for (auto* subsystem : subsystems) { - RegisterSubsystem(subsystem); - } -} - -void CommandScheduler::UnregisterSubsystem( - std::initializer_list subsystems) { - for (auto* subsystem : subsystems) { - UnregisterSubsystem(subsystem); - } -} - -void CommandScheduler::UnregisterSubsystem( - std::span subsystems) { - for (auto* subsystem : subsystems) { - UnregisterSubsystem(subsystem); - } -} - -/* -void CommandScheduler::SetDefaultCommand(Subsystem* subsystem, - CommandPtr&& defaultCommand) { - if (!defaultCommand.get()->HasRequirement(subsystem)) { - throw FRC_MakeError(frc::err::CommandIllegalUse, "{}", - "Default commands must require their subsystem!"); - } - RequireUngrouped(defaultCommand.get()); - - SetDefaultCommandImpl(subsystem, std::move(defaultCommand).Unwrap()); -} -*/ - -void CommandScheduler::RemoveDefaultCommand(Subsystem* subsystem) { - m_impl->subsystems[subsystem] = std::shared_ptr(); -} - -std::shared_ptr CommandScheduler::GetDefaultCommand(const Subsystem* subsystem) const { - auto&& find = m_impl->subsystems.find(subsystem); - if (find != m_impl->subsystems.end()) { - return find->second; - } else { - return nullptr; - } -} - -void CommandScheduler::Cancel(std::shared_ptr command) { - if (!m_impl) { - return; - } - - if (m_impl->inRunLoop) { - m_impl->toCancel.emplace_back(command); - return; - } - - auto find = m_impl->scheduledCommands.find(command); - if (find == m_impl->scheduledCommands.end()) { - return; - } - m_impl->scheduledCommands.erase(find); - for (auto&& requirement : m_impl->requirements) { - if (requirement.second == command) { - m_impl->requirements.erase(requirement.first); - } - } - command->End(true); - for (auto&& action : m_impl->interruptActions) { - action(command); - } - m_watchdog.AddEpoch(command->GetName() + ".End(true)"); -} - -void CommandScheduler::Cancel(Command *command) { - if (!m_impl) { - return; - } - auto found = m_impl->scheduledCommands.find_as(command); - if (found == m_impl->scheduledCommands.end()) { - return; - } - - Cancel(found->first); -} - -/* -void CommandScheduler::Cancel(const CommandPtr& command) { - Cancel(command.get()); -} -*/ - -void CommandScheduler::Cancel(std::span> commands) { - for (auto command : commands) { - Cancel(command); - } -} - -void CommandScheduler::Cancel(std::initializer_list> commands) { - for (auto command : commands) { - Cancel(command); - } -} - -void CommandScheduler::CancelAll() { - wpi::SmallVector, 16> commands; - for (auto&& command : m_impl->scheduledCommands) { - commands.emplace_back(command.first); - } - Cancel(commands); -} - -bool CommandScheduler::IsScheduled( - std::span> commands) const { - for (auto command : commands) { - if (!IsScheduled(command)) { - return false; - } - } - return true; -} - -bool CommandScheduler::IsScheduled( - std::initializer_list> commands) const { - for (auto command : commands) { - if (!IsScheduled(command)) { - return false; - } - } - return true; -} - -bool CommandScheduler::IsScheduled(std::shared_ptr command) const { - return ContainsKey(m_impl->scheduledCommands, command); -} - -bool CommandScheduler::IsScheduled(const Command* command) const { - return m_impl->scheduledCommands.find_as(command) != - m_impl->scheduledCommands.end(); -} - -/* -bool CommandScheduler::IsScheduled(const CommandPtr& command) const { - return m_impl->scheduledCommands.contains(command.get()); -}*/ - -std::shared_ptr CommandScheduler::Requiring(const std::shared_ptr subsystem) const { - auto find = m_impl->requirements.find(subsystem); - if (find != m_impl->requirements.end()) { - return find->second; - } else { - return nullptr; - } -} - -std::shared_ptr CommandScheduler::Requiring(const Subsystem* subsystem) const { - auto find = m_impl->requirements.find_as(subsystem); - if (find != m_impl->requirements.end()) { - return find->second; - } else { - return nullptr; - } -} - -void CommandScheduler::Disable() { - m_impl->disabled = true; -} - -void CommandScheduler::Enable() { - m_impl->disabled = false; -} - -void CommandScheduler::OnCommandInitialize(Action action) { - m_impl->initActions.emplace_back(std::move(action)); -} - -void CommandScheduler::OnCommandExecute(Action action) { - m_impl->executeActions.emplace_back(std::move(action)); -} - -void CommandScheduler::OnCommandInterrupt(Action action) { - m_impl->interruptActions.emplace_back(std::move(action)); -} - -void CommandScheduler::OnCommandFinish(Action action) { - m_impl->finishActions.emplace_back(std::move(action)); -} - -void CommandScheduler::RequireUngrouped(const std::shared_ptr command) { - if (command->IsComposed()) { - throw FRC_MakeError(frc::err::CommandIllegalUse, - "Commands that have been composed may not be added to " - "another composition or scheduled " - "individually!"); - } -} - -void CommandScheduler::RequireUngrouped( - std::span> commands) { - for (auto&& command : commands) { - RequireUngrouped(command); - } -} - -void CommandScheduler::RequireUngrouped( - std::initializer_list> commands) { - for (auto&& command : commands) { - RequireUngrouped(command); - } -} - -void CommandScheduler::InitSendable(nt::NTSendableBuilder& builder) { - builder.SetSmartDashboardType("Scheduler"); - builder.SetUpdateTable( - [this, - namesPub = nt::StringArrayTopic{builder.GetTopic("Names")}.Publish(), - idsPub = nt::IntegerArrayTopic{builder.GetTopic("Ids")}.Publish(), - cancelEntry = nt::IntegerArrayTopic{builder.GetTopic("Cancel")}.GetEntry( - {})]() mutable { - auto toCancel = cancelEntry.Get(); - if (!toCancel.empty()) { - for (auto cancel : cancelEntry.Get()) { - uintptr_t ptrTmp = static_cast(cancel); - Command* command = reinterpret_cast(ptrTmp); - if (m_impl->scheduledCommands.find_as(command) != - m_impl->scheduledCommands.end()) { - Cancel(command); - } - } - cancelEntry.Set({}); - } - - wpi::SmallVector names; - wpi::SmallVector ids; - for (auto &&command : m_impl->scheduledCommands) { - names.emplace_back(command.first->GetName()); - uintptr_t ptrTmp = reinterpret_cast(command.first.get()); - ids.emplace_back(static_cast(ptrTmp)); - } - namesPub.Set(names); - idsPub.Set(ids); - }); -} - -void CommandScheduler::SetDefaultCommandImpl(Subsystem* subsystem, - std::shared_ptr command) { - if (command->GetInterruptionBehavior() == - Command::InterruptionBehavior::kCancelIncoming) { - std::puts( - "Registering a non-interruptible default command!\n" - "This will likely prevent any other commands from " - "requiring this subsystem."); - // Warn, but allow -- there might be a use case for this. - } - m_impl->subsystems[subsystem] = std::move(command); -} diff --git a/commands2/src/cpp/frc2/command/Commands.cpp b/commands2/src/cpp/frc2/command/Commands.cpp deleted file mode 100644 index 521be7b3..00000000 --- a/commands2/src/cpp/frc2/command/Commands.cpp +++ /dev/null @@ -1,117 +0,0 @@ -// 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. - -#include "frc2/command/Commands.h" - -#include "frc2/command/ConditionalCommand.h" -#include "frc2/command/FunctionalCommand.h" -#include "frc2/command/InstantCommand.h" -#include "frc2/command/ParallelCommandGroup.h" -#include "frc2/command/ParallelDeadlineGroup.h" -#include "frc2/command/ParallelRaceGroup.h" -#include "frc2/command/PrintCommand.h" -#include "frc2/command/ProxyScheduleCommand.h" -#include "frc2/command/RepeatCommand.h" -#include "frc2/command/RunCommand.h" -#include "frc2/command/SequentialCommandGroup.h" -#include "frc2/command/WaitCommand.h" -#include "frc2/command/WaitUntilCommand.h" - -using namespace frc2; - -// Factories - -std::shared_ptr cmd::None() { - return std::make_shared(); -} - -std::shared_ptr cmd::RunOnce(std::function action, - std::initializer_list> requirements) { - return std::make_shared(std::move(action), requirements); -} - -std::shared_ptr cmd::RunOnce(std::function action, - std::span> requirements) { - return std::make_shared(std::move(action), requirements); -} - -std::shared_ptr cmd::Run(std::function action, - std::initializer_list> requirements) { - return std::make_shared(std::move(action), requirements); -} - -std::shared_ptr cmd::Run(std::function action, - std::span> requirements) { - return std::make_shared(std::move(action), requirements); -} - -std::shared_ptr cmd::StartEnd(std::function start, std::function end, - std::initializer_list> requirements) { - return std::make_shared( - std::move(start), [] {}, - [end = std::move(end)](bool interrupted) { end(); }, - [] { return false; }, requirements); -} - -std::shared_ptr cmd::StartEnd(std::function start, std::function end, - std::span> requirements) { - return std::make_shared( - std::move(start), [] {}, - [end = std::move(end)](bool interrupted) { end(); }, - [] { return false; }, requirements); -} - -std::shared_ptr cmd::RunEnd(std::function run, std::function end, - std::initializer_list> requirements) { - return std::make_shared([] {}, std::move(run), - [end = std::move(end)](bool interrupted) { end(); }, - [] { return false; }, requirements); -} - -std::shared_ptr cmd::RunEnd(std::function run, std::function end, - std::span> requirements) { - return std::make_shared([] {}, std::move(run), - [end = std::move(end)](bool interrupted) { end(); }, - [] { return false; }, requirements); -} - -std::shared_ptr cmd::Print(std::string_view msg) { - return std::make_shared(msg); -} - -std::shared_ptr cmd::Wait(units::second_t duration) { - return std::make_shared(duration); -} - -std::shared_ptr cmd::WaitUntil(std::function condition) { - return std::make_shared(condition); -} - -std::shared_ptr cmd::Either(std::shared_ptr onTrue, std::shared_ptr onFalse, - std::function selector) { - return std::make_shared(std::move(onTrue), - std::move(onFalse), std::move(selector)); -} - -std::shared_ptr cmd::Sequence(std::vector >&& commands) { - return std::make_shared(std::move(commands)); -} - -std::shared_ptr cmd::RepeatingSequence(std::vector >&& commands) { - return std::make_shared(Sequence(std::move(commands))); -} - -std::shared_ptr cmd::Parallel(std::vector >&& commands) { - return std::make_shared(std::move(commands)); -} - -std::shared_ptr cmd::Race(std::vector >&& commands) { - return std::make_shared(std::move(commands)); -} - -std::shared_ptr cmd::Deadline(std::shared_ptr deadline, - std::vector >&& others) { - return std::make_shared(std::move(deadline), - std::move(others)); -} diff --git a/commands2/src/cpp/frc2/command/ConditionalCommand.cpp b/commands2/src/cpp/frc2/command/ConditionalCommand.cpp deleted file mode 100644 index 16670267..00000000 --- a/commands2/src/cpp/frc2/command/ConditionalCommand.cpp +++ /dev/null @@ -1,72 +0,0 @@ -// 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. - -#include "frc2/command/ConditionalCommand.h" - -#include - -using namespace frc2; - -ConditionalCommand::ConditionalCommand(std::shared_ptr onTrue, - std::shared_ptr onFalse, - std::function condition) - : m_condition{std::move(condition)} { - CommandScheduler::GetInstance().RequireUngrouped( - {onTrue, onFalse}); - - m_onTrue = std::move(onTrue); - m_onFalse = std::move(onFalse); - - m_onTrue->SetComposed(true); - m_onFalse->SetComposed(true); - - m_runsWhenDisabled &= m_onTrue->RunsWhenDisabled(); - m_runsWhenDisabled &= m_onFalse->RunsWhenDisabled(); - - AddRequirements(m_onTrue->GetRequirements()); - AddRequirements(m_onFalse->GetRequirements()); -} - -void ConditionalCommand::Initialize() { - if (m_condition()) { - m_selectedCommand = m_onTrue; - } else { - m_selectedCommand = m_onFalse; - } - m_selectedCommand->Initialize(); -} - -void ConditionalCommand::Execute() { - m_selectedCommand->Execute(); -} - -void ConditionalCommand::End(bool interrupted) { - m_selectedCommand->End(interrupted); -} - -bool ConditionalCommand::IsFinished() { - return m_selectedCommand->IsFinished(); -} - -bool ConditionalCommand::RunsWhenDisabled() const { - return m_runsWhenDisabled; -} - -void ConditionalCommand::InitSendable(wpi::SendableBuilder& builder) { - CommandBase::InitSendable(builder); - builder.AddStringProperty( - "onTrue", [this] { return m_onTrue->GetName(); }, nullptr); - builder.AddStringProperty( - "onFalse", [this] { return m_onFalse->GetName(); }, nullptr); - builder.AddStringProperty( - "selected", - [this] { - if (m_selectedCommand) { - return m_selectedCommand->GetName(); - } else { - return std::string{"null"}; - } - }, - nullptr); -} diff --git a/commands2/src/cpp/frc2/command/FunctionalCommand.cpp b/commands2/src/cpp/frc2/command/FunctionalCommand.cpp deleted file mode 100644 index c1810d74..00000000 --- a/commands2/src/cpp/frc2/command/FunctionalCommand.cpp +++ /dev/null @@ -1,46 +0,0 @@ -// 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. - -#include "frc2/command/FunctionalCommand.h" - -using namespace frc2; - -FunctionalCommand::FunctionalCommand( - std::function onInit, std::function onExecute, - std::function onEnd, std::function isFinished, - std::initializer_list> requirements) - : m_onInit{std::move(onInit)}, - m_onExecute{std::move(onExecute)}, - m_onEnd{std::move(onEnd)}, - m_isFinished{std::move(isFinished)} { - AddRequirements(requirements); -} - -FunctionalCommand::FunctionalCommand(std::function onInit, - std::function onExecute, - std::function onEnd, - std::function isFinished, - std::span> requirements) - : m_onInit{std::move(onInit)}, - m_onExecute{std::move(onExecute)}, - m_onEnd{std::move(onEnd)}, - m_isFinished{std::move(isFinished)} { - AddRequirements(requirements); -} - -void FunctionalCommand::Initialize() { - m_onInit(); -} - -void FunctionalCommand::Execute() { - m_onExecute(); -} - -void FunctionalCommand::End(bool interrupted) { - m_onEnd(interrupted); -} - -bool FunctionalCommand::IsFinished() { - return m_isFinished(); -} diff --git a/commands2/src/cpp/frc2/command/InstantCommand.cpp b/commands2/src/cpp/frc2/command/InstantCommand.cpp deleted file mode 100644 index b9f434de..00000000 --- a/commands2/src/cpp/frc2/command/InstantCommand.cpp +++ /dev/null @@ -1,21 +0,0 @@ -// 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. - -#include "frc2/command/InstantCommand.h" - -using namespace frc2; - -InstantCommand::InstantCommand(std::function toRun, - std::initializer_list> requirements) - : FunctionalCommand( - std::move(toRun), [] {}, [](bool interrupted) {}, [] { return true; }, - requirements) {} - -InstantCommand::InstantCommand(std::function toRun, - std::span> requirements) - : FunctionalCommand( - std::move(toRun), [] {}, [](bool interrupted) {}, [] { return true; }, - requirements) {} - -InstantCommand::InstantCommand() : InstantCommand([] {}) {} diff --git a/commands2/src/cpp/frc2/command/MecanumControllerCommand.cpp b/commands2/src/cpp/frc2/command/MecanumControllerCommand.cpp deleted file mode 100644 index b666dbf6..00000000 --- a/commands2/src/cpp/frc2/command/MecanumControllerCommand.cpp +++ /dev/null @@ -1,337 +0,0 @@ -// 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. - -#include "frc2/command/MecanumControllerCommand.h" - -#include - -using namespace frc2; - -MecanumControllerCommand::MecanumControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SimpleMotorFeedforward feedforward, - frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, - frc2::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function desiredRotation, - units::meters_per_second_t maxWheelVelocity, - std::function currentWheelSpeeds, - frc2::PIDController frontLeftController, - frc2::PIDController rearLeftController, - frc2::PIDController frontRightController, - frc2::PIDController rearRightController, - std::function - output, - std::initializer_list> requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_feedforward(feedforward), - m_kinematics(kinematics), - m_controller(xController, yController, thetaController), - m_desiredRotation(std::move(desiredRotation)), - m_maxWheelVelocity(maxWheelVelocity), - m_frontLeftController( - std::make_unique(frontLeftController)), - m_rearLeftController( - std::make_unique(rearLeftController)), - m_frontRightController( - std::make_unique(frontRightController)), - m_rearRightController( - std::make_unique(rearRightController)), - m_currentWheelSpeeds(std::move(currentWheelSpeeds)), - m_outputVolts(std::move(output)), - m_usePID(true) { - AddRequirements(requirements); -} - -MecanumControllerCommand::MecanumControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SimpleMotorFeedforward feedforward, - frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, - frc2::PIDController yController, - frc::ProfiledPIDController thetaController, - units::meters_per_second_t maxWheelVelocity, - std::function currentWheelSpeeds, - frc2::PIDController frontLeftController, - frc2::PIDController rearLeftController, - frc2::PIDController frontRightController, - frc2::PIDController rearRightController, - std::function - output, - std::initializer_list> requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_feedforward(feedforward), - m_kinematics(kinematics), - m_controller(xController, yController, thetaController), - m_maxWheelVelocity(maxWheelVelocity), - m_frontLeftController( - std::make_unique(frontLeftController)), - m_rearLeftController( - std::make_unique(rearLeftController)), - m_frontRightController( - std::make_unique(frontRightController)), - m_rearRightController( - std::make_unique(rearRightController)), - m_currentWheelSpeeds(std::move(currentWheelSpeeds)), - m_outputVolts(std::move(output)), - m_usePID(true) { - AddRequirements(requirements); -} - -MecanumControllerCommand::MecanumControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SimpleMotorFeedforward feedforward, - frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, - frc2::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function desiredRotation, - units::meters_per_second_t maxWheelVelocity, - std::function currentWheelSpeeds, - frc2::PIDController frontLeftController, - frc2::PIDController rearLeftController, - frc2::PIDController frontRightController, - frc2::PIDController rearRightController, - std::function - output, - std::span> requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_feedforward(feedforward), - m_kinematics(kinematics), - m_controller(xController, yController, thetaController), - m_desiredRotation(std::move(desiredRotation)), - m_maxWheelVelocity(maxWheelVelocity), - m_frontLeftController( - std::make_unique(frontLeftController)), - m_rearLeftController( - std::make_unique(rearLeftController)), - m_frontRightController( - std::make_unique(frontRightController)), - m_rearRightController( - std::make_unique(rearRightController)), - m_currentWheelSpeeds(std::move(currentWheelSpeeds)), - m_outputVolts(std::move(output)), - m_usePID(true) { - AddRequirements(requirements); -} - -MecanumControllerCommand::MecanumControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SimpleMotorFeedforward feedforward, - frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, - frc2::PIDController yController, - frc::ProfiledPIDController thetaController, - units::meters_per_second_t maxWheelVelocity, - std::function currentWheelSpeeds, - frc2::PIDController frontLeftController, - frc2::PIDController rearLeftController, - frc2::PIDController frontRightController, - frc2::PIDController rearRightController, - std::function - output, - std::span> requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_feedforward(feedforward), - m_kinematics(kinematics), - m_controller(xController, yController, thetaController), - m_maxWheelVelocity(maxWheelVelocity), - m_frontLeftController( - std::make_unique(frontLeftController)), - m_rearLeftController( - std::make_unique(rearLeftController)), - m_frontRightController( - std::make_unique(frontRightController)), - m_rearRightController( - std::make_unique(rearRightController)), - m_currentWheelSpeeds(std::move(currentWheelSpeeds)), - m_outputVolts(std::move(output)), - m_usePID(true) { - AddRequirements(requirements); -} - -MecanumControllerCommand::MecanumControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, - frc2::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function desiredRotation, - units::meters_per_second_t maxWheelVelocity, - std::function - output, - std::initializer_list> requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_kinematics(kinematics), - m_controller(xController, yController, thetaController), - m_desiredRotation(std::move(desiredRotation)), - m_maxWheelVelocity(maxWheelVelocity), - m_outputVel(std::move(output)), - m_usePID(false) { - AddRequirements(requirements); -} - -MecanumControllerCommand::MecanumControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, - frc2::PIDController yController, - frc::ProfiledPIDController thetaController, - units::meters_per_second_t maxWheelVelocity, - std::function - output, - std::initializer_list> requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_kinematics(kinematics), - m_controller(xController, yController, thetaController), - m_maxWheelVelocity(maxWheelVelocity), - m_outputVel(std::move(output)), - m_usePID(false) { - AddRequirements(requirements); -} - -MecanumControllerCommand::MecanumControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, - frc2::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function desiredRotation, - units::meters_per_second_t maxWheelVelocity, - std::function - output, - std::span> requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_kinematics(kinematics), - m_controller(xController, yController, thetaController), - m_desiredRotation(std::move(desiredRotation)), - m_maxWheelVelocity(maxWheelVelocity), - m_outputVel(std::move(output)), - m_usePID(false) { - AddRequirements(requirements); -} - -MecanumControllerCommand::MecanumControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, - frc2::PIDController yController, - frc::ProfiledPIDController thetaController, - units::meters_per_second_t maxWheelVelocity, - std::function - output, - std::span> requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_kinematics(kinematics), - m_controller(xController, yController, thetaController), - m_maxWheelVelocity(maxWheelVelocity), - m_outputVel(std::move(output)), - m_usePID(false) { - AddRequirements(requirements); -} - -void MecanumControllerCommand::Initialize() { - if (m_desiredRotation == nullptr) { - m_desiredRotation = [&] { - return m_trajectory.States().back().pose.Rotation(); - }; - } - m_prevTime = 0_s; - auto initialState = m_trajectory.Sample(0_s); - - auto initialXVelocity = - initialState.velocity * initialState.pose.Rotation().Cos(); - auto initialYVelocity = - initialState.velocity * initialState.pose.Rotation().Sin(); - - m_prevSpeeds = m_kinematics.ToWheelSpeeds( - frc::ChassisSpeeds{initialXVelocity, initialYVelocity, 0_rad_per_s}); - - m_timer.Restart(); - if (m_usePID) { - m_frontLeftController->Reset(); - m_rearLeftController->Reset(); - m_frontRightController->Reset(); - m_rearRightController->Reset(); - } -} - -void MecanumControllerCommand::Execute() { - auto curTime = m_timer.Get(); - auto dt = curTime - m_prevTime; - - auto m_desiredState = m_trajectory.Sample(curTime); - - auto targetChassisSpeeds = - m_controller.Calculate(m_pose(), m_desiredState, m_desiredRotation()); - auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(targetChassisSpeeds); - - targetWheelSpeeds.Desaturate(m_maxWheelVelocity); - - auto frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeft; - auto rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeft; - auto frontRightSpeedSetpoint = targetWheelSpeeds.frontRight; - auto rearRightSpeedSetpoint = targetWheelSpeeds.rearRight; - - if (m_usePID) { - auto frontLeftFeedforward = m_feedforward.Calculate( - frontLeftSpeedSetpoint, - (frontLeftSpeedSetpoint - m_prevSpeeds.frontLeft) / dt); - - auto rearLeftFeedforward = m_feedforward.Calculate( - rearLeftSpeedSetpoint, - (rearLeftSpeedSetpoint - m_prevSpeeds.rearLeft) / dt); - - auto frontRightFeedforward = m_feedforward.Calculate( - frontRightSpeedSetpoint, - (frontRightSpeedSetpoint - m_prevSpeeds.frontRight) / dt); - - auto rearRightFeedforward = m_feedforward.Calculate( - rearRightSpeedSetpoint, - (rearRightSpeedSetpoint - m_prevSpeeds.rearRight) / dt); - - auto frontLeftOutput = units::volt_t{m_frontLeftController->Calculate( - m_currentWheelSpeeds().frontLeft.value(), - frontLeftSpeedSetpoint.value())} + - frontLeftFeedforward; - auto rearLeftOutput = units::volt_t{m_rearLeftController->Calculate( - m_currentWheelSpeeds().rearLeft.value(), - rearLeftSpeedSetpoint.value())} + - rearLeftFeedforward; - auto frontRightOutput = units::volt_t{m_frontRightController->Calculate( - m_currentWheelSpeeds().frontRight.value(), - frontRightSpeedSetpoint.value())} + - frontRightFeedforward; - auto rearRightOutput = units::volt_t{m_rearRightController->Calculate( - m_currentWheelSpeeds().rearRight.value(), - rearRightSpeedSetpoint.value())} + - rearRightFeedforward; - - m_outputVolts(frontLeftOutput, rearLeftOutput, frontRightOutput, - rearRightOutput); - } else { - m_outputVel(frontLeftSpeedSetpoint, rearLeftSpeedSetpoint, - frontRightSpeedSetpoint, rearRightSpeedSetpoint); - - m_prevTime = curTime; - m_prevSpeeds = targetWheelSpeeds; - } -} - -void MecanumControllerCommand::End(bool interrupted) { - m_timer.Stop(); -} - -bool MecanumControllerCommand::IsFinished() { - return m_timer.HasElapsed(m_trajectory.TotalTime()); -} diff --git a/commands2/src/cpp/frc2/command/NotifierCommand.cpp b/commands2/src/cpp/frc2/command/NotifierCommand.cpp deleted file mode 100644 index aa883699..00000000 --- a/commands2/src/cpp/frc2/command/NotifierCommand.cpp +++ /dev/null @@ -1,41 +0,0 @@ -// 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. - -#include "frc2/command/NotifierCommand.h" - -using namespace frc2; - -NotifierCommand::NotifierCommand(std::function toRun, - units::second_t period, - std::initializer_list> requirements) - : m_toRun(toRun), m_notifier{std::move(toRun)}, m_period{period} { - AddRequirements(requirements); -} - -NotifierCommand::NotifierCommand(std::function toRun, - units::second_t period, - std::span> requirements) - : m_toRun(toRun), m_notifier{std::move(toRun)}, m_period{period} { - AddRequirements(requirements); -} - -NotifierCommand::NotifierCommand(NotifierCommand&& other) - : CommandBase(std::move(other)), - m_toRun(other.m_toRun), - m_notifier(other.m_toRun), - m_period(other.m_period) {} - -NotifierCommand::NotifierCommand(const NotifierCommand& other) - : CommandBase(other), - m_toRun(other.m_toRun), - m_notifier(frc::Notifier(other.m_toRun)), - m_period(other.m_period) {} - -void NotifierCommand::Initialize() { - m_notifier.StartPeriodic(m_period); -} - -void NotifierCommand::End(bool interrupted) { - m_notifier.Stop(); -} diff --git a/commands2/src/cpp/frc2/command/PIDCommand.cpp b/commands2/src/cpp/frc2/command/PIDCommand.cpp deleted file mode 100644 index 4bfaa8fd..00000000 --- a/commands2/src/cpp/frc2/command/PIDCommand.cpp +++ /dev/null @@ -1,65 +0,0 @@ -// 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. - -#include "frc2/command/PIDCommand.h" - -#include - -using namespace frc2; - -PIDCommand::PIDCommand(PIDController controller, - std::function measurementSource, - std::function setpointSource, - std::function useOutput, - std::initializer_list> requirements) - : m_controller{std::move(controller)}, - m_measurement{std::move(measurementSource)}, - m_setpoint{std::move(setpointSource)}, - m_useOutput{std::move(useOutput)} { - AddRequirements(requirements); -} - -PIDCommand::PIDCommand(PIDController controller, - std::function measurementSource, - std::function setpointSource, - std::function useOutput, - std::span> requirements) - : m_controller{std::move(controller)}, - m_measurement{std::move(measurementSource)}, - m_setpoint{std::move(setpointSource)}, - m_useOutput{std::move(useOutput)} { - AddRequirements(requirements); -} - -PIDCommand::PIDCommand(PIDController controller, - std::function measurementSource, - double setpoint, std::function useOutput, - std::initializer_list> requirements) - : PIDCommand( - controller, measurementSource, [setpoint] { return setpoint; }, - useOutput, requirements) {} - -PIDCommand::PIDCommand(PIDController controller, - std::function measurementSource, - double setpoint, std::function useOutput, - std::span> requirements) - : PIDCommand( - controller, measurementSource, [setpoint] { return setpoint; }, - useOutput, requirements) {} - -void PIDCommand::Initialize() { - m_controller.Reset(); -} - -void PIDCommand::Execute() { - m_useOutput(m_controller.Calculate(m_measurement(), m_setpoint())); -} - -void PIDCommand::End(bool interrupted) { - m_useOutput(0); -} - -PIDController& PIDCommand::GetController() { - return m_controller; -} diff --git a/commands2/src/cpp/frc2/command/PIDSubsystem.cpp b/commands2/src/cpp/frc2/command/PIDSubsystem.cpp deleted file mode 100644 index 8db032a7..00000000 --- a/commands2/src/cpp/frc2/command/PIDSubsystem.cpp +++ /dev/null @@ -1,47 +0,0 @@ -// 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. - -#include "frc2/command/PIDSubsystem.h" - -#include - -using namespace frc2; - -PIDSubsystem::PIDSubsystem(PIDController controller, double initialPosition) - : m_controller{std::move(controller)} { - SetSetpoint(initialPosition); - AddChild("PID Controller", &m_controller); -} - -void PIDSubsystem::Periodic() { - if (m_enabled) { - UseOutput(m_controller.Calculate(GetMeasurement()), GetSetpoint()); - } -} - -void PIDSubsystem::SetSetpoint(double setpoint) { - m_controller.SetSetpoint(setpoint); -} - -double PIDSubsystem::GetSetpoint() const { - return m_controller.GetSetpoint(); -} - -void PIDSubsystem::Enable() { - m_controller.Reset(); - m_enabled = true; -} - -void PIDSubsystem::Disable() { - UseOutput(0, 0); - m_enabled = false; -} - -bool PIDSubsystem::IsEnabled() { - return m_enabled; -} - -PIDController& PIDSubsystem::GetController() { - return m_controller; -} diff --git a/commands2/src/cpp/frc2/command/ParallelCommandGroup.cpp b/commands2/src/cpp/frc2/command/ParallelCommandGroup.cpp deleted file mode 100644 index 6538c6f6..00000000 --- a/commands2/src/cpp/frc2/command/ParallelCommandGroup.cpp +++ /dev/null @@ -1,90 +0,0 @@ -// 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. - -#include "frc2/command/ParallelCommandGroup.h" - -using namespace frc2; - -ParallelCommandGroup::ParallelCommandGroup( - std::vector>&& commands) { - AddCommands(std::move(commands)); -} - -void ParallelCommandGroup::Initialize() { - for (auto& commandRunning : m_commands) { - commandRunning.first->Initialize(); - commandRunning.second = true; - } - isRunning = true; -} - -void ParallelCommandGroup::Execute() { - for (auto& commandRunning : m_commands) { - if (!commandRunning.second) { - continue; - } - commandRunning.first->Execute(); - if (commandRunning.first->IsFinished()) { - commandRunning.first->End(false); - commandRunning.second = false; - } - } -} - -void ParallelCommandGroup::End(bool interrupted) { - if (interrupted) { - for (auto& commandRunning : m_commands) { - if (commandRunning.second) { - commandRunning.first->End(true); - } - } - } - isRunning = false; -} - -bool ParallelCommandGroup::IsFinished() { - for (auto& command : m_commands) { - if (command.second) { - return false; - } - } - return true; -} - -bool ParallelCommandGroup::RunsWhenDisabled() const { - return m_runWhenDisabled; -} - -Command::InterruptionBehavior ParallelCommandGroup::GetInterruptionBehavior() - const { - return m_interruptBehavior; -} - -void ParallelCommandGroup::AddCommands( - std::vector>&& commands) { - CommandScheduler::GetInstance().RequireUngrouped(commands); - - if (isRunning) { - throw FRC_MakeError(frc::err::CommandIllegalUse, - "Commands cannot be added to a CommandGroup " - "while the group is running"); - } - - for (auto&& command : commands) { - if (RequirementsDisjoint(this, command.get())) { - command->SetComposed(true); - AddRequirements(command->GetRequirements()); - m_runWhenDisabled &= command->RunsWhenDisabled(); - if (command->GetInterruptionBehavior() == - Command::InterruptionBehavior::kCancelSelf) { - m_interruptBehavior = Command::InterruptionBehavior::kCancelSelf; - } - m_commands.emplace_back(std::move(command), false); - } else { - throw FRC_MakeError(frc::err::CommandIllegalUse, - "Multiple commands in a parallel group cannot " - "require the same subsystems"); - } - } -} diff --git a/commands2/src/cpp/frc2/command/ParallelDeadlineGroup.cpp b/commands2/src/cpp/frc2/command/ParallelDeadlineGroup.cpp deleted file mode 100644 index 18cd9968..00000000 --- a/commands2/src/cpp/frc2/command/ParallelDeadlineGroup.cpp +++ /dev/null @@ -1,104 +0,0 @@ -// 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. - -#include "frc2/command/ParallelDeadlineGroup.h" - -#include - -using namespace frc2; - -ParallelDeadlineGroup::ParallelDeadlineGroup( - std::shared_ptr deadline, - std::vector>&& commands) { - SetDeadline(std::move(deadline)); - AddCommands(std::move(commands)); -} - -void ParallelDeadlineGroup::Initialize() { - for (auto& commandRunning : m_commands) { - commandRunning.first->Initialize(); - commandRunning.second = true; - } - m_finished = false; -} - -void ParallelDeadlineGroup::Execute() { - for (auto& commandRunning : m_commands) { - if (!commandRunning.second) { - continue; - } - commandRunning.first->Execute(); - if (commandRunning.first->IsFinished()) { - commandRunning.first->End(false); - commandRunning.second = false; - if (commandRunning.first == m_deadline) { - m_finished = true; - } - } - } -} - -void ParallelDeadlineGroup::End(bool interrupted) { - for (auto& commandRunning : m_commands) { - if (commandRunning.second) { - commandRunning.first->End(true); - } - } -} - -bool ParallelDeadlineGroup::IsFinished() { - return m_finished; -} - -bool ParallelDeadlineGroup::RunsWhenDisabled() const { - return m_runWhenDisabled; -} - -Command::InterruptionBehavior ParallelDeadlineGroup::GetInterruptionBehavior() - const { - return m_interruptBehavior; -} - -void ParallelDeadlineGroup::AddCommands( - std::vector>&& commands) { - CommandScheduler::GetInstance().RequireUngrouped(commands); - - if (!m_finished) { - throw FRC_MakeError(frc::err::CommandIllegalUse, - "Commands cannot be added to a CommandGroup " - "while the group is running"); - } - - for (auto&& command : commands) { - if (RequirementsDisjoint(this, command.get())) { - command->SetComposed(true); - AddRequirements(command->GetRequirements()); - m_runWhenDisabled &= command->RunsWhenDisabled(); - if (command->GetInterruptionBehavior() == - Command::InterruptionBehavior::kCancelSelf) { - m_interruptBehavior = Command::InterruptionBehavior::kCancelSelf; - } - m_commands.emplace_back(std::move(command), false); - } else { - throw FRC_MakeError(frc::err::CommandIllegalUse, - "Multiple commands in a parallel group cannot " - "require the same subsystems"); - } - } -} - -void ParallelDeadlineGroup::SetDeadline(std::shared_ptr deadline) { - m_deadline = deadline; - m_deadline->SetComposed(true); - m_commands.emplace_back(std::move(deadline), false); - AddRequirements(m_deadline->GetRequirements()); - m_runWhenDisabled &= m_deadline->RunsWhenDisabled(); -} - -void ParallelDeadlineGroup::InitSendable(wpi::SendableBuilder& builder) { - CommandBase::InitSendable(builder); - - builder.AddStringProperty( - "deadline", [this] { return m_deadline->GetName(); }, nullptr); -} diff --git a/commands2/src/cpp/frc2/command/ParallelRaceGroup.cpp b/commands2/src/cpp/frc2/command/ParallelRaceGroup.cpp deleted file mode 100644 index 8d0f036a..00000000 --- a/commands2/src/cpp/frc2/command/ParallelRaceGroup.cpp +++ /dev/null @@ -1,77 +0,0 @@ -// 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. - -#include "frc2/command/ParallelRaceGroup.h" - -using namespace frc2; - -ParallelRaceGroup::ParallelRaceGroup( - std::vector>&& commands) { - AddCommands(std::move(commands)); -} - -void ParallelRaceGroup::Initialize() { - m_finished = false; - for (auto& commandRunning : m_commands) { - commandRunning->Initialize(); - } - isRunning = true; -} - -void ParallelRaceGroup::Execute() { - for (auto& commandRunning : m_commands) { - commandRunning->Execute(); - if (commandRunning->IsFinished()) { - m_finished = true; - } - } -} - -void ParallelRaceGroup::End(bool interrupted) { - for (auto& commandRunning : m_commands) { - commandRunning->End(!commandRunning->IsFinished()); - } - isRunning = false; -} - -bool ParallelRaceGroup::IsFinished() { - return m_finished; -} - -bool ParallelRaceGroup::RunsWhenDisabled() const { - return m_runWhenDisabled; -} - -Command::InterruptionBehavior ParallelRaceGroup::GetInterruptionBehavior() - const { - return m_interruptBehavior; -} - -void ParallelRaceGroup::AddCommands( - std::vector>&& commands) { - CommandScheduler::GetInstance().RequireUngrouped(commands); - - if (isRunning) { - throw FRC_MakeError(frc::err::CommandIllegalUse, - "Commands cannot be added to a CommandGroup " - "while the group is running"); - } - - for (auto&& command : commands) { - if (RequirementsDisjoint(this, command.get())) { - command->SetComposed(true); - AddRequirements(command->GetRequirements()); - m_runWhenDisabled &= command->RunsWhenDisabled(); - if (command->GetInterruptionBehavior() == - Command::InterruptionBehavior::kCancelSelf) { - m_interruptBehavior = Command::InterruptionBehavior::kCancelSelf; - } - m_commands.emplace_back(std::move(command)); - } else { - throw FRC_MakeError(frc::err::CommandIllegalUse, - "Multiple commands in a parallel group cannot " - "require the same subsystems"); - } - } -} diff --git a/commands2/src/cpp/frc2/command/PerpetualCommand.cpp b/commands2/src/cpp/frc2/command/PerpetualCommand.cpp deleted file mode 100644 index 1ef6d38f..00000000 --- a/commands2/src/cpp/frc2/command/PerpetualCommand.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// 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. - -#include "frc2/command/PerpetualCommand.h" - -using namespace frc2; - -PerpetualCommand::PerpetualCommand(std::shared_ptr command) { - CommandScheduler::GetInstance().RequireUngrouped(command); - m_command = std::move(command); - m_command->SetComposed(true); - AddRequirements(m_command->GetRequirements()); -} - -void PerpetualCommand::Initialize() { - m_command->Initialize(); -} - -void PerpetualCommand::Execute() { - m_command->Execute(); -} - -void PerpetualCommand::End(bool interrupted) { - m_command->End(interrupted); -} diff --git a/commands2/src/cpp/frc2/command/PrintCommand.cpp b/commands2/src/cpp/frc2/command/PrintCommand.cpp deleted file mode 100644 index 6c6d92a8..00000000 --- a/commands2/src/cpp/frc2/command/PrintCommand.cpp +++ /dev/null @@ -1,17 +0,0 @@ -// 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. - -#include "frc2/command/PrintCommand.h" - -#include - -using namespace frc2; - -PrintCommand::PrintCommand(std::string_view message) - : InstantCommand{[str = std::string(message)] { fmt::print("{}\n", str); }, - {}} {} - -bool PrintCommand::RunsWhenDisabled() const { - return true; -} diff --git a/commands2/src/cpp/frc2/command/ProxyCommand.cpp b/commands2/src/cpp/frc2/command/ProxyCommand.cpp deleted file mode 100644 index 8a064aef..00000000 --- a/commands2/src/cpp/frc2/command/ProxyCommand.cpp +++ /dev/null @@ -1,57 +0,0 @@ -// 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. - -#include "frc2/command/ProxyCommand.h" - -#include - -using namespace frc2; - -ProxyCommand::ProxyCommand(std::function()> supplier) - : m_supplier(std::move(supplier)) {} - -ProxyCommand::ProxyCommand(std::shared_ptr command) - : m_supplier([command] { return command; }) { - SetName(std::string{"Proxy("}.append(command->GetName()).append(")")); -} - -/* -ProxyCommand::ProxyCommand(std::unique_ptr command) - : m_supplier([command = std::move(command)] { return command.get(); }) {} -*/ - -void ProxyCommand::Initialize() { - m_command = m_supplier(); - Command_Schedule(m_command);; -} - -void ProxyCommand::End(bool interrupted) { - if (interrupted) { - m_command->Cancel(); - } - m_command.reset(); -} - -void ProxyCommand::Execute() {} - -bool ProxyCommand::IsFinished() { - // because we're between `initialize` and `end`, `m_command` is necessarily - // not null but if called otherwise and m_command is null, it's UB, so we can - // do whatever we want -- like return true. - return m_command == nullptr || !m_command->IsScheduled(); -} - -void ProxyCommand::InitSendable(wpi::SendableBuilder& builder) { - CommandBase::InitSendable(builder); - builder.AddStringProperty( - "proxied", - [this] { - if (m_command) { - return m_command->GetName(); - } else { - return std::string{"null"}; - } - }, - nullptr); -} diff --git a/commands2/src/cpp/frc2/command/ProxyScheduleCommand.cpp b/commands2/src/cpp/frc2/command/ProxyScheduleCommand.cpp deleted file mode 100644 index 70ecd87b..00000000 --- a/commands2/src/cpp/frc2/command/ProxyScheduleCommand.cpp +++ /dev/null @@ -1,42 +0,0 @@ -// 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. - -#include "frc2/command/ProxyScheduleCommand.h" - -using namespace frc2; - -ProxyScheduleCommand::ProxyScheduleCommand( - std::span> toSchedule) { - SetInsert(m_toSchedule, toSchedule); -} - -ProxyScheduleCommand::ProxyScheduleCommand(std::shared_ptr toSchedule) { - std::shared_ptr v[] = {toSchedule}; - SetInsert(m_toSchedule, {v, 1}); -} - -void ProxyScheduleCommand::Initialize() { - for (auto command : m_toSchedule) { - Command_Schedule(command); - } -} - -void ProxyScheduleCommand::End(bool interrupted) { - if (interrupted) { - for (auto command : m_toSchedule) { - command->Cancel(); - } - } -} - -void ProxyScheduleCommand::Execute() { - m_finished = true; - for (auto command : m_toSchedule) { - m_finished &= !command->IsScheduled(); - } -} - -bool ProxyScheduleCommand::IsFinished() { - return m_finished; -} diff --git a/commands2/src/cpp/frc2/command/RamseteCommand.cpp b/commands2/src/cpp/frc2/command/RamseteCommand.cpp deleted file mode 100644 index d3bf8d58..00000000 --- a/commands2/src/cpp/frc2/command/RamseteCommand.cpp +++ /dev/null @@ -1,169 +0,0 @@ -// 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. - -#include "frc2/command/RamseteCommand.h" - -#include - -#include - -using namespace frc2; - -RamseteCommand::RamseteCommand( - frc::Trajectory trajectory, std::function pose, - frc::RamseteController controller, - frc::SimpleMotorFeedforward feedforward, - frc::DifferentialDriveKinematics kinematics, - std::function wheelSpeeds, - frc2::PIDController leftController, frc2::PIDController rightController, - std::function output, - std::initializer_list> requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_controller(controller), - m_feedforward(feedforward), - m_kinematics(kinematics), - m_speeds(std::move(wheelSpeeds)), - m_leftController(std::make_unique(leftController)), - m_rightController(std::make_unique(rightController)), - m_outputVolts(std::move(output)), - m_usePID(true) { - AddRequirements(requirements); -} - -RamseteCommand::RamseteCommand( - frc::Trajectory trajectory, std::function pose, - frc::RamseteController controller, - frc::SimpleMotorFeedforward feedforward, - frc::DifferentialDriveKinematics kinematics, - std::function wheelSpeeds, - frc2::PIDController leftController, frc2::PIDController rightController, - std::function output, - std::span> requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_controller(controller), - m_feedforward(feedforward), - m_kinematics(kinematics), - m_speeds(std::move(wheelSpeeds)), - m_leftController(std::make_unique(leftController)), - m_rightController(std::make_unique(rightController)), - m_outputVolts(std::move(output)), - m_usePID(true) { - AddRequirements(requirements); -} - -RamseteCommand::RamseteCommand( - frc::Trajectory trajectory, std::function pose, - frc::RamseteController controller, - frc::DifferentialDriveKinematics kinematics, - std::function - output, - std::initializer_list> requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_controller(controller), - m_kinematics(kinematics), - m_outputVel(std::move(output)), - m_usePID(false) { - AddRequirements(requirements); -} - -RamseteCommand::RamseteCommand( - frc::Trajectory trajectory, std::function pose, - frc::RamseteController controller, - frc::DifferentialDriveKinematics kinematics, - std::function - output, - std::span> requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_controller(controller), - m_kinematics(kinematics), - m_outputVel(std::move(output)), - m_usePID(false) { - AddRequirements(requirements); -} - -void RamseteCommand::Initialize() { - m_prevTime = -1_s; - auto initialState = m_trajectory.Sample(0_s); - m_prevSpeeds = m_kinematics.ToWheelSpeeds( - frc::ChassisSpeeds{initialState.velocity, 0_mps, - initialState.velocity * initialState.curvature}); - m_timer.Restart(); - if (m_usePID) { - m_leftController->Reset(); - m_rightController->Reset(); - } -} - -void RamseteCommand::Execute() { - auto curTime = m_timer.Get(); - auto dt = curTime - m_prevTime; - - if (m_prevTime < 0_s) { - if (m_usePID) { - m_outputVolts(0_V, 0_V); - } else { - m_outputVel(0_mps, 0_mps); - } - - m_prevTime = curTime; - return; - } - - auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds( - m_controller.Calculate(m_pose(), m_trajectory.Sample(curTime))); - - if (m_usePID) { - auto leftFeedforward = m_feedforward.Calculate( - targetWheelSpeeds.left, - (targetWheelSpeeds.left - m_prevSpeeds.left) / dt); - - auto rightFeedforward = m_feedforward.Calculate( - targetWheelSpeeds.right, - (targetWheelSpeeds.right - m_prevSpeeds.right) / dt); - - auto leftOutput = - units::volt_t{m_leftController->Calculate( - m_speeds().left.value(), targetWheelSpeeds.left.value())} + - leftFeedforward; - - auto rightOutput = - units::volt_t{m_rightController->Calculate( - m_speeds().right.value(), targetWheelSpeeds.right.value())} + - rightFeedforward; - - m_outputVolts(leftOutput, rightOutput); - } else { - m_outputVel(targetWheelSpeeds.left, targetWheelSpeeds.right); - } - m_prevSpeeds = targetWheelSpeeds; - m_prevTime = curTime; -} - -void RamseteCommand::End(bool interrupted) { - m_timer.Stop(); - - if (interrupted) { - if (m_usePID) { - m_outputVolts(0_V, 0_V); - } else { - m_outputVel(0_mps, 0_mps); - } - } -} - -bool RamseteCommand::IsFinished() { - return m_timer.HasElapsed(m_trajectory.TotalTime()); -} - -void RamseteCommand::InitSendable(wpi::SendableBuilder& builder) { - CommandBase::InitSendable(builder); - builder.AddDoubleProperty( - "leftVelocity", [this] { return m_prevSpeeds.left.value(); }, nullptr); - builder.AddDoubleProperty( - "rightVelocity", [this] { return m_prevSpeeds.right.value(); }, nullptr); -} diff --git a/commands2/src/cpp/frc2/command/RepeatCommand.cpp b/commands2/src/cpp/frc2/command/RepeatCommand.cpp deleted file mode 100644 index 108f5923..00000000 --- a/commands2/src/cpp/frc2/command/RepeatCommand.cpp +++ /dev/null @@ -1,57 +0,0 @@ -// 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. - -#include "frc2/command/RepeatCommand.h" - -#include - -using namespace frc2; - -RepeatCommand::RepeatCommand(std::shared_ptr command) { - CommandScheduler::GetInstance().RequireUngrouped(command); - m_command = command; - m_command->SetComposed(true); - AddRequirements(m_command->GetRequirements()); - SetName(std::string{"Repeat("}.append(m_command->GetName()).append(")")); -} - -void RepeatCommand::Initialize() { - m_ended = false; - m_command->Initialize(); -} - -void RepeatCommand::Execute() { - if (m_ended) { - m_ended = false; - m_command->Initialize(); - } - m_command->Execute(); - if (m_command->IsFinished()) { - // restart command - m_command->End(false); - m_ended = true; - } -} - -bool RepeatCommand::IsFinished() { - return false; -} - -void RepeatCommand::End(bool interrupted) { - m_command->End(interrupted); -} - -bool RepeatCommand::RunsWhenDisabled() const { - return m_command->RunsWhenDisabled(); -} - -Command::InterruptionBehavior RepeatCommand::GetInterruptionBehavior() const { - return m_command->GetInterruptionBehavior(); -} - -void RepeatCommand::InitSendable(wpi::SendableBuilder& builder) { - CommandBase::InitSendable(builder); - builder.AddStringProperty( - "command", [this] { return m_command->GetName(); }, nullptr); -} diff --git a/commands2/src/cpp/frc2/command/RunCommand.cpp b/commands2/src/cpp/frc2/command/RunCommand.cpp deleted file mode 100644 index 9ce15071..00000000 --- a/commands2/src/cpp/frc2/command/RunCommand.cpp +++ /dev/null @@ -1,17 +0,0 @@ -// 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. - -#include "frc2/command/RunCommand.h" - -using namespace frc2; - -RunCommand::RunCommand(std::function toRun, - std::initializer_list> requirements) - : FunctionalCommand([] {}, std::move(toRun), [](bool interrupted) {}, - [] { return false; }, requirements) {} - -RunCommand::RunCommand(std::function toRun, - std::span> requirements) - : FunctionalCommand([] {}, std::move(toRun), [](bool interrupted) {}, - [] { return false; }, requirements) {} diff --git a/commands2/src/cpp/frc2/command/ScheduleCommand.cpp b/commands2/src/cpp/frc2/command/ScheduleCommand.cpp deleted file mode 100644 index 058de1f8..00000000 --- a/commands2/src/cpp/frc2/command/ScheduleCommand.cpp +++ /dev/null @@ -1,30 +0,0 @@ -// 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. - -#include "frc2/command/ScheduleCommand.h" - -using namespace frc2; - -ScheduleCommand::ScheduleCommand(std::span> toSchedule) { - SetInsert(m_toSchedule, toSchedule); -} - -ScheduleCommand::ScheduleCommand(std::shared_ptr toSchedule) { - std::shared_ptr v[] = {toSchedule}; - SetInsert(m_toSchedule, {v, 1}); -} - -void ScheduleCommand::Initialize() { - for (auto command : m_toSchedule) { - Command_Schedule(command); - } -} - -bool ScheduleCommand::IsFinished() { - return true; -} - -bool ScheduleCommand::RunsWhenDisabled() const { - return true; -} diff --git a/commands2/src/cpp/frc2/command/SequentialCommandGroup.cpp b/commands2/src/cpp/frc2/command/SequentialCommandGroup.cpp deleted file mode 100644 index 2d193a1c..00000000 --- a/commands2/src/cpp/frc2/command/SequentialCommandGroup.cpp +++ /dev/null @@ -1,89 +0,0 @@ -// 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. - -#include "frc2/command/SequentialCommandGroup.h" - -#include - -using namespace frc2; - -SequentialCommandGroup::SequentialCommandGroup( - std::vector>&& commands) { - AddCommands(std::move(commands)); -} - -void SequentialCommandGroup::Initialize() { - m_currentCommandIndex = 0; - - if (!m_commands.empty()) { - m_commands[0]->Initialize(); - } -} - -void SequentialCommandGroup::Execute() { - if (m_commands.empty()) { - return; - } - - auto& currentCommand = m_commands[m_currentCommandIndex]; - - currentCommand->Execute(); - if (currentCommand->IsFinished()) { - currentCommand->End(false); - m_currentCommandIndex++; - if (m_currentCommandIndex < m_commands.size()) { - m_commands[m_currentCommandIndex]->Initialize(); - } - } -} - -void SequentialCommandGroup::End(bool interrupted) { - if (interrupted && !m_commands.empty() && - m_currentCommandIndex != invalid_index && - m_currentCommandIndex < m_commands.size()) { - m_commands[m_currentCommandIndex]->End(interrupted); - } - m_currentCommandIndex = invalid_index; -} - -bool SequentialCommandGroup::IsFinished() { - return m_currentCommandIndex == m_commands.size(); -} - -bool SequentialCommandGroup::RunsWhenDisabled() const { - return m_runWhenDisabled; -} - -Command::InterruptionBehavior SequentialCommandGroup::GetInterruptionBehavior() - const { - return m_interruptBehavior; -} - -void SequentialCommandGroup::AddCommands( - std::vector>&& commands) { - CommandScheduler::GetInstance().RequireUngrouped(commands); - - if (m_currentCommandIndex != invalid_index) { - throw FRC_MakeError(frc::err::CommandIllegalUse, - "Commands cannot be added to a CommandGroup " - "while the group is running"); - } - - for (auto&& command : commands) { - command->SetComposed(true); - AddRequirements(command->GetRequirements()); - m_runWhenDisabled &= command->RunsWhenDisabled(); - if (command->GetInterruptionBehavior() == - Command::InterruptionBehavior::kCancelSelf) { - m_interruptBehavior = Command::InterruptionBehavior::kCancelSelf; - } - m_commands.emplace_back(std::move(command)); - } -} - -void SequentialCommandGroup::InitSendable(wpi::SendableBuilder& builder) { - CommandBase::InitSendable(builder); - builder.AddIntegerProperty( - "index", [this] { return m_currentCommandIndex; }, nullptr); -} diff --git a/commands2/src/cpp/frc2/command/StartEndCommand.cpp b/commands2/src/cpp/frc2/command/StartEndCommand.cpp deleted file mode 100644 index a3634f5d..00000000 --- a/commands2/src/cpp/frc2/command/StartEndCommand.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// 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. - -#include "frc2/command/StartEndCommand.h" - -using namespace frc2; - -StartEndCommand::StartEndCommand(std::function onInit, - std::function onEnd, - std::initializer_list> requirements) - : FunctionalCommand( - std::move(onInit), [] {}, - [onEnd = std::move(onEnd)](bool interrupted) { onEnd(); }, - [] { return false; }, requirements) {} - -StartEndCommand::StartEndCommand(std::function onInit, - std::function onEnd, - std::span> requirements) - : FunctionalCommand( - std::move(onInit), [] {}, - [onEnd = std::move(onEnd)](bool interrupted) { onEnd(); }, - [] { return false; }, requirements) {} diff --git a/commands2/src/cpp/frc2/command/Subsystem.cpp b/commands2/src/cpp/frc2/command/Subsystem.cpp deleted file mode 100644 index caa26bb2..00000000 --- a/commands2/src/cpp/frc2/command/Subsystem.cpp +++ /dev/null @@ -1,58 +0,0 @@ -// 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. - -#include "frc2/command/Subsystem.h" - -#include "frc2/command/CommandPtr.h" -#include "frc2/command/Commands.h" - -using namespace frc2; -Subsystem::~Subsystem() { - CommandScheduler::GetInstance().UnregisterSubsystem(this); -} - -void Subsystem::Periodic() {} - -void Subsystem::SimulationPeriodic() {} - -void Subsystem::SetDefaultCommand(std::shared_ptr defaultCommand) { - CommandScheduler::GetInstance().SetDefaultCommand(this, - defaultCommand); -} - -void Subsystem::RemoveDefaultCommand() { - CommandScheduler::GetInstance().RemoveDefaultCommand(this); -} - -std::shared_ptr Subsystem::GetDefaultCommand() { - return CommandScheduler::GetInstance().GetDefaultCommand(this); -} - -std::shared_ptr Subsystem::GetCurrentCommand() { - return CommandScheduler::GetInstance().Requiring(this); -} - -void Subsystem::Register() { - return CommandScheduler::GetInstance().RegisterSubsystem(this); -} - -/* -std::shared_ptr Subsystem::RunOnce(std::function action) { - return cmd::RunOnce(std::move(action), {this}); -} - -std::shared_ptr Subsystem::Run(std::function action) { - return cmd::Run(std::move(action), {this}); -} - -std::shared_ptr Subsystem::StartEnd(std::function start, - std::function end) { - return cmd::StartEnd(std::move(start), std::move(end), {this}); -} - -std::shared_ptr Subsystem::RunEnd(std::function run, - std::function end) { - return cmd::RunEnd(std::move(run), std::move(end), {this}); -} -*/ \ No newline at end of file diff --git a/commands2/src/cpp/frc2/command/SubsystemBase.cpp b/commands2/src/cpp/frc2/command/SubsystemBase.cpp deleted file mode 100644 index 8216a075..00000000 --- a/commands2/src/cpp/frc2/command/SubsystemBase.cpp +++ /dev/null @@ -1,68 +0,0 @@ -// 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. - -#include "frc2/command/SubsystemBase.h" - -#include -#include - -#include "frc2/command/Command.h" -#include "frc2/command/CommandScheduler.h" - -using namespace frc2; - -SubsystemBase::SubsystemBase() { - wpi::SendableRegistry::AddLW(this, GetTypeName(*this)); - CommandScheduler::GetInstance().RegisterSubsystem({this}); -} - -void SubsystemBase::InitSendable(wpi::SendableBuilder& builder) { - builder.SetSmartDashboardType("Subsystem"); - builder.AddBooleanProperty( - ".hasDefault", [this] { return GetDefaultCommand() != nullptr; }, - nullptr); - builder.AddStringProperty( - ".default", - [this]() -> std::string { - auto command = GetDefaultCommand(); - if (command == nullptr) { - return "none"; - } - return command->GetName(); - }, - nullptr); - builder.AddBooleanProperty( - ".hasCommand", [this] { return GetCurrentCommand() != nullptr; }, - nullptr); - builder.AddStringProperty( - ".command", - [this]() -> std::string { - auto command = GetCurrentCommand(); - if (command == nullptr) { - return "none"; - } - return command->GetName(); - }, - nullptr); -} - -std::string SubsystemBase::GetName() const { - return wpi::SendableRegistry::GetName(this); -} - -void SubsystemBase::SetName(std::string_view name) { - wpi::SendableRegistry::SetName(this, name); -} - -std::string SubsystemBase::GetSubsystem() const { - return wpi::SendableRegistry::GetSubsystem(this); -} - -void SubsystemBase::SetSubsystem(std::string_view name) { - wpi::SendableRegistry::SetSubsystem(this, name); -} - -void SubsystemBase::AddChild(std::string name, wpi::Sendable* child) { - wpi::SendableRegistry::AddLW(child, GetSubsystem(), name); -} diff --git a/commands2/src/cpp/frc2/command/WaitCommand.cpp b/commands2/src/cpp/frc2/command/WaitCommand.cpp deleted file mode 100644 index f0092ed1..00000000 --- a/commands2/src/cpp/frc2/command/WaitCommand.cpp +++ /dev/null @@ -1,37 +0,0 @@ -// 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. - -#include "frc2/command/WaitCommand.h" - -#include -#include -#include - -using namespace frc2; - -WaitCommand::WaitCommand(units::second_t duration) : m_duration{duration} { - SetName(fmt::format("{}: {}", GetName(), duration)); -} - -void WaitCommand::Initialize() { - m_timer.Restart(); -} - -void WaitCommand::End(bool interrupted) { - m_timer.Stop(); -} - -bool WaitCommand::IsFinished() { - return m_timer.HasElapsed(m_duration); -} - -bool WaitCommand::RunsWhenDisabled() const { - return true; -} - -void WaitCommand::InitSendable(wpi::SendableBuilder& builder) { - CommandBase::InitSendable(builder); - builder.AddDoubleProperty( - "duration", [this] { return m_duration.value(); }, nullptr); -} diff --git a/commands2/src/cpp/frc2/command/WaitUntilCommand.cpp b/commands2/src/cpp/frc2/command/WaitUntilCommand.cpp deleted file mode 100644 index 9ccd94ed..00000000 --- a/commands2/src/cpp/frc2/command/WaitUntilCommand.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// 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. - -#include "frc2/command/WaitUntilCommand.h" - -#include - -using namespace frc2; - -WaitUntilCommand::WaitUntilCommand(std::function condition) - : m_condition{std::move(condition)} {} - -WaitUntilCommand::WaitUntilCommand(units::second_t time) - : m_condition{[=] { return frc::Timer::GetMatchTime() - time > 0_s; }} {} - -bool WaitUntilCommand::IsFinished() { - return m_condition(); -} - -bool WaitUntilCommand::RunsWhenDisabled() const { - return true; -} diff --git a/commands2/src/cpp/frc2/command/WrapperCommand.cpp b/commands2/src/cpp/frc2/command/WrapperCommand.cpp deleted file mode 100644 index e8c9f66a..00000000 --- a/commands2/src/cpp/frc2/command/WrapperCommand.cpp +++ /dev/null @@ -1,45 +0,0 @@ -// 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. - -#include "frc2/command/WrapperCommand.h" - -#include "frc2/command/Command.h" - -using namespace frc2; - -WrapperCommand::WrapperCommand(std::unique_ptr&& command) { - CommandScheduler::GetInstance().RequireUngrouped(command); - m_command = std::move(command); - m_command->SetComposed(true); - // copy the wrapped command's name - SetName(m_command->GetName()); -} - -void WrapperCommand::Initialize() { - m_command->Initialize(); -} - -void WrapperCommand::Execute() { - m_command->Execute(); -} - -bool WrapperCommand::IsFinished() { - return m_command->IsFinished(); -} - -void WrapperCommand::End(bool interrupted) { - m_command->End(interrupted); -} - -bool WrapperCommand::RunsWhenDisabled() const { - return m_command->RunsWhenDisabled(); -} - -Command::InterruptionBehavior WrapperCommand::GetInterruptionBehavior() const { - return m_command->GetInterruptionBehavior(); -} - -wpi::SmallSet, 4> WrapperCommand::GetRequirements() const { - return m_command->GetRequirements(); -} diff --git a/commands2/src/cpp/frc2/command/button/Button.cpp b/commands2/src/cpp/frc2/command/button/Button.cpp deleted file mode 100644 index 90e2ab63..00000000 --- a/commands2/src/cpp/frc2/command/button/Button.cpp +++ /dev/null @@ -1,105 +0,0 @@ -// 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. - -#include "frc2/command/button/Button.h" - -using namespace frc2; - -Button::Button(std::function isPressed) : Trigger(isPressed) {} - -Button Button::WhenPressed(std::shared_ptr command) { - WPI_IGNORE_DEPRECATED - WhenActive(command); - WPI_UNIGNORE_DEPRECATED - return *this; -} - -/* -Button Button::WhenPressed(std::function toRun, - std::initializer_list> requirements) { - WPI_IGNORE_DEPRECATED - WhenActive(std::move(toRun), requirements); - WPI_UNIGNORE_DEPRECATED - return *this; -} -*/ - -Button Button::WhenPressed(std::function toRun, - std::span> requirements) { - WPI_IGNORE_DEPRECATED - WhenActive(std::move(toRun), requirements); - WPI_UNIGNORE_DEPRECATED - return *this; -} - -Button Button::WhileHeld(std::shared_ptr command) { - WPI_IGNORE_DEPRECATED - WhileActiveContinous(command); - WPI_UNIGNORE_DEPRECATED - return *this; -} - -/* -Button Button::WhileHeld(std::function toRun, - std::initializer_list> requirements) { - WPI_IGNORE_DEPRECATED - WhileActiveContinous(std::move(toRun), requirements); - WPI_UNIGNORE_DEPRECATED - return *this; -} -*/ - -Button Button::WhileHeld(std::function toRun, - std::span> requirements) { - WPI_IGNORE_DEPRECATED - WhileActiveContinous(std::move(toRun), requirements); - WPI_UNIGNORE_DEPRECATED - return *this; -} - -Button Button::WhenHeld(std::shared_ptr command) { - WPI_IGNORE_DEPRECATED - WhileActiveOnce(command); - WPI_UNIGNORE_DEPRECATED - return *this; -} - -Button Button::WhenReleased(std::shared_ptr command) { - WPI_IGNORE_DEPRECATED - WhenInactive(command); - WPI_UNIGNORE_DEPRECATED - return *this; -} - -/* -Button Button::WhenReleased(std::function toRun, - std::initializer_list> requirements) { - WPI_IGNORE_DEPRECATED - WhenInactive(std::move(toRun), requirements); - WPI_UNIGNORE_DEPRECATED - return *this; -} -*/ - -Button Button::WhenReleased(std::function toRun, - std::span> requirements) { - WPI_IGNORE_DEPRECATED - WhenInactive(std::move(toRun), requirements); - WPI_UNIGNORE_DEPRECATED - return *this; -} - -Button Button::ToggleWhenPressed(std::shared_ptr command) { - WPI_IGNORE_DEPRECATED - ToggleWhenActive(command); - WPI_UNIGNORE_DEPRECATED - return *this; -} - -Button Button::CancelWhenPressed(std::shared_ptr command) { - WPI_IGNORE_DEPRECATED - CancelWhenActive(command); - WPI_UNIGNORE_DEPRECATED - return *this; -} diff --git a/commands2/src/cpp/frc2/command/button/CommandGenericHID.cpp b/commands2/src/cpp/frc2/command/button/CommandGenericHID.cpp deleted file mode 100644 index 60e28b7d..00000000 --- a/commands2/src/cpp/frc2/command/button/CommandGenericHID.cpp +++ /dev/null @@ -1,72 +0,0 @@ -// 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. - -#include "frc2/command/button/CommandGenericHID.h" - -using namespace frc2; - -#define loop (loop_arg ? *loop_arg : CommandScheduler::GetInstance().GetDefaultButtonLoop()) - -Trigger CommandGenericHID::Button(int button, std::optional loop_arg) const { - return GenericHID::Button(button, loop).CastTo(); -} - -Trigger CommandGenericHID::POV(int angle, std::optional loop_arg) const { - return POV(0, angle, loop); -} - -Trigger CommandGenericHID::POV(int pov, int angle, std::optional loop_arg) const { - return Trigger(loop, - [this, pov, angle] { return this->GetPOV(pov) == angle; }); -} - -Trigger CommandGenericHID::POVUp(std::optional loop_arg) const { - return POV(0, loop); -} - -Trigger CommandGenericHID::POVUpRight(std::optional loop_arg) const { - return POV(45, loop); -} - -Trigger CommandGenericHID::POVRight(std::optional loop_arg) const { - return POV(90, loop); -} - -Trigger CommandGenericHID::POVDownRight(std::optional loop_arg) const { - return POV(135, loop); -} - -Trigger CommandGenericHID::POVDown(std::optional loop_arg) const { - return POV(180, loop); -} - -Trigger CommandGenericHID::POVDownLeft(std::optional loop_arg) const { - return POV(225, loop); -} - -Trigger CommandGenericHID::POVLeft(std::optional loop_arg) const { - return POV(270, loop); -} - -Trigger CommandGenericHID::POVUpLeft(std::optional loop_arg) const { - return POV(315, loop); -} - -Trigger CommandGenericHID::POVCenter(std::optional loop_arg) const { - return POV(360, loop); -} - -Trigger CommandGenericHID::AxisLessThan(int axis, double threshold, - std::optional loop_arg) const { - return Trigger(loop, [this, axis, threshold]() { - return this->GetRawAxis(axis) < threshold; - }); -} - -Trigger CommandGenericHID::AxisGreaterThan(int axis, double threshold, - std::optional loop_arg) const { - return Trigger(loop, [this, axis, threshold]() { - return this->GetRawAxis(axis) > threshold; - }); -} diff --git a/commands2/src/cpp/frc2/command/button/CommandJoystick.cpp b/commands2/src/cpp/frc2/command/button/CommandJoystick.cpp deleted file mode 100644 index 38390a87..00000000 --- a/commands2/src/cpp/frc2/command/button/CommandJoystick.cpp +++ /dev/null @@ -1,21 +0,0 @@ -// 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. - -#include "frc2/command/button/CommandJoystick.h" - -using namespace frc2; - -#define loop (loop_arg ? *loop_arg : CommandScheduler::GetInstance().GetDefaultButtonLoop()) - -Trigger CommandJoystick::Button(int button, std::optional loop_arg) const { - return GenericHID::Button(button, loop).CastTo(); -} - -Trigger CommandJoystick::Trigger(std::optional loop_arg) const { - return Joystick::Trigger(loop).CastTo(); -} - -Trigger CommandJoystick::Top(std::optional loop_arg) const { - return Joystick::Top(loop).CastTo(); -} diff --git a/commands2/src/cpp/frc2/command/button/CommandPS4Controller.cpp b/commands2/src/cpp/frc2/command/button/CommandPS4Controller.cpp deleted file mode 100644 index 729a8920..00000000 --- a/commands2/src/cpp/frc2/command/button/CommandPS4Controller.cpp +++ /dev/null @@ -1,65 +0,0 @@ -// 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. - -#include "frc2/command/button/CommandPS4Controller.h" - -using namespace frc2; - -#define loop (loop_arg ? *loop_arg : CommandScheduler::GetInstance().GetDefaultButtonLoop()) - -Trigger CommandPS4Controller::Button(int button, std::optional loop_arg) const { - return GenericHID::Button(button, loop).CastTo(); -} - -Trigger CommandPS4Controller::Square(std::optional loop_arg) const { - return PS4Controller::Square(loop).CastTo(); -} - -Trigger CommandPS4Controller::Cross(std::optional loop_arg) const { - return PS4Controller::Cross(loop).CastTo(); -} - -Trigger CommandPS4Controller::Circle(std::optional loop_arg) const { - return PS4Controller::Circle(loop).CastTo(); -} - -Trigger CommandPS4Controller::Triangle(std::optional loop_arg) const { - return PS4Controller::Triangle(loop).CastTo(); -} - -Trigger CommandPS4Controller::L1(std::optional loop_arg) const { - return PS4Controller::L1(loop).CastTo(); -} - -Trigger CommandPS4Controller::R1(std::optional loop_arg) const { - return PS4Controller::R1(loop).CastTo(); -} - -Trigger CommandPS4Controller::L2(std::optional loop_arg) const { - return PS4Controller::L2(loop).CastTo(); -} - -Trigger CommandPS4Controller::R2(std::optional loop_arg) const { - return PS4Controller::R2(loop).CastTo(); -} - -Trigger CommandPS4Controller::Options(std::optional loop_arg) const { - return PS4Controller::Options(loop).CastTo(); -} - -Trigger CommandPS4Controller::L3(std::optional loop_arg) const { - return PS4Controller::L3(loop).CastTo(); -} - -Trigger CommandPS4Controller::R3(std::optional loop_arg) const { - return PS4Controller::R3(loop).CastTo(); -} - -Trigger CommandPS4Controller::PS(std::optional loop_arg) const { - return PS4Controller::PS(loop).CastTo(); -} - -Trigger CommandPS4Controller::Touchpad(std::optional loop_arg) const { - return PS4Controller::Touchpad(loop).CastTo(); -} diff --git a/commands2/src/cpp/frc2/command/button/CommandXboxController.cpp b/commands2/src/cpp/frc2/command/button/CommandXboxController.cpp deleted file mode 100644 index 19e14b2c..00000000 --- a/commands2/src/cpp/frc2/command/button/CommandXboxController.cpp +++ /dev/null @@ -1,63 +0,0 @@ -// 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. - -#include "frc2/command/button/CommandXboxController.h" - -using namespace frc2; - -#define loop (loop_arg ? *loop_arg : CommandScheduler::GetInstance().GetDefaultButtonLoop()) - -Trigger CommandXboxController::Button(int button, std::optional loop_arg) const { - return GenericHID::Button(button, loop).CastTo(); -} - -Trigger CommandXboxController::LeftBumper(std::optional loop_arg) const { - return XboxController::LeftBumper(loop).CastTo(); -} - -Trigger CommandXboxController::RightBumper(std::optional loop_arg) const { - return XboxController::RightBumper(loop).CastTo(); -} - -Trigger CommandXboxController::LeftStick(std::optional loop_arg) const { - return XboxController::LeftStick(loop).CastTo(); -} - -Trigger CommandXboxController::RightStick(std::optional loop_arg) const { - return XboxController::RightStick(loop).CastTo(); -} - -Trigger CommandXboxController::A(std::optional loop_arg) const { - return XboxController::A(loop).CastTo(); -} - -Trigger CommandXboxController::B(std::optional loop_arg) const { - return XboxController::B(loop).CastTo(); -} - -Trigger CommandXboxController::X(std::optional loop_arg) const { - return XboxController::X(loop).CastTo(); -} - -Trigger CommandXboxController::Y(std::optional loop_arg) const { - return XboxController::Y(loop).CastTo(); -} - -Trigger CommandXboxController::Back(std::optional loop_arg) const { - return XboxController::Back(loop).CastTo(); -} - -Trigger CommandXboxController::Start(std::optional loop_arg) const { - return XboxController::Start(loop).CastTo(); -} - -Trigger CommandXboxController::LeftTrigger(double threshold, - std::optional loop_arg) const { - return XboxController::LeftTrigger(threshold, loop).CastTo(); -} - -Trigger CommandXboxController::RightTrigger(double threshold, - std::optional loop_arg) const { - return XboxController::RightTrigger(threshold, loop).CastTo(); -} diff --git a/commands2/src/cpp/frc2/command/button/NetworkButton.cpp b/commands2/src/cpp/frc2/command/button/NetworkButton.cpp deleted file mode 100644 index e26cd147..00000000 --- a/commands2/src/cpp/frc2/command/button/NetworkButton.cpp +++ /dev/null @@ -1,30 +0,0 @@ -// 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. - -#include "frc2/command/button/NetworkButton.h" - -#include - -using namespace frc2; - -WPI_IGNORE_DEPRECATED -NetworkButton::NetworkButton(nt::BooleanTopic topic) - : NetworkButton(topic.Subscribe(false)) {} - -NetworkButton::NetworkButton(nt::BooleanSubscriber sub) - : Button([sub = std::make_shared(std::move(sub))] { - return sub->GetTopic().GetInstance().IsConnected() && sub->Get(); - }) {} -WPI_UNIGNORE_DEPRECATED - -NetworkButton::NetworkButton(std::shared_ptr table, - std::string_view field) - : NetworkButton(table->GetBooleanTopic(field)) {} - -NetworkButton::NetworkButton(std::string_view table, std::string_view field) - : NetworkButton(nt::NetworkTableInstance::GetDefault(), table, field) {} - -NetworkButton::NetworkButton(nt::NetworkTableInstance inst, - std::string_view table, std::string_view field) - : NetworkButton(inst.GetTable(table), field) {} diff --git a/commands2/src/cpp/frc2/command/button/Trigger.cpp b/commands2/src/cpp/frc2/command/button/Trigger.cpp deleted file mode 100644 index 69b96020..00000000 --- a/commands2/src/cpp/frc2/command/button/Trigger.cpp +++ /dev/null @@ -1,344 +0,0 @@ -// 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. - -#include "frc2/command/button/Trigger.h" - -#include - -#include "frc2/command/InstantCommand.h" - -using namespace frc; -using namespace frc2; - -Trigger::Trigger(const Trigger& other) = default; - -Trigger Trigger::OnTrue(std::shared_ptr command) { - m_loop->Bind( - [condition = m_condition, previous = m_condition(), command]() mutable { - bool current = condition(); - - if (!previous && current) { - Command_Schedule(command); - } - - previous = current; - }); - return *this; -} - -/* -Trigger Trigger::OnTrue(CommandPtr&& command) { - m_loop->Bind([condition = m_condition, previous = m_condition(), - command = std::move(command)]() mutable { - bool current = condition(); - - if (!previous && current) { - command.Schedule(); - } - - previous = current; - }); - return *this; -} -*/ - -Trigger Trigger::OnFalse(std::shared_ptr command) { - m_loop->Bind( - [condition = m_condition, previous = m_condition(), command]() mutable { - bool current = condition(); - - if (previous && !current) { - Command_Schedule(command); - } - - previous = current; - }); - return *this; -} - -/* -Trigger Trigger::OnFalse(CommandPtr&& command) { - m_loop->Bind([condition = m_condition, previous = m_condition(), - command = std::move(command)]() mutable { - bool current = condition(); - - if (previous && !current) { - command.Schedule(); - } - - previous = current; - }); - return *this; -} -*/ - -Trigger Trigger::WhileTrue(std::shared_ptr command) { - m_loop->Bind( - [condition = m_condition, previous = m_condition(), command]() mutable { - bool current = condition(); - - if (!previous && current) { - Command_Schedule(command); - } else if (previous && !current) { - command->Cancel(); - } - - previous = current; - }); - return *this; -} - -/* -Trigger Trigger::WhileTrue(CommandPtr&& command) { - m_loop->Bind([condition = m_condition, previous = m_condition(), - command = std::move(command)]() mutable { - bool current = condition(); - - if (!previous && current) { - command.Schedule(); - } else if (previous && !current) { - command.Cancel(); - } - - previous = current; - }); - return *this; -} -*/ - -Trigger Trigger::WhileFalse(std::shared_ptr command) { - m_loop->Bind( - [condition = m_condition, previous = m_condition(), command]() mutable { - bool current = condition(); - - if (previous && !current) { - Command_Schedule(command); - } else if (!previous && current) { - command->Cancel(); - } - - previous = current; - }); - return *this; -} - -/* -Trigger Trigger::WhileFalse(CommandPtr&& command) { - m_loop->Bind([condition = m_condition, previous = m_condition(), - command = std::move(command)]() mutable { - bool current = condition(); - - if (!previous && current) { - command.Schedule(); - } else if (previous && !current) { - command.Cancel(); - } - - previous = current; - }); - return *this; -} -*/ - -Trigger Trigger::ToggleOnTrue(std::shared_ptr command) { - m_loop->Bind([condition = m_condition, previous = m_condition(), - command = command]() mutable { - bool current = condition(); - - if (!previous && current) { - if (command->IsScheduled()) { - command->Cancel(); - } else { - Command_Schedule(command); - } - } - - previous = current; - }); - return *this; -} - -/* -Trigger Trigger::ToggleOnTrue(CommandPtr&& command) { - m_loop->Bind([condition = m_condition, previous = m_condition(), - command = std::move(command)]() mutable { - bool current = condition(); - - if (!previous && current) { - if (command.IsScheduled()) { - command.Cancel(); - } else { - command.Schedule(); - } - } - - previous = current; - }); - return *this; -} -*/ - -Trigger Trigger::ToggleOnFalse(std::shared_ptr command) { - m_loop->Bind([condition = m_condition, previous = m_condition(), - command = command]() mutable { - bool current = condition(); - - if (previous && !current) { - if (command->IsScheduled()) { - command->Cancel(); - } else { - Command_Schedule(command); - } - } - - previous = current; - }); - return *this; -} - -/* -Trigger Trigger::ToggleOnFalse(CommandPtr&& command) { - m_loop->Bind([condition = m_condition, previous = m_condition(), - command = std::move(command)]() mutable { - bool current = condition(); - - if (previous && !current) { - if (command.IsScheduled()) { - command.Cancel(); - } else { - command.Schedule(); - } - } - - previous = current; - }); - return *this; -} -*/ - -WPI_IGNORE_DEPRECATED -Trigger Trigger::WhenActive(std::shared_ptr command) { - return OnTrue(command); -} - -// Trigger Trigger::WhenActive(std::function toRun, -// std::initializer_list> requirements) { -// return WhenActive(std::move(toRun), -// {requirements.begin(), requirements.end()}); -// } - -Trigger Trigger::WhenActive(std::function toRun, - std::span> requirements) { - return WhenActive(std::make_shared(std::move(toRun), requirements)); -} - -Trigger Trigger::WhileActiveContinous(std::shared_ptr command) { - m_loop->Bind([condition = m_condition, previous = m_condition(), - command = std::move(command)]() mutable { - bool current = condition(); - - if (current) { - Command_Schedule(command); - } else if (previous && !current) { - command->Cancel(); - } - - previous = current; - }); - return *this; -} - -// Trigger Trigger::WhileActiveContinous( -// std::function toRun, -// std::initializer_list> requirements) { -// return WhileActiveContinous(std::move(toRun), -// {requirements.begin(), requirements.end()}); -// } - -Trigger Trigger::WhileActiveContinous( - std::function toRun, std::span> requirements) { - return WhileActiveContinous(std::make_shared(std::move(toRun), requirements)); -} - -Trigger Trigger::WhileActiveOnce(std::shared_ptr command) { - m_loop->Bind( - [condition = m_condition, previous = m_condition(), command]() mutable { - bool current = condition(); - - if (!previous && current) { - Command_Schedule(command); - } else if (previous && !current) { - command->Cancel(); - } - - previous = current; - }); - return *this; -} - -Trigger Trigger::WhenInactive(std::shared_ptr command) { - m_loop->Bind( - [condition = m_condition, previous = m_condition(), command]() mutable { - bool current = condition(); - - if (previous && !current) { - Command_Schedule(command); - } - - previous = current; - }); - return *this; -} - -// Trigger Trigger::WhenInactive(std::function toRun, -// std::initializer_list> requirements) { -// return WhenInactive(std::move(toRun), -// {requirements.begin(), requirements.end()}); -// } - -Trigger Trigger::WhenInactive(std::function toRun, - std::span> requirements) { - return WhenInactive(std::make_shared(std::move(toRun), requirements)); -} - -Trigger Trigger::ToggleWhenActive(std::shared_ptr command) { - m_loop->Bind([condition = m_condition, previous = m_condition(), - command = command]() mutable { - bool current = condition(); - - if (!previous && current) { - if (command->IsScheduled()) { - command->Cancel(); - } else { - Command_Schedule(command); - } - } - - previous = current; - }); - return *this; -} - -Trigger Trigger::CancelWhenActive(std::shared_ptr command) { - m_loop->Bind([condition = m_condition, previous = m_condition(), - command = std::move(command)]() mutable { - bool current = condition(); - - if (!previous && current) { - command->Cancel(); - } - - previous = current; - }); - return *this; -} -WPI_UNIGNORE_DEPRECATED - -Trigger Trigger::Debounce(units::second_t debounceTime, - frc::Debouncer::DebounceType type) { - return Trigger(m_loop, [debouncer = frc::Debouncer(debounceTime, type), - condition = m_condition]() mutable { - return debouncer.Calculate(condition()); - }); -} diff --git a/commands2/src/helpers.cpp b/commands2/src/helpers.cpp deleted file mode 100644 index e5d8319e..00000000 --- a/commands2/src/helpers.cpp +++ /dev/null @@ -1,26 +0,0 @@ - -#include "helpers.h" - -std::vector> pyargs2cmdList(py::args cmds) { - std::vector> commands; - for (auto cmd : cmds) { - commands.emplace_back(py::cast>(cmd)); - } - return commands; -} - -std::vector> pyargs2SharedSubsystemList(py::args subs) { - std::vector> subsystems; - for (auto sub : subs) { - subsystems.emplace_back(py::cast>(sub)); - } - return subsystems; -} - -std::vector pyargs2SubsystemList(py::args subs) { - std::vector subsystems; - for (auto sub : subs) { - subsystems.emplace_back(py::cast(sub)); - } - return subsystems; -} \ No newline at end of file diff --git a/commands2/src/helpers.h b/commands2/src/helpers.h deleted file mode 100644 index c3e3649a..00000000 --- a/commands2/src/helpers.h +++ /dev/null @@ -1,19 +0,0 @@ - -#pragma once -#include -#include -#include - -std::vector> pyargs2cmdList(py::args cmds); - -std::vector> pyargs2SharedSubsystemList(py::args subs); - -std::vector pyargs2SubsystemList(py::args subs); - -template -std::shared_ptr convertToSharedPtrHack(T *orig) { - py::gil_scoped_acquire gil; - - py::object pyo = py::cast(orig); - return py::cast>(pyo); -} \ No newline at end of file diff --git a/commands2/src/include/frc2/command/Command.h b/commands2/src/include/frc2/command/Command.h deleted file mode 100644 index df53bfb1..00000000 --- a/commands2/src/include/frc2/command/Command.h +++ /dev/null @@ -1,434 +0,0 @@ -// 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. - -#pragma once - -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "frc2/command/Subsystem.h" - -namespace frc2 { - -template -std::string GetTypeName(const T& type) { - return wpi::Demangle(typeid(type).name()); -} - -class PerpetualCommand; - -/** - * A state machine representing a complete action to be performed by the robot. - * Commands are run by the CommandScheduler, and can be composed into - * CommandGroups to allow users to build complicated multi-step 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. - * - * @see CommandScheduler - */ -class Command { - public: - Command() = default; - virtual ~Command(); - -// Command(const Command&) = default; -// Command& operator=(const Command& rhs); -// Command(Command&&) = default; -// Command& operator=(Command&&) = default; - - /** - * The initial subroutine of a command. Called once when the command is - * initially scheduled. - */ - virtual void Initialize(); - - /** - * The main body of a command. Called repeatedly while the command is - * scheduled. - */ - virtual void Execute(); - - /** - * The action to take when the command ends. Called when either the command - * finishes normally, or when it interrupted/canceled. - * - * @param interrupted whether the command was interrupted/canceled - */ - virtual void End(bool interrupted); - - /** - * Whether the command has finished. Once a command finishes, the scheduler - * will call its end() method and un-schedule it. - * - * @return whether the command has finished. - */ - virtual bool IsFinished() { return false; } - - /** - * 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, 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. - * - * @return the set of subsystems that are required - * @see InterruptionBehavior - */ - virtual wpi::SmallSet, 4> GetRequirements() const = 0; - - /** - * An enum describing the command's behavior when another command with a - * shared requirement is scheduled. - */ - enum class InterruptionBehavior { - /** - * This command ends, End(true) is called, and the incoming command is - * scheduled normally. - * - *

This is the default behavior. - */ - kCancelSelf, - /** This command continues, and the incoming command is not scheduled. */ - kCancelIncoming - }; - - friend class CommandPtr; - - /** - * 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. - * - * @param duration the timeout duration - * @return the command with the timeout added - */ - /* - [[nodiscard]] CommandPtr WithTimeout(units::second_t duration) &&; - */ - - /** - * 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 that this only applies to the - * command returned by this method; the calling command is not itself changed. - * - * @param condition the interrupt condition - * @return the command with the interrupt condition added - */ - /* - [[nodiscard]] CommandPtr Until(std::function condition) &&; - */ - - /** - * 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 that this only applies to the - * command returned by this method; the calling command is not itself changed. - * - * @param condition the interrupt condition - * @return the command with the interrupt condition added - * @deprecated Replace with Until() - */ - /* - WPI_DEPRECATED("Replace with Until()") - [[nodiscard]] CommandPtr WithInterrupt(std::function condition) &&; - */ - - /** - * Decorates this command with a runnable to run before this command starts. - * - * @param toRun the Runnable to run - * @param requirements the required subsystems - * @return the decorated command - */ - /* - [[nodiscard]] CommandPtr BeforeStarting( - std::function toRun, - std::initializer_list requirements) &&; - */ - - /** - * Decorates this command with a runnable to run before this command starts. - * - * @param toRun the Runnable to run - * @param requirements the required subsystems - * @return the decorated command - */ - /* - [[nodiscard]] CommandPtr BeforeStarting( - std::function toRun, - std::span requirements = {}) &&; - */ - - /** - * Decorates this command with a runnable to run after the command finishes. - * - * @param toRun the Runnable to run - * @param requirements the required subsystems - * @return the decorated command - */ - /* - [[nodiscard]] CommandPtr AndThen( - std::function toRun, - std::initializer_list requirements) &&; - */ - - /** - * Decorates this command with a runnable to run after the command finishes. - * - * @param toRun the Runnable to run - * @param requirements the required subsystems - * @return the decorated command - */ - /* - [[nodiscard]] CommandPtr AndThen( - std::function toRun, - std::span requirements = {}) &&; - */ - - /** - * Decorates this command to run perpetually, ignoring its ordinary end - * conditions. The decorated command can still be interrupted or canceled. - * - * @return the decorated command - * @deprecated PerpetualCommand violates the assumption that execute() doesn't -get called after isFinished() returns true -- an assumption that should be -valid. This was unsafe/undefined behavior from the start, and RepeatCommand -provides an easy way to achieve similar end results with slightly different (and -safe) semantics. - */ - /* - WPI_DEPRECATED( - "PerpetualCommand violates the assumption that execute() doesn't get " - "called after isFinished() returns true -- an assumption that should be " - "valid." - "This was unsafe/undefined behavior from the start, and RepeatCommand " - "provides an easy way to achieve similar end results with slightly " - "different (and safe) semantics.") - PerpetualCommand Perpetually() &&; - */ - - /** - * Decorates this command to run "by proxy" by wrapping it in a - * ProxyCommand. This is useful for "forking off" from command groups - * when the user does not wish to extend the command's requirements to the - * entire command group. - * - *

This overload transfers command ownership to the returned CommandPtr. - * - * @return the decorated command - */ - /* - [[nodiscard]] CommandPtr AsProxy() &&; - */ - - /** - * 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. - * - * @param condition the condition that will prevent the command from running - * @return the decorated command - */ - /* - [[nodiscard]] CommandPtr Unless(std::function condition) &&; - */ - - /** - * Decorates this command to run or stop when disabled. - * - * @param doesRunWhenDisabled true to run when disabled. - * @return the decorated command - */ - /* - [[nodiscard]] CommandPtr IgnoringDisable(bool doesRunWhenDisabled) &&; - */ - - /** - * Decorates this command to run or stop when disabled. - * - * @param interruptBehavior true to run when disabled. - * @return the decorated command - */ - /* - [[nodiscard]] CommandPtr WithInterruptBehavior( - Command::InterruptionBehavior interruptBehavior) &&; - */ - - /** - * Decorates this command with a lambda to call on interrupt or end, following - * the command's inherent Command::End(bool) method. - * - * @param end a lambda accepting a boolean parameter specifying whether the - * command was interrupted. - * @return the decorated command - */ - /* - [[nodiscard]] CommandPtr FinallyDo(std::function end) &&; - */ - - /** - * Decorates this command with a lambda to call on interrupt, following the - * command's inherent Command::End(bool) method. - * - * @param handler a lambda to run when the command is interrupted - * @return the decorated command - */ - /* - [[nodiscard]] CommandPtr HandleInterrupt(std::function handler) &&; - */ - - /** - * Decorates this Command with a name. - * - * @param name name - * @return the decorated Command - */ - /* - [[nodiscard]] CommandPtr WithName(std::string_view name) &&; - */ - - /** - * Schedules this command. - */ - void Schedule(); - - /** - * Cancels this command. Will call End(true). Commands will be canceled - * regardless of interruption behavior. - */ - void Cancel(); - - /** - * Whether or not 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. - * - * @return Whether the command is scheduled. - */ - bool IsScheduled(); - - /** - * Whether the command requires a given subsystem. Named "HasRequirement" - * rather than "requires" to avoid confusion with Command::Requires(Subsystem) - * -- this may be able to be changed in a few years. - * - * @param requirement the subsystem to inquire about - * @return whether the subsystem is required - */ - bool HasRequirement(Subsystem* requirement) const; - - /** - * Whether the command is currently grouped in a command group. Used as extra - * insurance to prevent accidental independent use of grouped commands. - */ - bool IsComposed() const; - - /** - * Sets whether the command is currently composed in a command composition. - * Can be used to "reclaim" a command if a composition is no longer going to - * use it. NOT ADVISED! - */ - void SetComposed(bool isComposed); - - /** - * Whether the command is currently grouped in a command group. Used as extra - * insurance to prevent accidental independent use of grouped commands. - * - * @deprecated Moved to IsComposed() - */ - WPI_DEPRECATED("Moved to IsComposed()") - bool IsGrouped() const; - - /** - * Sets whether the command is currently grouped in a command group. Can be - * used to "reclaim" a command if a group is no longer going to use it. NOT - * ADVISED! - * - * @deprecated Moved to SetComposed() - */ - WPI_DEPRECATED("Moved to SetComposed()") - void SetGrouped(bool grouped); - - /** - * Whether the given command should run when the robot is disabled. Override - * to return true if the command should run when disabled. - * - * @return whether the command should run when the robot is disabled - */ - virtual bool RunsWhenDisabled() const { return false; } - - /** - * How the command behaves when another command with a shared requirement is - * scheduled. - * - * @return a variant of InterruptionBehavior, defaulting to kCancelSelf. - */ - virtual InterruptionBehavior GetInterruptionBehavior() const { - return InterruptionBehavior::kCancelSelf; - } - - /** - * Gets the name of this Command. Defaults to the simple class name if not - * overridden. - * - * @return The display name of the Command - */ - virtual std::string GetName() const; - - /** - * Sets the name of this Command. Nullop if not overridden. - * - * @param name The display name of the Command. - */ - virtual void SetName(std::string_view name); - - /** - * Transfers ownership of this command to a unique pointer. Used for - * decorator methods. - */ - /* - virtual CommandPtr ToPtr() && = 0; - */ - - protected: - /** - * Transfers ownership of this command to a unique pointer. Used for - * decorator methods. - */ - // virtual std::shared_ptr TransferOwnership() && = 0; - - bool m_isComposed = false; -}; - -/** - * Checks if two commands have disjoint requirement sets. - * - * @param first The first command to check. - * @param second The second command to check. - * @return False if first and second share a requirement. - */ -bool RequirementsDisjoint(Command* first, Command* second); - -void Command_Schedule(std::shared_ptr self); - - -} // namespace frc2 - diff --git a/commands2/src/include/frc2/command/CommandBase.h b/commands2/src/include/frc2/command/CommandBase.h deleted file mode 100644 index e4d587af..00000000 --- a/commands2/src/include/frc2/command/CommandBase.h +++ /dev/null @@ -1,124 +0,0 @@ -// 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. - -#pragma once - -#include -#include -#include -#include - -#include -#include -#include - -#include "frc2/command/Command.h" - -namespace frc2 { -/** - * A Sendable base class for Commands. - */ -class CommandBase : public Command, - public wpi::Sendable, - public wpi::SendableHelper { - public: - /** - * Adds the specified Subsystem requirements to the command. - * - * The scheduler will prevent two commands that require the same subsystem - * from being scheduled simultaneously. - * - * Note that 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 Subsystem requirements to add - */ - void AddRequirements(std::initializer_list> requirements); - - /** - * Adds the specified Subsystem requirements to the command. - * - * The scheduler will prevent two commands that require the same subsystem - * from being scheduled simultaneously. - * - * Note that 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 Subsystem requirements to add - */ - void AddRequirements(std::span> requirements); - - /** - * Adds the specified Subsystem requirements to the command. - * - * The scheduler will prevent two commands that require the same subsystem - * from being scheduled simultaneously. - * - * Note that 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 Subsystem requirements to add - */ - void AddRequirements(wpi::SmallSet, 4> requirements); - - /** - * Adds the specified Subsystem requirement to the command. - * - * The scheduler will prevent two commands that require the same subsystem - * from being scheduled simultaneously. - * - * Note that 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 requirement the Subsystem requirement to add - */ - void AddRequirements(std::shared_ptr requirement); - - /** - * Gets the Subsystem requirements of the command. - * - * @return the Command's Subsystem requirements - */ - wpi::SmallSet, 4> GetRequirements() const override; - - /** - * Sets the name of this Command. - * - * @param name name - */ - void SetName(std::string_view name) override; - - /** - * Gets the name of this Command. - * - * @return Name - */ - std::string GetName() const override; - - /** - * Gets the subsystem name of this Command. - * - * @return Subsystem name - */ - std::string GetSubsystem() const; - - /** - * Sets the subsystem name of this Command. - * - * @param subsystem subsystem name - */ - void SetSubsystem(std::string_view subsystem); - - void InitSendable(wpi::SendableBuilder& builder) override; - - protected: - CommandBase(); - public: - wpi::SmallSet, 4> m_requirements; -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/CommandGroupBase.h b/commands2/src/include/frc2/command/CommandGroupBase.h deleted file mode 100644 index b3fe550d..00000000 --- a/commands2/src/include/frc2/command/CommandGroupBase.h +++ /dev/null @@ -1,32 +0,0 @@ -// 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. - -#pragma once - -#include -#include - -#include - -#include "frc2/command/CommandBase.h" - -namespace frc2 { - -/** - * A base for CommandGroups. - * - * @deprecated This class is an empty abstraction. Inherit directly from - * CommandBase. - */ -class CommandGroupBase : public CommandBase { - public: - /** - * Adds the given commands to the command group. - * - * @param commands The commands to add. - */ - virtual void AddCommands( - std::vector>&& commands) = 0; -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/CommandHelper.h b/commands2/src/include/frc2/command/CommandHelper.h deleted file mode 100644 index 782f100a..00000000 --- a/commands2/src/include/frc2/command/CommandHelper.h +++ /dev/null @@ -1,42 +0,0 @@ -// 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. - -#pragma once - -#include -#include -#include - -#include "frc2/command/Command.h" -#include "frc2/command/CommandPtr.h" - -namespace frc2 { - -/** - * CRTP implementation to allow polymorphic decorator functions in Command. - * - *

Note: ALWAYS create a subclass by extending CommandHelper, - * or decorators will not function! - */ -template >> -class CommandHelper : public Base { - using Base::Base; - - public: - CommandHelper() = default; - - /* - CommandPtr ToPtr() && override { - return CommandPtr( - std::make_unique(std::move(*static_cast(this)))); - } - */ - - protected: - std::unique_ptr TransferOwnership() && override { - return std::make_unique(std::move(*static_cast(this))); - } -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/CommandPtr.h b/commands2/src/include/frc2/command/CommandPtr.h deleted file mode 100644 index cc61106b..00000000 --- a/commands2/src/include/frc2/command/CommandPtr.h +++ /dev/null @@ -1,306 +0,0 @@ -// 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. - -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -#include "frc2/command/CommandBase.h" - -namespace frc2 { -/** - * A wrapper around std::unique_ptr so commands have move-only - * semantics. Commands should only be stored and passed around when held in a - * CommandPtr instance. For more info, see - * https://github.com/wpilibsuite/allwpilib/issues/4303. - * - * Various classes in the command-based library accept a - * std::unique_ptr, use CommandPtr::Unwrap to convert. - * CommandPtr::UnwrapVector does the same for vectors. - */ -class CommandPtr final { - public: - explicit CommandPtr(std::unique_ptr&& command) - : m_ptr(std::move(command)) {} - - template >>> - explicit CommandPtr(T&& command) - : CommandPtr(std::make_unique>( - std::forward(command))) {} - - CommandPtr(CommandPtr&&) = default; - CommandPtr& operator=(CommandPtr&&) = default; - - /** - * Decorates this command to run repeatedly, restarting it when it ends, until - * this command is interrupted. The decorated command can still be canceled. - * - * @return the decorated command - */ - [[nodiscard]] CommandPtr Repeatedly() &&; - - /** - * Decorates this command to run "by proxy" by wrapping it in a - * ProxyCommand. This is useful for "forking off" from command groups - * when the user does not wish to extend the command's requirements to the - * entire command group. - * - * @return the decorated command - */ - [[nodiscard]] CommandPtr AsProxy() &&; - - /** - * Decorates this command to run or stop when disabled. - * - * @param doesRunWhenDisabled true to run when disabled. - * @return the decorated command - */ - [[nodiscard]] CommandPtr IgnoringDisable(bool doesRunWhenDisabled) &&; - - /** - * Decorates this command to run or stop when disabled. - * - * @param interruptBehavior true to run when disabled. - * @return the decorated command - */ - [[nodiscard]] CommandPtr WithInterruptBehavior( - Command::InterruptionBehavior interruptBehavior) &&; - - /** - * Decorates this command with a runnable to run after the command finishes. - * - * @param toRun the Runnable to run - * @param requirements the required subsystems - * @return the decorated command - */ - [[nodiscard]] CommandPtr AndThen( - std::function toRun, - std::span requirements = {}) &&; - - /** - * Decorates this command with a runnable to run after the command finishes. - * - * @param toRun the Runnable to run - * @param requirements the required subsystems - * @return the decorated command - */ - [[nodiscard]] CommandPtr AndThen( - std::function toRun, - std::initializer_list requirements) &&; - - /** - * Decorates this command with a set of commands to run after it in sequence. - * Often more convenient/less-verbose than constructing a new {@link - * SequentialCommandGroup} explicitly. - * - * @param next the commands to run next - * @return the decorated command - */ - [[nodiscard]] CommandPtr AndThen(CommandPtr&& next) &&; - - /** - * Decorates this command with a runnable to run before this command starts. - * - * @param toRun the Runnable to run - * @param requirements the required subsystems - * @return the decorated command - */ - [[nodiscard]] CommandPtr BeforeStarting( - std::function toRun, - std::initializer_list requirements) &&; - - /** - * Decorates this command with a runnable to run before this command starts. - * - * @param toRun the Runnable to run - * @param requirements the required subsystems - * @return the decorated command - */ - [[nodiscard]] CommandPtr BeforeStarting( - std::function toRun, - std::span requirements = {}) &&; - - /** - * Decorates this command with another command to run before this command - * starts. - * - * @param before the command to run before this one - * @return the decorated command - */ - [[nodiscard]] CommandPtr BeforeStarting(CommandPtr&& before) &&; - - /** - * 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. - * - * @param duration the timeout duration - * @return the command with the timeout added - */ - [[nodiscard]] CommandPtr WithTimeout(units::second_t duration) &&; - - /** - * 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 that this only applies to the - * command returned by this method; the calling command is not itself changed. - * - * @param condition the interrupt condition - * @return the command with the interrupt condition added - */ - [[nodiscard]] CommandPtr Until(std::function condition) &&; - - /** - * 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. - * - * @param condition the condition that will prevent the command from running - * @return the decorated command - */ - [[nodiscard]] CommandPtr Unless(std::function condition) &&; - - /** - * 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 {@link - * ParallelDeadlineGroup} explicitly. - * - * @param parallel the commands to run in parallel - * @return the decorated command - */ - [[nodiscard]] CommandPtr DeadlineWith(CommandPtr&& parallel) &&; - - /** - * 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. - * - * @param parallel the commands to run in parallel - * @return the decorated command - */ - [[nodiscard]] CommandPtr AlongWith(CommandPtr&& parallel) &&; - - /** - * 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. - * - * @param parallel the commands to run in parallel - * @return the decorated command - */ - [[nodiscard]] CommandPtr RaceWith(CommandPtr&& parallel) &&; - - /** - * Decorates this command with a lambda to call on interrupt or end, following - * the command's inherent Command::End(bool) method. - * - * @param end a lambda accepting a boolean parameter specifying whether the - * command was interrupted. - * @return the decorated command - */ - [[nodiscard]] CommandPtr FinallyDo(std::function end) &&; - - /** - * Decorates this command with a lambda to call on interrupt, following the - * command's inherent Command::End(bool) method. - * - * @param handler a lambda to run when the command is interrupted - * @return the decorated command - */ - [[nodiscard]] CommandPtr HandleInterrupt(std::function handler) &&; - - /** - * Decorates this Command with a name. Is an inline function for - * Command::SetName(std::string_view); - * - * @param name name - * @return the decorated Command - */ - [[nodiscard]] CommandPtr WithName(std::string_view name) &&; - - /** - * Get a raw pointer to the held command. - */ - CommandBase* get() const&; - - // Prevent calls on a temporary, as the returned pointer would be invalid - CommandBase* get() && = delete; - - /** - * Convert to the underlying unique_ptr. - */ - std::unique_ptr Unwrap() &&; - - /** - * Schedules this command. - */ - void Schedule() const&; - - // Prevent calls on a temporary, as the returned pointer would be invalid - void Schedule() && = delete; - - /** - * Cancels this command. Will call End(true). Commands will be canceled - * regardless of interruption behavior. - */ - void Cancel() const&; - - // Prevent calls on a temporary, as the returned pointer would be invalid - void Cancel() && = delete; - - /** - * Whether or not 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. - * - * @return Whether the command is scheduled. - */ - bool IsScheduled() const&; - - // Prevent calls on a temporary, as the returned pointer would be invalid - void IsScheduled() && = delete; - - /** - * Whether the command requires a given subsystem. Named "HasRequirement" - * rather than "requires" to avoid confusion with Command::Requires(Subsystem) - * -- this may be able to be changed in a few years. - * - * @param requirement the subsystem to inquire about - * @return whether the subsystem is required - */ - bool HasRequirement(Subsystem* requirement) const&; - - // Prevent calls on a temporary, as the returned pointer would be invalid - void HasRequirement(Subsystem* requirement) && = delete; - - /** - * Check if this CommandPtr object is valid and wasn't moved-from. - */ - explicit operator bool() const&; - - // Prevent calls on a temporary, as the returned pointer would be invalid - explicit operator bool() && = delete; - - /** - * Convert a vector of CommandPtr objects to their underlying unique_ptrs. - */ - static std::vector> UnwrapVector( - std::vector&& vec); - - private: - std::unique_ptr m_ptr; - void AssertValid() const; -}; - -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/CommandScheduler.h b/commands2/src/include/frc2/command/CommandScheduler.h deleted file mode 100644 index 99691ab1..00000000 --- a/commands2/src/include/frc2/command/CommandScheduler.h +++ /dev/null @@ -1,421 +0,0 @@ -// 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. - -#pragma once - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace frc2 { -class Command; -class CommandPtr; -class Subsystem; - -/** - * The scheduler responsible for running Commands. A Command-based robot should - * call 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 RegisterSubsystem() in order for their Periodic() - * methods to be called and for their default commands to be scheduled. - */ -class CommandScheduler final : public nt::NTSendable, - public wpi::SendableHelper { - public: - /** - * Returns the Scheduler instance. - * - * @return the instance - */ - static CommandScheduler& GetInstance(); - - /** - * python-specific: clear the global command scheduler instance - */ - static void ResetInstance(); - - ~CommandScheduler() override; - CommandScheduler(const CommandScheduler&) = delete; - CommandScheduler& operator=(const CommandScheduler&) = delete; - - using Action = std::function)>; - - /** - * Changes the period of the loop overrun watchdog. This should be kept in - * sync with the TimedRobot period. - */ - void SetPeriod(units::second_t period); - - /** - * Get the active button poll. - * - * @return a reference to the current {@link frc::EventLoop} object polling - * buttons. - */ - frc::EventLoop* GetActiveButtonLoop() const; - - /** - * Replace the button poll with another one. - * - * @param loop the new button polling loop object. - */ - void SetActiveButtonLoop(frc::EventLoop* loop); - - /** - * Get the default button poll. - * - * @return a reference to the default {@link frc::EventLoop} object polling - * buttons. - */ - frc::EventLoop* GetDefaultButtonLoop() const; - - /** - * Removes all button bindings from the scheduler. - */ - WPI_DEPRECATED("Call Clear on the EventLoop instance directly!") - void ClearButtons(); - - /** - * 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 are - * interruptible. If this is the case, they will be interrupted and the - * command will be scheduled. - * - * @param command the command to schedule - */ - /* - void Schedule(const CommandPtr& command); - */ - - /** - * 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. - * - * @param command the command to schedule - */ - void Schedule(std::shared_ptr command); - - /** - * Schedules multiple commands for execution. Does nothing for commands - * already scheduled. - * - * @param commands the commands to schedule - */ - void Schedule(std::span> commands); - - /** - * Schedules multiple commands for execution. Does nothing for commands - * already scheduled. - * - * @param commands the commands to schedule - */ - void Schedule(std::initializer_list> commands); - - /** - * 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. - */ - void Run(); - - /** - * 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 subsystem the subsystem to register - */ - void RegisterSubsystem(Subsystem* subsystem); - - /** - * 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 subsystem the subsystem to un-register - */ - void UnregisterSubsystem(Subsystem* subsystem); - - void RegisterSubsystem(std::initializer_list subsystems); - void RegisterSubsystem(std::span subsystems); - - void UnregisterSubsystem(std::initializer_list subsystems); - void UnregisterSubsystem(std::span subsystems); - - /** - * 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 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 - */ - template - void SetDefaultCommand(Subsystem* subsystem, T defaultCommand) { - if (!defaultCommand->HasRequirement(subsystem)) { - throw FRC_MakeError(frc::err::CommandIllegalUse, "{}", - "Default commands must require their subsystem!"); - } - SetDefaultCommandImpl(subsystem, defaultCommand); - } - - /** - * 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 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 - */ - /* - void SetDefaultCommand(Subsystem* subsystem, CommandPtr&& defaultCommand); - */ - - /** - * 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 - */ - void RemoveDefaultCommand(Subsystem* subsystem); - - /** - * 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 - * @return the default command associated with the subsystem - */ - std::shared_ptr GetDefaultCommand(const Subsystem* subsystem) const; - - /** - * Cancels commands. The scheduler will only call Command::End() - * method of the canceled command with true, indicating they were - * canceled (as opposed to finishing normally). - * - *

Commands will be canceled even if they are not scheduled as - * interruptible. - * - * @param command the command to cancel - */ - void Cancel(std::shared_ptr command); - void Cancel(Command* command); - - /** - * Cancels commands. The scheduler will only call Command::End() - * method of the canceled command with true, indicating they were - * canceled (as opposed to finishing normally). - * - *

Commands will be canceled even if they are not scheduled as - * interruptible. - * - * @param command the command to cancel - */ - /* - void Cancel(const CommandPtr& command); - */ - - /** - * Cancels commands. The scheduler will only call Command::End() - * method of the canceled command with true, indicating they were - * canceled (as opposed to finishing normally). - * - *

Commands will be canceled even if they are not scheduled as - * interruptible. - * - * @param commands the commands to cancel - */ - void Cancel(std::span> commands); - - /** - * Cancels commands. The scheduler will only call Command::End() - * method of the canceled command with true, indicating they were - * canceled (as opposed to finishing normally). - * - *

Commands will be canceled even if they are not scheduled as - * interruptible. - * - * @param commands the commands to cancel - */ - void Cancel(std::initializer_list> commands); - - /** - * Cancels all commands that are currently scheduled. - */ - void CancelAll(); - - /** - * 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 of CommandGroups, as the scheduler does not see them. - * - * @param commands the command to query - * @return whether the command is currently scheduled - */ - bool IsScheduled(std::span> commands) const; - - /** - * 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 of CommandGroups, as the scheduler does not see them. - * - * @param commands the command to query - * @return whether the command is currently scheduled - */ - bool IsScheduled(std::initializer_list> commands) const; - - /** - * Whether a given command is running. Note that this only works on commands - * that are directly scheduled by the scheduler; it will not work on commands - * inside of CommandGroups, as the scheduler does not see them. - * - * @param command the command to query - * @return whether the command is currently scheduled - */ - bool IsScheduled(std::shared_ptr command) const; - bool IsScheduled(const Command* command) const; - - /** - * Whether a given command is running. Note that this only works on commands - * that are directly scheduled by the scheduler; it will not work on commands - * inside of CommandGroups, as the scheduler does not see them. - * - * @param command the command to query - * @return whether the command is currently scheduled - */ - /* - bool IsScheduled(const CommandPtr& command) const; - */ - - /** - * Returns the command currently requiring a given subsystem. Null if no - * command is currently requiring the subsystem - * - * @param subsystem the subsystem to be inquired about - * @return the command currently requiring the subsystem - */ - std::shared_ptr Requiring(const std::shared_ptr subsystem) const; - std::shared_ptr Requiring(const Subsystem* subsystem) const; - - /** - * Disables the command scheduler. - */ - void Disable(); - - /** - * Enables the command scheduler. - */ - void Enable(); - - /** - * Adds an action to perform on the initialization of any command by the - * scheduler. - * - * @param action the action to perform - */ - void OnCommandInitialize(Action action); - - /** - * Adds an action to perform on the execution of any command by the scheduler. - * - * @param action the action to perform - */ - void OnCommandExecute(Action action); - - /** - * Adds an action to perform on the interruption of any command by the - * scheduler. - * - * @param action the action to perform - */ - void OnCommandInterrupt(Action action); - - /** - * Adds an action to perform on the finishing of any command by the scheduler. - * - * @param action the action to perform - */ - void OnCommandFinish(Action action); - - /** - * Requires that the specified command hasn't been already added to a - * composition. - * - * @param command The command to check - * @throws if the given commands have already been composed. - */ - void RequireUngrouped(const std::shared_ptr command); - - /** - * Requires that the specified commands not have been already added to a - * composition. - * - * @param commands The commands to check - * @throws if the given commands have already been composed. - */ - void RequireUngrouped(std::span> commands); - - /** - * Requires that the specified commands not have been already added to a - * composition. - * - * @param commands The commands to check - * @throws IllegalArgumentException if the given commands have already been - * composed. - */ - void RequireUngrouped(std::initializer_list> commands); - - void InitSendable(nt::NTSendableBuilder& builder) override; - - private: - // Constructor; private as this is a singleton - CommandScheduler(); - - void SetDefaultCommandImpl(Subsystem* subsystem, - std::shared_ptr command); - - class Impl; - std::unique_ptr m_impl; - - frc::Watchdog m_watchdog; - - friend class CommandTestBase; - - template - friend class CommandTestBaseWithParam; -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/Commands.h b/commands2/src/include/frc2/command/Commands.h deleted file mode 100644 index 950591f2..00000000 --- a/commands2/src/include/frc2/command/Commands.h +++ /dev/null @@ -1,274 +0,0 @@ -// 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. - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "frc2/command/CommandPtr.h" -#include "frc2/command/SelectCommand.h" - -namespace frc2 { -class Subsystem; - -/** - * Namespace for command factories. - */ -namespace cmd { - -/** - * Constructs a command that does nothing, finishing immediately. - */ -[[nodiscard]] std::shared_ptr None(); - -// Action Commands - -/** - * Constructs a command that runs an action once and finishes. - * - * @param action the action to run - * @param requirements subsystems the action requires - */ -[[nodiscard]] std::shared_ptr RunOnce( - std::function action, - std::initializer_list> requirements); - -/** - * Constructs a command that runs an action once and finishes. - * - * @param action the action to run - * @param requirements subsystems the action requires - */ -[[nodiscard]] std::shared_ptr RunOnce(std::function action, - std::span> requirements = {}); - -/** - * Constructs a command that runs an action every iteration until interrupted. - * - * @param action the action to run - * @param requirements subsystems the action requires - */ -[[nodiscard]] std::shared_ptr Run(std::function action, - std::initializer_list> requirements); - -/** - * Constructs a command that runs an action every iteration until interrupted. - * - * @param action the action to run - * @param requirements subsystems the action requires - */ -[[nodiscard]] std::shared_ptr Run(std::function action, - std::span> requirements = {}); - -/** - * 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 - */ -[[nodiscard]] std::shared_ptr StartEnd( - std::function start, std::function end, - std::initializer_list> requirements); - -/** - * 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 - */ -[[nodiscard]] std::shared_ptr StartEnd( - std::function start, std::function end, - std::span> requirements = {}); - -/** - * 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 - */ -[[nodiscard]] std::shared_ptr RunEnd(std::function run, - std::function end, - std::initializer_list> requirements); - -/** - * 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 - */ -[[nodiscard]] std::shared_ptr RunEnd(std::function run, - std::function end, - std::span> requirements = {}); - -/** - * Constructs a command that prints a message and finishes. - * - * @param msg the message to print - */ -[[nodiscard]] std::shared_ptr Print(std::string_view msg); - -// Idling Commands - -/** - * Constructs a command that does nothing, finishing after a specified duration. - * - * @param duration after how long the command finishes - */ -[[nodiscard]] std::shared_ptr Wait(units::second_t duration); - -/** - * Constructs a command that does nothing, finishing once a condition becomes - * true. - * - * @param condition the condition - */ -[[nodiscard]] std::shared_ptr WaitUntil(std::function condition); - -// Selector Commands - -/** - * 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 - */ -[[nodiscard]] std::shared_ptr Either(std::shared_ptr onTrue, std::shared_ptr onFalse, - std::function selector); - -/** - * Runs one of several commands, based on the selector function. - * - * @param selector the selector function - * @param commands map of commands to select from - */ -template -[[nodiscard]] std::shared_ptr Select( - std::function selector, - std::vector>> commands) { - return std::make_shared>(std::move(selector), std::move(commands)); -} - -// Command Groups - -namespace impl { - -/** - * Create a vector of commands. - */ -/* -template -std::vector MakeVector(Args&&... args) { - std::vector data; - data.reserve(sizeof...(Args)); - (data.emplace_back(std::forward(args)), ...); - return data; -} -*/ - -} // namespace impl - -/** - * Runs a group of commands in series, one after the other. - */ -[[nodiscard]] std::shared_ptr Sequence(std::vector>&& commands); - -/** - * Runs a group of commands in series, one after the other. - */ -/* -template -[[nodiscard]] CommandPtr Sequence(Args&&... commands) { - return Sequence(impl::MakeVector(std::forward(commands)...)); -} -*/ - -/** - * Runs a group of commands in series, one after the other. Once the last - * command ends, the group is restarted. - */ -[[nodiscard]] std::shared_ptr RepeatingSequence(std::vector>&& commands); - -/** - * Runs a group of commands in series, one after the other. Once the last - * command ends, the group is restarted. - */ -/* -template -[[nodiscard]] CommandPtr RepeatingSequence(Args&&... commands) { - return RepeatingSequence(impl::MakeVector(std::forward(commands)...)); -} -*/ - -/** - * Runs a group of commands at the same time. Ends once all commands in the - * group finish. - */ -[[nodiscard]] std::shared_ptr Parallel(std::vector>&& commands); - -/** - * Runs a group of commands at the same time. Ends once all commands in the - * group finish. - */ -/* -template -[[nodiscard]] CommandPtr Parallel(Args&&... commands) { - return Parallel(impl::MakeVector(std::forward(commands)...)); -} -*/ - -/** - * Runs a group of commands at the same time. Ends once any command in the group - * finishes, and cancels the others. - */ -[[nodiscard]] std::shared_ptr Race(std::vector>&& commands); - -/** - * Runs a group of commands at the same time. Ends once any command in the group - * finishes, and cancels the others. - */ -/* -template -[[nodiscard]] CommandPtr Race(Args&&... commands) { - return Race(impl::MakeVector(std::forward(commands)...)); -} -*/ - -/** - * Runs a group of commands at the same time. Ends once a specific command - * finishes, and cancels the others. - */ -[[nodiscard]] std::shared_ptr Deadline(std::shared_ptr deadline, - std::vector>&& others); - -/** - * Runs a group of commands at the same time. Ends once a specific command - * finishes, and cancels the others. - */ -/* -template -[[nodiscard]] CommandPtr Deadline(CommandPtr&& deadline, Args&&... commands) { - return Deadline(std::move(deadline), - impl::MakeVector(std::forward(commands)...)); -} -*/ - -} // namespace cmd - -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/ConditionalCommand.h b/commands2/src/include/frc2/command/ConditionalCommand.h deleted file mode 100644 index f64f15e6..00000000 --- a/commands2/src/include/frc2/command/ConditionalCommand.h +++ /dev/null @@ -1,82 +0,0 @@ -// 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. - -#pragma once - -#include -#include -#include - -#include "frc2/command/CommandBase.h" -#include "frc2/command/CommandHelper.h" - -namespace frc2 { -/** - * 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 are owned by the composition and cannot be added to any other - * composition or scheduled individually, and the composition requires all - * subsystems its components require. - * - * @see ScheduleCommand - */ -class ConditionalCommand : public CommandBase { - public: - /** - * 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 - */ - template >>, - typename = std::enable_if_t< - std::is_base_of_v>>> - ConditionalCommand(T1&& onTrue, T2&& onFalse, std::function condition) - : ConditionalCommand(std::make_shared>( - std::forward(onTrue)), - std::make_shared>( - std::forward(onFalse)), - condition) {} - - /** - * 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 - */ - ConditionalCommand(std::shared_ptr onTrue, - std::shared_ptr onFalse, - std::function condition); - - ConditionalCommand(ConditionalCommand&& other) = default; - - // No copy constructors for command groups - ConditionalCommand(const ConditionalCommand& other) = delete; - - void Initialize() override; - - void Execute() override; - - void End(bool interrupted) override; - - bool IsFinished() override; - - bool RunsWhenDisabled() const override; - - void InitSendable(wpi::SendableBuilder& builder) override; - - private: - std::shared_ptr m_onTrue; - std::shared_ptr m_onFalse; - std::function m_condition; - std::shared_ptr m_selectedCommand{nullptr}; - bool m_runsWhenDisabled = true; -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/DenseMapInfo.h b/commands2/src/include/frc2/command/DenseMapInfo.h deleted file mode 100644 index 14a395d3..00000000 --- a/commands2/src/include/frc2/command/DenseMapInfo.h +++ /dev/null @@ -1,81 +0,0 @@ - -#pragma once - -#include -#include - -#include -#include - -#include - -namespace wpi { - -// Provide DenseMapInfo for command and subsystem -template<> -struct DenseMapInfo> { - - using T = frc2::Command; - - static inline std::shared_ptr getEmptyKey() { - static auto empty = std::make_shared(); - return empty; - } - - static inline std::shared_ptr getTombstoneKey() { - static auto tombstone = std::make_shared(); - return tombstone; - } - - static unsigned getHashValue(const std::shared_ptr PtrVal) { - return (unsigned((uintptr_t)PtrVal.get()) >> 4) ^ - (unsigned((uintptr_t)PtrVal.get()) >> 9); - } - - static bool isEqual(const std::shared_ptr LHS, const std::shared_ptr RHS) { return LHS == RHS; } - - // support find_as - - static unsigned getHashValue(const T* PtrVal) { - return (unsigned((uintptr_t)PtrVal) >> 4) ^ - (unsigned((uintptr_t)PtrVal) >> 9); - } - - static bool isEqual(const T* LHS, const std::shared_ptr RHS) { return LHS == RHS.get(); } - -}; - -template<> -struct DenseMapInfo> { - - using T = frc2::Subsystem; - - static inline std::shared_ptr getEmptyKey() { - static auto empty = std::make_shared(); - return empty; - } - - static inline std::shared_ptr getTombstoneKey() { - static auto tombstone = std::make_shared(); - return tombstone; - } - - static unsigned getHashValue(const std::shared_ptr PtrVal) { - return (unsigned((uintptr_t)PtrVal.get()) >> 4) ^ - (unsigned((uintptr_t)PtrVal.get()) >> 9); - } - - static bool isEqual(const std::shared_ptr LHS, const std::shared_ptr RHS) { return LHS == RHS; } - - // support find_as - - static unsigned getHashValue(const T* PtrVal) { - return (unsigned((uintptr_t)PtrVal) >> 4) ^ - (unsigned((uintptr_t)PtrVal) >> 9); - } - - static bool isEqual(const T* LHS, const std::shared_ptr RHS) { return LHS == RHS.get(); } -}; - - -}; // namespace wpi \ No newline at end of file diff --git a/commands2/src/include/frc2/command/FunctionalCommand.h b/commands2/src/include/frc2/command/FunctionalCommand.h deleted file mode 100644 index a898ab6b..00000000 --- a/commands2/src/include/frc2/command/FunctionalCommand.h +++ /dev/null @@ -1,74 +0,0 @@ -// 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. - -#pragma once - -#include -#include -#include - -#include "frc2/command/CommandBase.h" -#include "frc2/command/CommandHelper.h" - -namespace frc2 { -/** - * 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. - */ -class FunctionalCommand : public CommandBase { - public: - /** - * 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 - */ - FunctionalCommand(std::function onInit, - std::function onExecute, - std::function onEnd, - std::function isFinished, - std::initializer_list> requirements); - - /** - * 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 - */ - FunctionalCommand(std::function onInit, - std::function onExecute, - std::function onEnd, - std::function isFinished, - std::span> requirements = {}); - - FunctionalCommand(FunctionalCommand&& other) = default; - - FunctionalCommand(const FunctionalCommand& other) = default; - - void Initialize() override; - - void Execute() override; - - void End(bool interrupted) override; - - bool IsFinished() override; - - private: - std::function m_onInit; - std::function m_onExecute; - std::function m_onEnd; - std::function m_isFinished; -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/InstantCommand.h b/commands2/src/include/frc2/command/InstantCommand.h deleted file mode 100644 index a7619c71..00000000 --- a/commands2/src/include/frc2/command/InstantCommand.h +++ /dev/null @@ -1,52 +0,0 @@ -// 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. - -#pragma once - -#include -#include -#include - -#include "frc2/command/CommandHelper.h" -#include "frc2/command/FunctionalCommand.h" - -namespace frc2 { -/** - * 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 Runnable and - * a set of requirements, or else subclass this command if desired. - */ -class InstantCommand : public FunctionalCommand { - public: - /** - * Creates a new InstantCommand that runs the given Runnable with the given - * requirements. - * - * @param toRun the Runnable to run - * @param requirements the subsystems required by this command - */ - InstantCommand(std::function toRun, - std::initializer_list> requirements); - - /** - * Creates a new InstantCommand that runs the given Runnable with the given - * requirements. - * - * @param toRun the Runnable to run - * @param requirements the subsystems required by this command - */ - explicit InstantCommand(std::function toRun, - std::span> requirements = {}); - - InstantCommand(InstantCommand&& other) = default; - - InstantCommand(const InstantCommand& other) = default; - - /** - * Creates a new InstantCommand with a Runnable that does nothing. Useful - * only as a no-arg constructor to call implicitly from subclass constructors. - */ - InstantCommand(); -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/MecanumControllerCommand.h b/commands2/src/include/frc2/command/MecanumControllerCommand.h deleted file mode 100644 index 1e77f9e2..00000000 --- a/commands2/src/include/frc2/command/MecanumControllerCommand.h +++ /dev/null @@ -1,451 +0,0 @@ -// 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. - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "CommandBase.h" -#include "CommandHelper.h" - -#pragma once - -namespace frc2 { -/** - * A command that uses two PID controllers (PIDController) and a profiled PID - * controller (ProfiledPIDController) to follow a trajectory (Trajectory) with a - * mecanum drive. - * - *

The command handles trajectory-following, - * Velocity PID calculations, and feedforwards internally. This - * is intended to be a more-or-less "complete solution" that can be used by - * teams without a great deal of controls expertise. - * - *

Advanced teams seeking more flexibility (for example, those who wish to - * use the onboard PID functionality of a "smart" motor controller) may use the - * secondary constructor that omits the PID and feedforward functionality, - * returning only the raw wheel speeds from the PID controllers. - * - *

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. - */ -class MecanumControllerCommand - : public CommandBase { - public: - /** - * Constructs a new MecanumControllerCommand that when executed will follow - * the provided trajectory. PID control and feedforward are handled - * internally. Outputs are scaled from -12 to 12 as a voltage output to the - * motor. - * - *

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, - * provided by the odometry class. - * @param feedforward The feedforward to use for the drivetrain. - * @param kinematics The kinematics for the robot drivetrain. - * @param xController The Trajectory Tracker PID controller - * for the robot's x position. - * @param yController The Trajectory Tracker PID controller - * for the robot's y position. - * @param thetaController The Trajectory Tracker PID controller - * for angle for the robot. - * @param desiredRotation The angle that the robot should be facing. - * This is sampled at each time step. - * @param maxWheelVelocity The maximum velocity of a drivetrain wheel. - * @param frontLeftController The front left wheel velocity PID. - * @param rearLeftController The rear left wheel velocity PID. - * @param frontRightController The front right wheel velocity PID. - * @param rearRightController The rear right wheel velocity PID. - * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing - * the current wheel speeds. - * @param output The output of the velocity PIDs. - * @param requirements The subsystems to require. - */ - MecanumControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SimpleMotorFeedforward feedforward, - frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, - frc2::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function desiredRotation, - units::meters_per_second_t maxWheelVelocity, - std::function currentWheelSpeeds, - frc2::PIDController frontLeftController, - frc2::PIDController rearLeftController, - frc2::PIDController frontRightController, - frc2::PIDController rearRightController, - std::function - output, - std::initializer_list> requirements); - - /** - * Constructs a new MecanumControllerCommand that when executed will follow - * the provided trajectory. PID control and feedforward are handled - * internally. Outputs are scaled from -12 to 12 as a voltage output to the - * motor. - * - *

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. - * - *

Note 2: The final rotation of the robot will be set to the rotation of - * the final pose in the trajectory. The robot will not follow the rotations - * from the poses at each timestep. If alternate rotation behavior is desired, - * the other constructor with a supplier for rotation should be used. - * - * @param trajectory The trajectory to follow. - * @param pose A function that supplies the robot pose, - * provided by the odometry class. - * @param feedforward The feedforward to use for the drivetrain. - * @param kinematics The kinematics for the robot drivetrain. - * @param xController The Trajectory Tracker PID controller - * for the robot's x position. - * @param yController The Trajectory Tracker PID controller - * for the robot's y position. - * @param thetaController The Trajectory Tracker PID controller - * for angle for the robot. - * @param maxWheelVelocity The maximum velocity of a drivetrain wheel. - * @param frontLeftController The front left wheel velocity PID. - * @param rearLeftController The rear left wheel velocity PID. - * @param frontRightController The front right wheel velocity PID. - * @param rearRightController The rear right wheel velocity PID. - * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing - * the current wheel speeds. - * @param output The output of the velocity PIDs. - * @param requirements The subsystems to require. - */ - MecanumControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SimpleMotorFeedforward feedforward, - frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, - frc2::PIDController yController, - frc::ProfiledPIDController thetaController, - units::meters_per_second_t maxWheelVelocity, - std::function currentWheelSpeeds, - frc2::PIDController frontLeftController, - frc2::PIDController rearLeftController, - frc2::PIDController frontRightController, - frc2::PIDController rearRightController, - std::function - output, - std::initializer_list> requirements); - - /** - * Constructs a new MecanumControllerCommand that when executed will follow - * the provided trajectory. PID control and feedforward are handled - * internally. Outputs are scaled from -12 to 12 as a voltage output to the - * motor. - * - *

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, - * provided by the odometry class. - * @param feedforward The feedforward to use for the drivetrain. - * @param kinematics The kinematics for the robot drivetrain. - * @param xController The Trajectory Tracker PID controller - * for the robot's x position. - * @param yController The Trajectory Tracker PID controller - * for the robot's y position. - * @param thetaController The Trajectory Tracker PID controller - * for angle for the robot. - * @param desiredRotation The angle that the robot should be facing. - * This is sampled at each time step. - * @param maxWheelVelocity The maximum velocity of a drivetrain wheel. - * @param frontLeftController The front left wheel velocity PID. - * @param rearLeftController The rear left wheel velocity PID. - * @param frontRightController The front right wheel velocity PID. - * @param rearRightController The rear right wheel velocity PID. - * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing - * the current wheel speeds. - * @param output The output of the velocity PIDs. - * @param requirements The subsystems to require. - */ - MecanumControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SimpleMotorFeedforward feedforward, - frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, - frc2::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function desiredRotation, - units::meters_per_second_t maxWheelVelocity, - std::function currentWheelSpeeds, - frc2::PIDController frontLeftController, - frc2::PIDController rearLeftController, - frc2::PIDController frontRightController, - frc2::PIDController rearRightController, - std::function - output, - std::span> requirements = {}); - - /** - * Constructs a new MecanumControllerCommand that when executed will follow - * the provided trajectory. PID control and feedforward are handled - * internally. Outputs are scaled from -12 to 12 as a voltage output to the - * motor. - * - *

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. - * - *

Note 2: The final rotation of the robot will be set to the rotation of - * the final pose in the trajectory. The robot will not follow the rotations - * from the poses at each timestep. If alternate rotation behavior is desired, - * the other constructor with a supplier for rotation should be used. - * - * @param trajectory The trajectory to follow. - * @param pose A function that supplies the robot pose, - * provided by the odometry class. - * @param feedforward The feedforward to use for the drivetrain. - * @param kinematics The kinematics for the robot drivetrain. - * @param xController The Trajectory Tracker PID controller - * for the robot's x position. - * @param yController The Trajectory Tracker PID controller - * for the robot's y position. - * @param thetaController The Trajectory Tracker PID controller - * for angle for the robot. - * @param maxWheelVelocity The maximum velocity of a drivetrain wheel. - * @param frontLeftController The front left wheel velocity PID. - * @param rearLeftController The rear left wheel velocity PID. - * @param frontRightController The front right wheel velocity PID. - * @param rearRightController The rear right wheel velocity PID. - * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing - * the current wheel speeds. - * @param output The output of the velocity PIDs. - * @param requirements The subsystems to require. - */ - MecanumControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SimpleMotorFeedforward feedforward, - frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, - frc2::PIDController yController, - frc::ProfiledPIDController thetaController, - units::meters_per_second_t maxWheelVelocity, - std::function currentWheelSpeeds, - frc2::PIDController frontLeftController, - frc2::PIDController rearLeftController, - frc2::PIDController frontRightController, - frc2::PIDController rearRightController, - std::function - output, - std::span> requirements = {}); - - /** - * Constructs a new MecanumControllerCommand that when executed will follow - * the provided trajectory. The user should implement a velocity PID on the - * desired output wheel velocities. - * - *

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 end-states. - * - * @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. - * @param xController The Trajectory Tracker PID controller - * for the robot's x position. - * @param yController The Trajectory Tracker PID controller - * for the robot's y position. - * @param thetaController The Trajectory Tracker PID controller - * for angle for the robot. - * @param desiredRotation The angle that the robot should be facing. - * This is sampled at each time step. - * @param maxWheelVelocity The maximum velocity of a drivetrain wheel. - * @param output The output of the position PIDs. - * @param requirements The subsystems to require. - */ - MecanumControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, - frc2::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function desiredRotation, - units::meters_per_second_t maxWheelVelocity, - std::function - output, - std::initializer_list> requirements); - - /** - * Constructs a new MecanumControllerCommand that when executed will follow - * the provided trajectory. The user should implement a velocity PID on the - * desired output wheel velocities. - * - *

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 end-states. - * - *

Note 2: The final rotation of the robot will be set to the rotation of - * the final pose in the trajectory. The robot will not follow the rotations - * from the poses at each timestep. If alternate rotation behavior is desired, - * the other constructor with a supplier for rotation should be used. - * - * @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. - * @param xController The Trajectory Tracker PID controller - * for the robot's x position. - * @param yController The Trajectory Tracker PID controller - * for the robot's y position. - * @param thetaController The Trajectory Tracker PID controller - * for angle for the robot. - * @param maxWheelVelocity The maximum velocity of a drivetrain wheel. - * @param output The output of the position PIDs. - * @param requirements The subsystems to require. - */ - MecanumControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, - frc2::PIDController yController, - frc::ProfiledPIDController thetaController, - units::meters_per_second_t maxWheelVelocity, - std::function - output, - std::initializer_list> requirements); - - /** - * Constructs a new MecanumControllerCommand that when executed will follow - * the provided trajectory. The user should implement a velocity PID on the - * desired output wheel velocities. - * - *

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 end-states. - * - * @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. - * @param xController The Trajectory Tracker PID controller - * for the robot's x position. - * @param yController The Trajectory Tracker PID controller - * for the robot's y position. - * @param thetaController The Trajectory Tracker PID controller - * for angle for the robot. - * @param desiredRotation The angle that the robot should be facing. - * This is sampled at every time step. - * @param maxWheelVelocity The maximum velocity of a drivetrain wheel. - * @param output The output of the position PIDs. - * @param requirements The subsystems to require. - */ - MecanumControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, - frc2::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function desiredRotation, - units::meters_per_second_t maxWheelVelocity, - std::function - output, - std::span> requirements = {}); - - /** - * Constructs a new MecanumControllerCommand that when executed will follow - * the provided trajectory. The user should implement a velocity PID on the - * desired output wheel velocities. - * - *

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 end-states. - * - *

Note2: The final rotation of the robot will be set to the rotation of - * the final pose in the trajectory. The robot will not follow the rotations - * from the poses at each timestep. If alternate rotation behavior is desired, - * the other constructor with a supplier for rotation should be used. - * - * @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. - * @param xController The Trajectory Tracker PID controller - * for the robot's x position. - * @param yController The Trajectory Tracker PID controller - * for the robot's y position. - * @param thetaController The Trajectory Tracker PID controller - * for angle for the robot. - * @param maxWheelVelocity The maximum velocity of a drivetrain wheel. - * @param output The output of the position PIDs. - * @param requirements The subsystems to require. - */ - MecanumControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, - frc2::PIDController yController, - frc::ProfiledPIDController thetaController, - units::meters_per_second_t maxWheelVelocity, - std::function - output, - std::span> requirements = {}); - - void Initialize() override; - - void Execute() override; - - void End(bool interrupted) override; - - bool IsFinished() override; - - private: - frc::Trajectory m_trajectory; - std::function m_pose; - frc::SimpleMotorFeedforward m_feedforward; - frc::MecanumDriveKinematics m_kinematics; - frc::HolonomicDriveController m_controller; - std::function m_desiredRotation; - const units::meters_per_second_t m_maxWheelVelocity; - std::unique_ptr m_frontLeftController; - std::unique_ptr m_rearLeftController; - std::unique_ptr m_frontRightController; - std::unique_ptr m_rearRightController; - std::function m_currentWheelSpeeds; - std::function - m_outputVel; - std::function - m_outputVolts; - - bool m_usePID; - frc::Timer m_timer; - frc::MecanumDriveWheelSpeeds m_prevSpeeds; - units::second_t m_prevTime; -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/NotifierCommand.h b/commands2/src/include/frc2/command/NotifierCommand.h deleted file mode 100644 index a415d839..00000000 --- a/commands2/src/include/frc2/command/NotifierCommand.h +++ /dev/null @@ -1,63 +0,0 @@ -// 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. - -#pragma once - -#include -#include -#include - -#include -#include - -#include "frc2/command/CommandBase.h" -#include "frc2/command/CommandHelper.h" - -namespace frc2 { -/** - * A command that starts a notifier to run the given runnable periodically in a - * separate thread. Has no end condition as-is; either subclass it or use - * Command::WithTimeout(double) or Command::Until(BooleanSupplier) 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. - */ -class NotifierCommand : public CommandBase { - public: - /** - * Creates a new NotifierCommand. - * - * @param toRun the runnable for the notifier to run - * @param period the period at which the notifier should run - * @param requirements the subsystems required by this command - */ - NotifierCommand(std::function toRun, units::second_t period, - std::initializer_list> requirements); - - /** - * Creates a new NotifierCommand. - * - * @param toRun the runnable for the notifier to run - * @param period the period at which the notifier should run - * @param requirements the subsystems required by this command - */ - NotifierCommand(std::function toRun, units::second_t period, - std::span> requirements = {}); - - NotifierCommand(NotifierCommand&& other); - - NotifierCommand(const NotifierCommand& other); - - void Initialize() override; - - void End(bool interrupted) override; - - private: - std::function m_toRun; - frc::Notifier m_notifier; - units::second_t m_period; -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/PIDCommand.h b/commands2/src/include/frc2/command/PIDCommand.h deleted file mode 100644 index d61998e4..00000000 --- a/commands2/src/include/frc2/command/PIDCommand.h +++ /dev/null @@ -1,112 +0,0 @@ -// 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. - -#pragma once - -#include -#include -#include - -#include - -#include "frc2/command/CommandBase.h" -#include "frc2/command/CommandHelper.h" - -namespace frc2 { -/** - * A command that controls an output with a 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. - * - * @see PIDController - */ -class PIDCommand : public CommandBase { - public: - /** - * Creates a new PIDCommand, which controls the given output with a - * PIDController. - * - * @param controller the controller that controls the output. - * @param measurementSource the measurement of the process variable - * @param setpointSource the controller's reference (aka setpoint) - * @param useOutput the controller's output - * @param requirements the subsystems required by this command - */ - PIDCommand(PIDController controller, - std::function measurementSource, - std::function setpointSource, - std::function useOutput, - std::initializer_list> requirements); - - /** - * Creates a new PIDCommand, which controls the given output with a - * PIDController. - * - * @param controller the controller that controls the output. - * @param measurementSource the measurement of the process variable - * @param setpointSource the controller's reference (aka setpoint) - * @param useOutput the controller's output - * @param requirements the subsystems required by this command - */ - PIDCommand(PIDController controller, - std::function measurementSource, - std::function setpointSource, - std::function useOutput, - std::span> requirements = {}); - - /** - * Creates a new PIDCommand, which controls the given output with a - * PIDController with a constant setpoint. - * - * @param controller the controller that controls the output. - * @param measurementSource the measurement of the process variable - * @param setpoint the controller's setpoint (aka setpoint) - * @param useOutput the controller's output - * @param requirements the subsystems required by this command - */ - PIDCommand(PIDController controller, - std::function measurementSource, double setpoint, - std::function useOutput, - std::initializer_list> requirements); - - /** - * Creates a new PIDCommand, which controls the given output with a - * PIDController with a constant setpoint. - * - * @param controller the controller that controls the output. - * @param measurementSource the measurement of the process variable - * @param setpoint the controller's setpoint (aka setpoint) - * @param useOutput the controller's output - * @param requirements the subsystems required by this command - */ - PIDCommand(PIDController controller, - std::function measurementSource, double setpoint, - std::function useOutput, - std::span> requirements = {}); - - PIDCommand(PIDCommand&& other) = default; - - PIDCommand(const PIDCommand& other) = default; - - void Initialize() override; - - void Execute() override; - - void End(bool interrupted) override; - - /** - * Returns the PIDController used by the command. - * - * @return The PIDController - */ - PIDController& GetController(); - - protected: - PIDController m_controller; - std::function m_measurement; - std::function m_setpoint; - std::function m_useOutput; -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/PIDSubsystem.h b/commands2/src/include/frc2/command/PIDSubsystem.h deleted file mode 100644 index aea12359..00000000 --- a/commands2/src/include/frc2/command/PIDSubsystem.h +++ /dev/null @@ -1,87 +0,0 @@ -// 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. - -#pragma once - -#include - -#include "frc2/command/SubsystemBase.h" - -namespace frc2 { -/** - * A subsystem that uses a PIDController to control an output. The controller - * is run synchronously from the subsystem's periodic() method. - * - * @see PIDController - */ -class PIDSubsystem : public SubsystemBase { - public: - /** - * Creates a new PIDSubsystem. - * - * @param controller the PIDController to use - * @param initialPosition the initial setpoint of the subsystem - */ - explicit PIDSubsystem(PIDController controller, double initialPosition = 0); - - void Periodic() override; - - /** - * Sets the setpoint for the subsystem. - * - * @param setpoint the setpoint for the subsystem - */ - void SetSetpoint(double setpoint); - - /** - * Gets the setpoint for the subsystem. - * - * @return the setpoint for the subsystem - */ - double GetSetpoint() const; - - /** - * Enables the PID control. Resets the controller. - */ - virtual void Enable(); - - /** - * Disables the PID control. Sets output to zero. - */ - virtual void Disable(); - - /** - * Returns whether the controller is enabled. - * - * @return Whether the controller is enabled. - */ - bool IsEnabled(); - - /** - * Returns the PIDController. - * - * @return The controller. - */ - PIDController& GetController(); - - protected: - PIDController m_controller; - bool m_enabled{false}; - - /** - * Returns the measurement of the process variable used by the PIDController. - * - * @return the measurement of the process variable - */ - virtual double GetMeasurement() = 0; - - /** - * Uses the output from the PIDController. - * - * @param output the output of the PIDController - * @param setpoint the setpoint of the PIDController (for feedforward) - */ - virtual void UseOutput(double output, double setpoint) = 0; -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/ParallelCommandGroup.h b/commands2/src/include/frc2/command/ParallelCommandGroup.h deleted file mode 100644 index a0b1848a..00000000 --- a/commands2/src/include/frc2/command/ParallelCommandGroup.h +++ /dev/null @@ -1,103 +0,0 @@ -// 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. - -#pragma once - -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable : 4521) -#endif - -#include -#include -#include - -#include "frc2/command/CommandGroupBase.h" -#include "frc2/command/CommandHelper.h" - -namespace frc2 { -/** - * 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 are owned by the composition and cannot be added to any other - * composition or scheduled individually, and the composition requires all - * subsystems its components require. - */ -class ParallelCommandGroup - : public CommandGroupBase { - public: - /** - * Creates a new ParallelCommandGroup. The given commands will be executed - * simultaneously. The command group 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. - */ - explicit ParallelCommandGroup( - std::vector>&& commands); - - /** - * Creates a new ParallelCommandGroup. The given commands will be executed - * simultaneously. The command group 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. - */ - template >...>>> - explicit ParallelCommandGroup(Types&&... commands) { - AddCommands(std::forward(commands)...); - } - - ParallelCommandGroup(ParallelCommandGroup&& other) = default; - - // No copy constructors for command groups - ParallelCommandGroup(const ParallelCommandGroup&) = delete; - - // Prevent template expansion from emulating copy ctor - ParallelCommandGroup(ParallelCommandGroup&) = delete; - - template >...>>> - void AddCommands(Types&&... commands) { - std::vector> foo; - ((void)foo.emplace_back(std::make_shared>( - std::forward(commands))), - ...); - AddCommands(std::move(foo)); - } - - void Initialize() final; - - void Execute() final; - - void End(bool interrupted) final; - - bool IsFinished() final; - - bool RunsWhenDisabled() const override; - - Command::InterruptionBehavior GetInterruptionBehavior() const override; - - public: - void AddCommands(std::vector>&& commands) final; - - private: - std::vector, bool>> m_commands; - bool m_runWhenDisabled{true}; - Command::InterruptionBehavior m_interruptBehavior{ - Command::InterruptionBehavior::kCancelIncoming}; - bool isRunning = false; -}; -} // namespace frc2 - -#ifdef _WIN32 -#pragma warning(pop) -#endif diff --git a/commands2/src/include/frc2/command/ParallelDeadlineGroup.h b/commands2/src/include/frc2/command/ParallelDeadlineGroup.h deleted file mode 100644 index 647098d4..00000000 --- a/commands2/src/include/frc2/command/ParallelDeadlineGroup.h +++ /dev/null @@ -1,123 +0,0 @@ -// 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. - -#pragma once - -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable : 4521) -#endif - -#include -#include -#include - -#include "frc2/command/CommandGroupBase.h" -#include "frc2/command/CommandHelper.h" - -namespace frc2 { -/** - * 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. - * -<<<<<<< HEAD - *

As a rule, CommandGroups require the union of the requirements of their - * component commands. -======= - *

The rules for command compositions apply: command instances that are - * passed to it are owned by the composition and cannot be added to any other - * composition or scheduled individually, and the composition requires all - * subsystems its components require. - * - * This class is provided by the NewCommands VendorDep ->>>>>>> upstream - */ -class ParallelDeadlineGroup - : public CommandGroupBase { - public: - /** - * 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 - */ - ParallelDeadlineGroup(std::shared_ptr deadline, - std::vector>&& commands); - /** - * 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 - */ - template >>, - typename = std::enable_if_t>...>>> - explicit ParallelDeadlineGroup(T&& deadline, Types&&... commands) { - SetDeadline(std::make_shared>( - std::forward(deadline))); - AddCommands(std::forward(commands)...); - } - - ParallelDeadlineGroup(ParallelDeadlineGroup&& other) = default; - - // No copy constructors for command groups - ParallelDeadlineGroup(const ParallelDeadlineGroup&) = delete; - - // Prevent template expansion from emulating copy ctor - ParallelDeadlineGroup(ParallelDeadlineGroup&) = delete; - - template >...>>> - void AddCommands(Types&&... commands) { - std::vector> foo; - ((void)foo.emplace_back(std::make_shared>( - std::forward(commands))), - ...); - AddCommands(std::move(foo)); - } - - void Initialize() final; - - void Execute() final; - - void End(bool interrupted) final; - - bool IsFinished() final; - - bool RunsWhenDisabled() const override; - - Command::InterruptionBehavior GetInterruptionBehavior() const override; - - void InitSendable(wpi::SendableBuilder& builder) override; - - public: - void AddCommands(std::vector>&& commands) final; - - private: - void SetDeadline(std::shared_ptr deadline); - - std::vector, bool>> m_commands; - std::shared_ptr m_deadline; - bool m_runWhenDisabled{true}; - Command::InterruptionBehavior m_interruptBehavior{ - Command::InterruptionBehavior::kCancelIncoming}; - bool m_finished{true}; -}; -} // namespace frc2 - -#ifdef _WIN32 -#pragma warning(pop) -#endif diff --git a/commands2/src/include/frc2/command/ParallelRaceGroup.h b/commands2/src/include/frc2/command/ParallelRaceGroup.h deleted file mode 100644 index 76ddfeeb..00000000 --- a/commands2/src/include/frc2/command/ParallelRaceGroup.h +++ /dev/null @@ -1,92 +0,0 @@ -// 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. - -#pragma once - -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable : 4521) -#endif - -#include -#include -#include - -#include "frc2/command/CommandGroupBase.h" -#include "frc2/command/CommandHelper.h" - -namespace frc2 { -/** - * 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 are owned by the composition and cannot be added to any other - * composition or scheduled individually, and the composition requires all - * subsystems its components require. - */ -class ParallelRaceGroup - : public CommandGroupBase { - public: - /** - * 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. - */ - explicit ParallelRaceGroup(std::vector>&& commands); - - template >...>>> - explicit ParallelRaceGroup(Types&&... commands) { - AddCommands(std::forward(commands)...); - } - - ParallelRaceGroup(ParallelRaceGroup&& other) = default; - - // No copy constructors for command groups - ParallelRaceGroup(const ParallelRaceGroup&) = delete; - - // Prevent template expansion from emulating copy ctor - ParallelRaceGroup(ParallelRaceGroup&) = delete; - - template - void AddCommands(Types&&... commands) { - std::vector> foo; - ((void)foo.emplace_back(std::make_shared>( - std::forward(commands))), - ...); - AddCommands(std::move(foo)); - } - - void Initialize() final; - - void Execute() final; - - void End(bool interrupted) final; - - bool IsFinished() final; - - bool RunsWhenDisabled() const override; - - Command::InterruptionBehavior GetInterruptionBehavior() const override; - - public: - void AddCommands(std::vector>&& commands) final; - - private: - std::vector> m_commands; - bool m_runWhenDisabled{true}; - Command::InterruptionBehavior m_interruptBehavior{ - Command::InterruptionBehavior::kCancelIncoming}; - bool m_finished{false}; - bool isRunning = false; -}; -} // namespace frc2 - -#ifdef _WIN32 -#pragma warning(pop) -#endif diff --git a/commands2/src/include/frc2/command/PerpetualCommand.h b/commands2/src/include/frc2/command/PerpetualCommand.h deleted file mode 100644 index f995c599..00000000 --- a/commands2/src/include/frc2/command/PerpetualCommand.h +++ /dev/null @@ -1,98 +0,0 @@ -// 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. - -#pragma once - -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable : 4521) -#endif - -#include -#include - -#include "frc2/command/CommandBase.h" -#include "frc2/command/CommandHelper.h" - -namespace frc2 { -/** - * A command that runs another command in perpetuity, ignoring that command's - * end conditions. While this class does not extend frc2::CommandGroupBase, - * it is still considered a CommandGroup, as it allows one to compose another - * command within it; the command instances that are passed to it cannot be - * added to any other groups, or scheduled individually. - * - *

As a rule, CommandGroups require the union of the requirements of their - * component commands. - * - * This class is provided by the NewCommands VendorDep - * - * @deprecated PerpetualCommand violates the assumption that execute() doesn't -get called after isFinished() returns true -- an assumption that should be -valid. This was unsafe/undefined behavior from the start, and RepeatCommand -provides an easy way to achieve similar end results with slightly different (and -safe) semantics. - */ -class PerpetualCommand : public CommandBase { - public: - /** - * Creates a new PerpetualCommand. Will run another command in perpetuity, - * ignoring that command's end conditions, unless this command itself is - * interrupted. - * - * @param command the command to run perpetually - */ - WPI_DEPRECATED( - "PerpetualCommand violates the assumption that execute() doesn't get " - "called after isFinished() returns true -- an assumption that should be " - "valid." - "This was unsafe/undefined behavior from the start, and RepeatCommand " - "provides an easy way to achieve similar end results with slightly " - "different (and safe) semantics.") - explicit PerpetualCommand(std::shared_ptr command); - WPI_IGNORE_DEPRECATED - - /** - * Creates a new PerpetualCommand. Will run another command in perpetuity, - * ignoring that command's end conditions, unless this command itself is - * interrupted. - * - * @param command the command to run perpetually - */ - template >>> - WPI_DEPRECATED( - "PerpetualCommand violates the assumption that execute() doesn't get " - "called after isFinished() returns true -- an assumption that should be " - "valid." - "This was unsafe/undefined behavior from the start, and RepeatCommand " - "provides an easy way to achieve similar end results with slightly " - "different (and safe) semantics.") - explicit PerpetualCommand(T&& command) - : PerpetualCommand(std::make_shared>( - std::forward(command))) {} - WPI_UNIGNORE_DEPRECATED - - PerpetualCommand(PerpetualCommand&& other) = default; - - // No copy constructors for command groups - PerpetualCommand(const PerpetualCommand& other) = delete; - - // Prevent template expansion from emulating copy ctor - PerpetualCommand(PerpetualCommand&) = delete; - - void Initialize() override; - - void Execute() override; - - void End(bool interrupted) override; - - private: - std::shared_ptr m_command; -}; -} // namespace frc2 - -#ifdef _WIN32 -#pragma warning(pop) -#endif diff --git a/commands2/src/include/frc2/command/PrintCommand.h b/commands2/src/include/frc2/command/PrintCommand.h deleted file mode 100644 index 74f7f241..00000000 --- a/commands2/src/include/frc2/command/PrintCommand.h +++ /dev/null @@ -1,31 +0,0 @@ -// 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. - -#pragma once - -#include - -#include "frc2/command/CommandHelper.h" -#include "frc2/command/InstantCommand.h" - -namespace frc2 { -/** - * A command that prints a string when initialized. - */ -class PrintCommand : public InstantCommand { - public: - /** - * Creates a new a PrintCommand. - * - * @param message the message to print - */ - explicit PrintCommand(std::string_view message); - - PrintCommand(PrintCommand&& other) = default; - - PrintCommand(const PrintCommand& other) = default; - - bool RunsWhenDisabled() const override; -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/ProfiledPIDCommand.h b/commands2/src/include/frc2/command/ProfiledPIDCommand.h deleted file mode 100644 index 819622ab..00000000 --- a/commands2/src/include/frc2/command/ProfiledPIDCommand.h +++ /dev/null @@ -1,227 +0,0 @@ -// 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. - -#pragma once - -#include -#include -#include -#include - -#include -#include - -#include "frc2/command/CommandBase.h" -#include "frc2/command/CommandHelper.h" - -namespace frc2 { -/** - * A command that controls an output with a 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. - * - * @see ProfiledPIDController - */ -template -class ProfiledPIDCommand - : public CommandBase { - public: - using Distance_t = units::unit_t; - using Velocity = - units::compound_unit>; - using Velocity_t = units::unit_t; - using State = typename frc::TrapezoidProfile::State; - - /** - * Creates a new PIDCommand, which controls the given output with a - * ProfiledPIDController. - * - * @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 - */ - ProfiledPIDCommand(frc::ProfiledPIDController controller, - std::function measurementSource, - std::function goalSource, - std::function useOutput, - std::initializer_list> requirements) - : m_controller{controller}, - m_measurement{std::move(measurementSource)}, - m_goal{std::move(goalSource)}, - m_useOutput{std::move(useOutput)} { - this->AddRequirements(requirements); - } - - /** - * Creates a new PIDCommand, which controls the given output with a - * ProfiledPIDController. - * - * @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 - */ - ProfiledPIDCommand(frc::ProfiledPIDController controller, - std::function measurementSource, - std::function goalSource, - std::function useOutput, - std::span> requirements = {}) - : m_controller{controller}, - m_measurement{std::move(measurementSource)}, - m_goal{std::move(goalSource)}, - m_useOutput{std::move(useOutput)} { - this->AddRequirements(requirements); - } - - /** - * Creates a new PIDCommand, which controls the given output with a - * ProfiledPIDController. - * - * @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 - */ - ProfiledPIDCommand(frc::ProfiledPIDController controller, - std::function measurementSource, - std::function goalSource, - std::function useOutput, - std::initializer_list> requirements) - : ProfiledPIDCommand( - controller, measurementSource, - [goalSource = std::move(goalSource)]() { - return State{goalSource(), Velocity_t{0}}; - }, - useOutput, requirements) {} - - /** - * Creates a new PIDCommand, which controls the given output with a - * ProfiledPIDController. - * - * @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 - */ - ProfiledPIDCommand(frc::ProfiledPIDController controller, - std::function measurementSource, - std::function goalSource, - std::function useOutput, - std::span> requirements = {}) - : ProfiledPIDCommand( - controller, measurementSource, - [goalSource = std::move(goalSource)]() { - return State{goalSource(), Velocity_t{0}}; - }, - useOutput, requirements) {} - - /** - * Creates a new PIDCommand, which controls the given output with a - * ProfiledPIDController with a constant goal. - * - * @param controller the controller that controls the output. - * @param measurementSource the measurement of the process variable - * @param goal the controller's goal - * @param useOutput the controller's output - * @param requirements the subsystems required by this command - */ - ProfiledPIDCommand(frc::ProfiledPIDController controller, - std::function measurementSource, State goal, - std::function useOutput, - std::initializer_list> requirements) - : ProfiledPIDCommand( - controller, measurementSource, [goal] { return goal; }, useOutput, - requirements) {} - - /** - * Creates a new PIDCommand, which controls the given output with a - * ProfiledPIDController with a constant goal. - * - * @param controller the controller that controls the output. - * @param measurementSource the measurement of the process variable - * @param goal the controller's goal - * @param useOutput the controller's output - * @param requirements the subsystems required by this command - */ - ProfiledPIDCommand(frc::ProfiledPIDController controller, - std::function measurementSource, State goal, - std::function useOutput, - std::span> requirements = {}) - : ProfiledPIDCommand( - controller, measurementSource, [goal] { return goal; }, useOutput, - requirements) {} - - /** - * Creates a new PIDCommand, which controls the given output with a - * ProfiledPIDController with a constant goal. - * - * @param controller the controller that controls the output. - * @param measurementSource the measurement of the process variable - * @param goal the controller's goal - * @param useOutput the controller's output - * @param requirements the subsystems required by this command - */ - ProfiledPIDCommand(frc::ProfiledPIDController controller, - std::function measurementSource, - Distance_t goal, - std::function useOutput, - std::initializer_list> requirements) - : ProfiledPIDCommand( - controller, measurementSource, [goal] { return goal; }, useOutput, - requirements) {} - - /** - * Creates a new PIDCommand, which controls the given output with a - * ProfiledPIDController with a constant goal. - * - * @param controller the controller that controls the output. - * @param measurementSource the measurement of the process variable - * @param goal the controller's goal - * @param useOutput the controller's output - * @param requirements the subsystems required by this command - */ - ProfiledPIDCommand(frc::ProfiledPIDController controller, - std::function measurementSource, - Distance_t goal, - std::function useOutput, - std::span> requirements = {}) - : ProfiledPIDCommand( - controller, measurementSource, [goal] { return goal; }, useOutput, - requirements) {} - - ProfiledPIDCommand(ProfiledPIDCommand&& other) = default; - - ProfiledPIDCommand(const ProfiledPIDCommand& other) = default; - - void Initialize() override { m_controller.Reset(m_measurement()); } - - void Execute() override { - m_useOutput(m_controller.Calculate(m_measurement(), m_goal()), - m_controller.GetSetpoint()); - } - - void End(bool interrupted) override { - m_useOutput(0, State{Distance_t(0), Velocity_t(0)}); - } - - /** - * Returns the ProfiledPIDController used by the command. - * - * @return The ProfiledPIDController - */ - frc::ProfiledPIDController& GetController() { return m_controller; } - - protected: - frc::ProfiledPIDController m_controller; - std::function m_measurement; - std::function m_goal; - std::function m_useOutput; -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/ProfiledPIDSubsystem.h b/commands2/src/include/frc2/command/ProfiledPIDSubsystem.h deleted file mode 100644 index dcc4069c..00000000 --- a/commands2/src/include/frc2/command/ProfiledPIDSubsystem.h +++ /dev/null @@ -1,112 +0,0 @@ -// 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. - -#pragma once - -#include -#include - -#include "frc2/command/SubsystemBase.h" - -namespace frc2 { -/** - * A subsystem that uses a ProfiledPIDController to control an output. The - * controller is run synchronously from the subsystem's periodic() method. - * - * @see ProfiledPIDController - */ -template -class ProfiledPIDSubsystem : public SubsystemBase { - public: - using Distance_t = units::unit_t; - using Velocity = - units::compound_unit>; - using Velocity_t = units::unit_t; - using State = typename frc::TrapezoidProfile::State; - - /** - * Creates a new ProfiledPIDSubsystem. - * - * @param controller the ProfiledPIDController to use - * @param initialPosition the initial goal position of the subsystem - */ - explicit ProfiledPIDSubsystem(frc::ProfiledPIDController controller, - Distance_t initialPosition = Distance_t{0}) - : m_controller{controller} { - SetGoal(initialPosition); - } - - void Periodic() override { - if (m_enabled) { - UseOutput(m_controller.Calculate(GetMeasurement()), - m_controller.GetSetpoint()); - } - } - - /** - * Sets the goal state for the subsystem. - * - * @param goal The goal state for the subsystem's motion profile. - */ - void SetGoal(State goal) { m_controller.SetGoal(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. - */ - void SetGoal(Distance_t goal) { SetGoal(State{goal, Velocity_t(0)}); } - - /** - * Enables the PID control. Resets the controller. - */ - virtual void Enable() { - m_controller.Reset(GetMeasurement()); - m_enabled = true; - } - - /** - * Disables the PID control. Sets output to zero. - */ - virtual void Disable() { - UseOutput(0, State{Distance_t(0), Velocity_t(0)}); - m_enabled = false; - } - - /** - * Returns whether the controller is enabled. - * - * @return Whether the controller is enabled. - */ - bool IsEnabled() { return m_enabled; } - - /** - * Returns the ProfiledPIDController. - * - * @return The controller. - */ - frc::ProfiledPIDController& GetController() { return m_controller; } - - protected: - frc::ProfiledPIDController m_controller; - bool m_enabled{false}; - - /** - * Returns the measurement of the process variable used by the - * ProfiledPIDController. - * - * @return the measurement of the process variable - */ - virtual Distance_t GetMeasurement() = 0; - - /** - * Uses the output from the ProfiledPIDController. - * - * @param output the output of the ProfiledPIDController - * @param setpoint the setpoint state of the ProfiledPIDController, for - * feedforward - */ - virtual void UseOutput(double output, State setpoint) = 0; -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/ProxyCommand.h b/commands2/src/include/frc2/command/ProxyCommand.h deleted file mode 100644 index c3acbb23..00000000 --- a/commands2/src/include/frc2/command/ProxyCommand.h +++ /dev/null @@ -1,72 +0,0 @@ -// 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. - -#pragma once - -#include -#include - -#include - -#include "frc2/command/CommandBase.h" -#include "frc2/command/CommandHelper.h" -#include "frc2/command/SetUtilities.h" - -namespace frc2 { -/** - * Schedules the given command when this command is initialized, and ends when - * it ends. Useful for forking off from CommandGroups. If this command is - * interrupted, it will cancel the command. - * - *

This class is provided by the NewCommands VendorDep - */ -class ProxyCommand : public CommandBase { - public: - /** - * Creates a new ProxyCommand that schedules the supplied command when - * initialized, and ends when it is no longer scheduled. Useful for lazily - * creating commands at runtime. - * - * @param supplier the command supplier - */ - explicit ProxyCommand(std::function()> supplier); - - /** - * 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 - */ - explicit ProxyCommand(std::shared_ptr command); - - /** - * Creates a new ProxyCommand that schedules the given command when - * initialized, and ends when it is no longer scheduled. - * - *

Note that this constructor passes ownership of the given command to the - * returned ProxyCommand. - * - * @param command the command to schedule - */ - /* - explicit ProxyCommand(std::unique_ptr command); - */ - - ProxyCommand(ProxyCommand&& other) = default; - - void Initialize() override; - - void End(bool interrupted) override; - - void Execute() override; - - bool IsFinished() override; - - void InitSendable(wpi::SendableBuilder& builder) override; - - private: - std::function()> m_supplier; - std::shared_ptr m_command = nullptr; -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/ProxyScheduleCommand.h b/commands2/src/include/frc2/command/ProxyScheduleCommand.h deleted file mode 100644 index 07bf6d48..00000000 --- a/commands2/src/include/frc2/command/ProxyScheduleCommand.h +++ /dev/null @@ -1,64 +0,0 @@ -// 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. - -#pragma once - -#include -#include - -#include -#include - -#include "frc2/command/CommandBase.h" -#include "frc2/command/CommandHelper.h" -#include "frc2/command/SetUtilities.h" - -namespace frc2 { -/** - * Schedules the given commands when this command is initialized, and ends when - * all the commands are no longer scheduled. Useful for forking off from - * CommandGroups. If this command is interrupted, it will cancel all of the - * commands. - */ -class ProxyScheduleCommand - : public CommandBase{ - public: - /** - * Creates a new ProxyScheduleCommand that schedules the given commands when - * initialized, and ends when they are all no longer scheduled. - * - * @param toSchedule the commands to schedule - * @deprecated Replace with {@link ProxyCommand}, - * composing multiple of them in a {@link ParallelRaceGroup} if needed. - */ - WPI_DEPRECATED("Replace with ProxyCommand") - explicit ProxyScheduleCommand(std::span> toSchedule); - - /** - * Creates a new ProxyScheduleCommand that schedules the given command when - * initialized, and ends when it is no longer scheduled. - * - *

Note that this constructor passes ownership of the given command to the - * returned ProxyScheduleCommand. - * - * @param toSchedule the command to schedule - */ - explicit ProxyScheduleCommand(std::shared_ptr toSchedule); - - ProxyScheduleCommand(ProxyScheduleCommand&& other) = default; - - void Initialize() override; - - void End(bool interrupted) override; - - void Execute() override; - - bool IsFinished() override; - - private: - wpi::SmallVector, 4> m_toSchedule; - // std::unique_ptr m_owning; - bool m_finished{false}; -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/RamseteCommand.h b/commands2/src/include/frc2/command/RamseteCommand.h deleted file mode 100644 index d9f12686..00000000 --- a/commands2/src/include/frc2/command/RamseteCommand.h +++ /dev/null @@ -1,195 +0,0 @@ -// 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. - -#pragma once - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "frc2/command/CommandBase.h" -#include "frc2/command/CommandHelper.h" - -namespace frc2 { -/** - * A command that uses a RAMSETE controller to follow a trajectory - * with a differential drive. - * - *

The command handles trajectory-following, PID calculations, and - * feedforwards internally. This is intended to be a more-or-less "complete - * solution" that can be used by teams without a great deal of controls - * expertise. - * - *

Advanced teams seeking more flexibility (for example, those who wish to - * use the onboard PID functionality of a "smart" motor controller) may use the - * secondary constructor that omits the PID and feedforward functionality, - * returning only the raw wheel speeds from the RAMSETE controller. - * - * @see RamseteController - * @see Trajectory - */ -class RamseteCommand : public CommandBase { - public: - /** - * Constructs a new RamseteCommand that, when executed, will follow the - * provided trajectory. PID control and feedforward are handled internally, - * and outputs are scaled -12 to 12 representing units of volts. - * - *

Note: The controller 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 controller The RAMSETE controller used to follow the - * trajectory. - * @param feedforward A component for calculating the feedforward for the - * drive. - * @param kinematics The kinematics for the robot drivetrain. - * @param wheelSpeeds A function that supplies the speeds of the left - * and right sides of the robot drive. - * @param leftController The PIDController for the left side of the robot - * drive. - * @param rightController The PIDController for the right side of the robot - * drive. - * @param output A function that consumes the computed left and right - * outputs (in volts) for the robot drive. - * @param requirements The subsystems to require. - */ - RamseteCommand(frc::Trajectory trajectory, std::function pose, - frc::RamseteController controller, - frc::SimpleMotorFeedforward feedforward, - frc::DifferentialDriveKinematics kinematics, - std::function wheelSpeeds, - frc2::PIDController leftController, - frc2::PIDController rightController, - std::function output, - std::initializer_list> requirements); - - /** - * Constructs a new RamseteCommand that, when executed, will follow the - * provided trajectory. PID control and feedforward are handled internally, - * and outputs are scaled -12 to 12 representing units of volts. - * - *

Note: The controller 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 controller The RAMSETE controller used to follow the - * trajectory. - * @param feedforward A component for calculating the feedforward for the - * drive. - * @param kinematics The kinematics for the robot drivetrain. - * @param wheelSpeeds A function that supplies the speeds of the left - * and right sides of the robot drive. - * @param leftController The PIDController for the left side of the robot - * drive. - * @param rightController The PIDController for the right side of the robot - * drive. - * @param output A function that consumes the computed left and right - * outputs (in volts) for the robot drive. - * @param requirements The subsystems to require. - */ - RamseteCommand(frc::Trajectory trajectory, std::function pose, - frc::RamseteController controller, - frc::SimpleMotorFeedforward feedforward, - frc::DifferentialDriveKinematics kinematics, - std::function wheelSpeeds, - frc2::PIDController leftController, - frc2::PIDController rightController, - std::function output, - std::span> requirements = {}); - - /** - * Constructs a new RamseteCommand that, when executed, will follow the - * provided trajectory. Performs no PID control and calculates no - * feedforwards; outputs are the raw wheel speeds from the RAMSETE controller, - * and will need to be converted into a usable form by the user. - * - * @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 controller The RAMSETE controller used to follow the - * trajectory. - * @param kinematics The kinematics for the robot drivetrain. - * @param output A function that consumes the computed left and right - * wheel speeds. - * @param requirements The subsystems to require. - */ - RamseteCommand(frc::Trajectory trajectory, std::function pose, - frc::RamseteController controller, - frc::DifferentialDriveKinematics kinematics, - std::function - output, - std::initializer_list> requirements); - - /** - * Constructs a new RamseteCommand that, when executed, will follow the - * provided trajectory. Performs no PID control and calculates no - * feedforwards; outputs are the raw wheel speeds from the RAMSETE controller, - * and will need to be converted into a usable form by the user. - * - * @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 controller The RAMSETE controller used to follow the - * trajectory. - * @param kinematics The kinematics for the robot drivetrain. - * @param output A function that consumes the computed left and right - * wheel speeds. - * @param requirements The subsystems to require. - */ - RamseteCommand(frc::Trajectory trajectory, std::function pose, - frc::RamseteController controller, - frc::DifferentialDriveKinematics kinematics, - std::function - output, - std::span> requirements = {}); - - void Initialize() override; - - void Execute() override; - - void End(bool interrupted) override; - - bool IsFinished() override; - - void InitSendable(wpi::SendableBuilder& builder) override; - - private: - frc::Trajectory m_trajectory; - std::function m_pose; - frc::RamseteController m_controller; - frc::SimpleMotorFeedforward m_feedforward; - frc::DifferentialDriveKinematics m_kinematics; - std::function m_speeds; - std::unique_ptr m_leftController; - std::unique_ptr m_rightController; - std::function m_outputVolts; - std::function - m_outputVel; - - frc::Timer m_timer; - units::second_t m_prevTime; - frc::DifferentialDriveWheelSpeeds m_prevSpeeds; - bool m_usePID; -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/RepeatCommand.h b/commands2/src/include/frc2/command/RepeatCommand.h deleted file mode 100644 index a28a988c..00000000 --- a/commands2/src/include/frc2/command/RepeatCommand.h +++ /dev/null @@ -1,83 +0,0 @@ -// 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. - -#pragma once - -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable : 4521) -#endif - -#include -#include - -#include "frc2/command/CommandBase.h" -#include "frc2/command/CommandHelper.h" - -namespace frc2 { -/** - * 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 are owned by the composition and cannot be added to any other - * composition or scheduled individually, and the composition requires all - * subsystems its components require. - * - *

This class is provided by the NewCommands VendorDep - */ -class RepeatCommand : public CommandBase { - public: - /** - * 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 - */ - explicit RepeatCommand(std::shared_ptr 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 - */ - template >>> - explicit RepeatCommand(T&& command) - : RepeatCommand(std::make_unique>( - std::forward(command))) {} - - RepeatCommand(RepeatCommand&& other) = default; - - // No copy constructors for command groups - RepeatCommand(const RepeatCommand& other) = delete; - - // Prevent template expansion from emulating copy ctor - RepeatCommand(RepeatCommand&) = delete; - - void Initialize() override; - - void Execute() override; - - bool IsFinished() override; - - void End(bool interrupted) override; - - bool RunsWhenDisabled() const override; - - Command::InterruptionBehavior GetInterruptionBehavior() const override; - - void InitSendable(wpi::SendableBuilder& builder) override; - - private: - std::shared_ptr m_command; - bool m_ended; -}; -} // namespace frc2 - -#ifdef _WIN32 -#pragma warning(pop) -#endif diff --git a/commands2/src/include/frc2/command/RunCommand.h b/commands2/src/include/frc2/command/RunCommand.h deleted file mode 100644 index c2186805..00000000 --- a/commands2/src/include/frc2/command/RunCommand.h +++ /dev/null @@ -1,47 +0,0 @@ -// 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. - -#pragma once - -#include -#include -#include - -#include "frc2/command/CommandHelper.h" -#include "frc2/command/FunctionalCommand.h" - -namespace frc2 { -/** - * A command that runs a Runnable continuously. Has no end condition as-is; - * either subclass it or use Command.WithTimeout() or - * Command.Until() to give it one. If you only wish - * to execute a Runnable once, use InstantCommand. - */ -class RunCommand : public FunctionalCommand { - public: - /** - * Creates a new RunCommand. The Runnable will be run continuously until the - * command ends. Does not run when disabled. - * - * @param toRun the Runnable to run - * @param requirements the subsystems to require - */ - RunCommand(std::function toRun, - std::initializer_list> requirements); - - /** - * Creates a new RunCommand. The Runnable will be run continuously until the - * command ends. Does not run when disabled. - * - * @param toRun the Runnable to run - * @param requirements the subsystems to require - */ - explicit RunCommand(std::function toRun, - std::span> requirements = {}); - - RunCommand(RunCommand&& other) = default; - - RunCommand(const RunCommand& other) = default; -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/ScheduleCommand.h b/commands2/src/include/frc2/command/ScheduleCommand.h deleted file mode 100644 index 923e198a..00000000 --- a/commands2/src/include/frc2/command/ScheduleCommand.h +++ /dev/null @@ -1,47 +0,0 @@ -// 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. - -#pragma once - -#include - -#include - -#include "frc2/command/CommandBase.h" -#include "frc2/command/CommandHelper.h" -#include "frc2/command/SetUtilities.h" - -namespace frc2 { -/** - * 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. - */ -class ScheduleCommand : public CommandBase { - public: - /** - * Creates a new ScheduleCommand that schedules the given commands when - * initialized. - * - * @param toSchedule the commands to schedule - */ - explicit ScheduleCommand(std::span> toSchedule); - - explicit ScheduleCommand(std::shared_ptr toSchedule); - - ScheduleCommand(ScheduleCommand&& other) = default; - - ScheduleCommand(const ScheduleCommand& other) = default; - - void Initialize() override; - - bool IsFinished() override; - - bool RunsWhenDisabled() const override; - - private: - wpi::SmallVector, 4> m_toSchedule; -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/SelectCommand.h b/commands2/src/include/frc2/command/SelectCommand.h deleted file mode 100644 index 0c180fbc..00000000 --- a/commands2/src/include/frc2/command/SelectCommand.h +++ /dev/null @@ -1,176 +0,0 @@ -// 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. - -#pragma once - -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable : 4521) -#endif - -#include -#include -#include -#include -#include -#include - -#include - -#include "frc2/command/CommandBase.h" -#include "frc2/command/PrintCommand.h" - -namespace frc2 { -/** - * 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 are owned by the composition and cannot be added to any other - * composition or scheduled individually, and the composition requires all - * subsystems its components require. - */ -template -class SelectCommand : public CommandBase { - public: - /** - * Creates a new SelectCommand. - * - * @param commands the map of commands to choose from - * @param selector the selector to determine which command to run - */ - template >...>>> - explicit SelectCommand(std::function selector, - std::pair... commands) - : m_selector{std::move(selector)} { - std::vector>> foo; - - ((void)foo.emplace_back(commands.first, - std::make_unique>( - std::move(commands.second))), - ...); - - for (auto&& command : foo) { - CommandScheduler::GetInstance().RequireUngrouped(command.second.get()); - } - - for (auto&& command : foo) { - this->AddRequirements(command.second->GetRequirements()); - m_runsWhenDisabled &= command.second->RunsWhenDisabled(); - if (command.second->GetInterruptionBehavior() == - Command::InterruptionBehavior::kCancelSelf) { - m_interruptBehavior = Command::InterruptionBehavior::kCancelSelf; - } - m_commands.emplace(std::move(command.first), std::move(command.second)); - } - } - - SelectCommand( - std::function selector, - std::vector>>&& commands) - : m_selector{std::move(selector)} { - for (auto&& command : commands) { - CommandScheduler::GetInstance().RequireUngrouped(command.second); - } - - for (auto&& command : commands) { - this->AddRequirements(command.second->GetRequirements()); - m_runsWhenDisabled &= command.second->RunsWhenDisabled(); - if (command.second->GetInterruptionBehavior() == - Command::InterruptionBehavior::kCancelSelf) { - m_interruptBehavior = Command::InterruptionBehavior::kCancelSelf; - } - m_commands.emplace(std::move(command.first), std::move(command.second)); - } - } - - // No copy constructors for command groups - SelectCommand(const SelectCommand& other) = delete; - - // Prevent template expansion from emulating copy ctor - SelectCommand(SelectCommand&) = delete; - - /** - * Creates a new selectcommand. - * - * @param toRun a supplier providing the command to run - * @deprecated Replace with {@link ProxyCommand}, - * composing multiple of them in a {@link ParallelRaceGroup} if needed. - */ - WPI_DEPRECATED("Replace with ProxyCommand") - explicit SelectCommand(std::function()> toRun) - : m_toRun{std::move(toRun)} {} - - SelectCommand(SelectCommand&& other) = default; - - void Initialize() override; - - void Execute() override { m_selectedCommand->Execute(); } - - void End(bool interrupted) override { - return m_selectedCommand->End(interrupted); - } - - bool IsFinished() override { return m_selectedCommand->IsFinished(); } - - bool RunsWhenDisabled() const override { return m_runsWhenDisabled; } - - Command::InterruptionBehavior GetInterruptionBehavior() const override { - return m_interruptBehavior; - } - - void InitSendable(wpi::SendableBuilder& builder) override { - CommandBase::InitSendable(builder); - - builder.AddStringProperty( - "selected", - [this] { - if (m_selectedCommand) { - return m_selectedCommand->GetName(); - } else { - return std::string{"null"}; - } - }, - nullptr); - } - - protected: - // std::shared_ptr TransferOwnership() && override { - // return std::make_unique(std::move(*this)); - // } - - private: - std::unordered_map> m_commands; - std::function m_selector; - std::function()> m_toRun; - std::shared_ptr m_selectedCommand; - bool m_runsWhenDisabled = true; - Command::InterruptionBehavior m_interruptBehavior{ - Command::InterruptionBehavior::kCancelIncoming}; -}; - -template -void SelectCommand::Initialize() { - if (m_selector) { - auto find = m_commands.find(m_selector()); - if (find == m_commands.end()) { - m_selectedCommand = std::make_shared( - "SelectCommand selector value does not correspond to any command!"); - return; - } - m_selectedCommand = find->second; - } else { - m_selectedCommand = m_toRun(); - } - m_selectedCommand->Initialize(); -} - -} // namespace frc2 - -#ifdef _WIN32 -#pragma warning(pop) -#endif diff --git a/commands2/src/include/frc2/command/SequentialCommandGroup.h b/commands2/src/include/frc2/command/SequentialCommandGroup.h deleted file mode 100644 index e5886c63..00000000 --- a/commands2/src/include/frc2/command/SequentialCommandGroup.h +++ /dev/null @@ -1,110 +0,0 @@ -// 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. - -#pragma once - -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable : 4521) -#endif - -#include -#include -#include -#include -#include -#include - -#include "frc2/command/CommandGroupBase.h" -#include "frc2/command/CommandHelper.h" - -namespace frc2 { - -const size_t invalid_index = std::numeric_limits::max(); - -/** - * A command composition that runs a list of commands in sequence. - * - *

The rules for command compositions apply: command instances that are - * passed to it are owned by the composition and cannot be added to any other - * composition or scheduled individually, and the composition requires all - * subsystems its components require. - */ -class SequentialCommandGroup - : public CommandGroupBase { - public: - /** - * 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. - */ - explicit SequentialCommandGroup( - std::vector>&& commands); - - /** - * 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. - */ - template >...>>> - explicit SequentialCommandGroup(Types&&... commands) { - AddCommands(std::forward(commands)...); - } - - SequentialCommandGroup(SequentialCommandGroup&& other) = default; - - // No copy constructors for command groups - SequentialCommandGroup(const SequentialCommandGroup&) = delete; - - // Prevent template expansion from emulating copy ctor - SequentialCommandGroup(SequentialCommandGroup&) = delete; - - template >...>>> - void AddCommands(Types&&... commands) { - std::vector> foo; - ((void)foo.emplace_back(std::make_shared>( - std::forward(commands))), - ...); - AddCommands(std::move(foo)); - } - - void Initialize() final; - - void Execute() final; - - void End(bool interrupted) final; - - bool IsFinished() final; - - bool RunsWhenDisabled() const override; - - Command::InterruptionBehavior GetInterruptionBehavior() const override; - - void InitSendable(wpi::SendableBuilder& builder) override; - - public: - void AddCommands(std::vector>&& commands) final; - - private: - - wpi::SmallVector, 4> m_commands; - size_t m_currentCommandIndex{invalid_index}; - bool m_runWhenDisabled{true}; - Command::InterruptionBehavior m_interruptBehavior{ - Command::InterruptionBehavior::kCancelIncoming}; -}; - -} // namespace frc2 - -#ifdef _WIN32 -#pragma warning(pop) -#endif diff --git a/commands2/src/include/frc2/command/SetUtilities.h b/commands2/src/include/frc2/command/SetUtilities.h deleted file mode 100644 index 60b05ee6..00000000 --- a/commands2/src/include/frc2/command/SetUtilities.h +++ /dev/null @@ -1,27 +0,0 @@ -// 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. - -#pragma once - -#include - -#include - -namespace frc2 { -template -void SetInsert(wpi::SmallVectorImpl& vector, std::span toAdd) { - for (auto addCommand : toAdd) { - bool exists = false; - for (auto existingCommand : vector) { - if (addCommand == existingCommand) { - exists = true; - break; - } - } - if (!exists) { - vector.emplace_back(addCommand); - } - } -} -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/StartEndCommand.h b/commands2/src/include/frc2/command/StartEndCommand.h deleted file mode 100644 index 86fadf52..00000000 --- a/commands2/src/include/frc2/command/StartEndCommand.h +++ /dev/null @@ -1,50 +0,0 @@ -// 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. - -#pragma once - -#include -#include -#include - -#include "frc2/command/CommandHelper.h" -#include "frc2/command/FunctionalCommand.h" - -namespace frc2 { -/** - * A command that runs a given runnable when it is initialized, and another - * runnable 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 Command.WithTimeout() or Command.Until() to give - * it one. - */ -class StartEndCommand : public FunctionalCommand { - public: - /** - * Creates a new StartEndCommand. Will run the given runnables when the - * command starts and when it ends. - * - * @param onInit the Runnable to run on command init - * @param onEnd the Runnable to run on command end - * @param requirements the subsystems required by this command - */ - StartEndCommand(std::function onInit, std::function onEnd, - std::initializer_list> requirements); - - /** - * Creates a new StartEndCommand. Will run the given runnables when the - * command starts and when it ends. - * - * @param onInit the Runnable to run on command init - * @param onEnd the Runnable to run on command end - * @param requirements the subsystems required by this command - */ - StartEndCommand(std::function onInit, std::function onEnd, - std::span> requirements = {}); - - StartEndCommand(StartEndCommand&& other) = default; - - StartEndCommand(const StartEndCommand& other) = default; -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/Subsystem.h b/commands2/src/include/frc2/command/Subsystem.h deleted file mode 100644 index 17213798..00000000 --- a/commands2/src/include/frc2/command/Subsystem.h +++ /dev/null @@ -1,146 +0,0 @@ -// 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. - -#pragma once - -#include -#include - -#include "frc2/command/CommandScheduler.h" - -namespace frc2 { -class Command; -class CommandPtr; -/** - * 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 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 - * CommandScheduler.RegisterSubsystem() method in order for the - * Periodic() method to be called. It is recommended that this method be called - * from the constructor of users' Subsystem implementations. The - * SubsystemBase class offers a simple base for user implementations - * that handles this. - * - * @see Command - * @see CommandScheduler - * @see SubsystemBase - */ -class Subsystem { - public: - virtual ~Subsystem(); - /** - * 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. - */ - virtual void Periodic(); - - /** - * 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. - */ - virtual void SimulationPeriodic(); - - /** - * 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 IsFinished() method should always return false. Will automatically - * register this subsystem with the CommandScheduler. - * - * @param defaultCommand the default command to associate with this subsystem - */ - // template - // void SetDefaultCommand(T defaultCommand) { - // CommandScheduler::GetInstance().SetDefaultCommand(this, defaultCommand); - // } - - /** - * 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 IsFinished() method should always return false. Will automatically - * register this subsystem with the CommandScheduler. - * - * @param defaultCommand the default command to associate with this subsystem - */ - void SetDefaultCommand(std::shared_ptr defaultCommand); - - /** - * Removes the default command for the subsystem. This will not cancel the - * default command if it is currently running. - */ - void RemoveDefaultCommand(); - - /** - * Gets the default command for this subsystem. Returns null if no default - * command is currently associated with the subsystem. - * - * @return the default command associated with this subsystem - */ - std::shared_ptr GetDefaultCommand(); - - /** - * Returns the command currently running on this subsystem. Returns null if - * no command is currently scheduled that requires this subsystem. - * - * @return the scheduled command currently requiring this subsystem - */ - std::shared_ptr GetCurrentCommand(); - - /** - * Registers this subsystem with the CommandScheduler, allowing its - * Periodic() method to be called when the scheduler runs. - */ - void Register(); - - /** - * Constructs a command that runs an action once and finishes. Requires this - * subsystem. - * - * @param action the action to run - */ - [[nodiscard]] std::shared_ptr RunOnce(std::function action); - - /** - * Constructs a command that runs an action every iteration until interrupted. - * Requires this subsystem. - * - * @param action the action to run - */ - [[nodiscard]] std::shared_ptr Run(std::function action); - - /** - * 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 - */ - [[nodiscard]] std::shared_ptr StartEnd(std::function start, - std::function end); - - /** - * 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 - */ - [[nodiscard]] std::shared_ptr RunEnd(std::function run, - std::function end); -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/SubsystemBase.h b/commands2/src/include/frc2/command/SubsystemBase.h deleted file mode 100644 index 748271a5..00000000 --- a/commands2/src/include/frc2/command/SubsystemBase.h +++ /dev/null @@ -1,66 +0,0 @@ -// 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. - -#pragma once - -#include -#include - -#include -#include - -#include "frc2/command/Subsystem.h" - -namespace frc2 { -/** - * A base for subsystems that handles registration in the constructor, and - * provides a more intuitive method for setting the default command. - */ -class SubsystemBase : public Subsystem, - public wpi::Sendable, - public wpi::SendableHelper { - public: - void InitSendable(wpi::SendableBuilder& builder) override; - - /** - * Gets the name of this Subsystem. - * - * @return Name - */ - std::string GetName() const; - - /** - * Sets the name of this Subsystem. - * - * @param name name - */ - void SetName(std::string_view name); - - /** - * Gets the subsystem name of this Subsystem. - * - * @return Subsystem name - */ - std::string GetSubsystem() const; - - /** - * Sets the subsystem name of this Subsystem. - * - * @param name subsystem name - */ - void SetSubsystem(std::string_view name); - - /** - * Associate a Sendable with this Subsystem. - * Also update the child's name. - * - * @param name name to give child - * @param child sendable - */ - void AddChild(std::string name, wpi::Sendable* child); - - protected: - SubsystemBase(); -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/SwerveControllerCommand.h b/commands2/src/include/frc2/command/SwerveControllerCommand.h deleted file mode 100644 index 3bf900e6..00000000 --- a/commands2/src/include/frc2/command/SwerveControllerCommand.h +++ /dev/null @@ -1,359 +0,0 @@ -// 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. - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "CommandBase.h" -#include "CommandHelper.h" - -#pragma once - -namespace frc2 { - -/** - * A command that uses two PID controllers (PIDController) and a profiled PID - * controller (ProfiledPIDController) to follow a trajectory (Trajectory) with a - * swerve drive. - * - *

The command handles trajectory-following, Velocity PID calculations, and - * feedforwards internally. This is intended to be a more-or-less "complete - * solution" that can be used by teams without a great deal of controls - * expertise. - * - *

Advanced teams seeking more flexibility (for example, those who wish to - * use the onboard PID functionality of a "smart" motor controller) may use the - * secondary constructor that omits the PID and feedforward functionality, - * returning only the raw module states from the position PID controllers. - * - *

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. - */ -template -class SwerveControllerCommand - : public CommandBase { - using voltsecondspermeter = - units::compound_unit>; - using voltsecondssquaredpermeter = - units::compound_unit, - units::inverse>; - - public: - /** - * 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, - * provided by the odometry class. - * @param kinematics The kinematics for the robot drivetrain. - * @param xController The Trajectory Tracker PID controller - * for the robot's x position. - * @param yController The Trajectory Tracker PID controller - * for the robot's y position. - * @param thetaController The Trajectory Tracker PID controller - * for angle for the robot. - * @param desiredRotation The angle that the drivetrain should be - * facing. This is sampled at each time step. - * @param output The raw output module states from the - * position controllers. - * @param requirements The subsystems to require. - */ - SwerveControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SwerveDriveKinematics kinematics, - frc2::PIDController xController, frc2::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function desiredRotation, - std::function)> - output, - std::initializer_list> requirements); - - /** - * 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. - * - *

Note 2: The final rotation of the robot will be set to the rotation of - * the final pose in the trajectory. The robot will not follow the rotations - * from the poses at each timestep. If alternate rotation behavior is desired, - * the other constructor with a supplier for rotation should be used. - * - * @param trajectory The trajectory to follow. - * @param pose A function that supplies the robot pose, - * provided by the odometry class. - * @param kinematics The kinematics for the robot drivetrain. - * @param xController The Trajectory Tracker PID controller - * for the robot's x position. - * @param yController The Trajectory Tracker PID controller - * for the robot's y position. - * @param thetaController The Trajectory Tracker PID controller - * for angle for the robot. - * @param output The raw output module states from the - * position controllers. - * @param requirements The subsystems to require. - */ - SwerveControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SwerveDriveKinematics kinematics, - frc2::PIDController xController, frc2::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function)> - output, - std::initializer_list> requirements); - - /** - * 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, - * provided by the odometry class. - * @param kinematics The kinematics for the robot drivetrain. - * @param xController The Trajectory Tracker PID controller - * for the robot's x position. - * @param yController The Trajectory Tracker PID controller - * for the robot's y position. - * @param thetaController The Trajectory Tracker PID controller - * for angle for the robot. - * @param desiredRotation The angle that the drivetrain should be - * facing. This is sampled at each time step. - * @param output The raw output module states from the - * position controllers. - * @param requirements The subsystems to require. - */ - SwerveControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SwerveDriveKinematics kinematics, - frc2::PIDController xController, frc2::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function desiredRotation, - std::function)> - output, - std::span> requirements = {}); - - /** - * 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. - * - *

Note 2: The final rotation of the robot will be set to the rotation of - * the final pose in the trajectory. The robot will not follow the rotations - * from the poses at each timestep. If alternate rotation behavior is desired, - * the other constructor with a supplier for rotation should be used. - * - * @param trajectory The trajectory to follow. - * @param pose A function that supplies the robot pose, - * provided by the odometry class. - * @param kinematics The kinematics for the robot drivetrain. - * @param xController The Trajectory Tracker PID controller - * for the robot's x position. - * @param yController The Trajectory Tracker PID controller - * for the robot's y position. - * @param thetaController The Trajectory Tracker PID controller - * for angle for the robot. - * @param output The raw output module states from the - * position controllers. - * @param requirements The subsystems to require. - */ - SwerveControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SwerveDriveKinematics kinematics, - frc2::PIDController xController, frc2::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function)> - output, - std::span> requirements = {}); - - /** - * 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, - * provided by the odometry class. - * @param kinematics The kinematics for the robot drivetrain. - * @param controller The HolonomicDriveController for the drivetrain. - * @param desiredRotation The angle that the drivetrain should be - * facing. This is sampled at each time step. - * @param output The raw output module states from the - * position controllers. - * @param requirements The subsystems to require. - */ - SwerveControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SwerveDriveKinematics kinematics, - frc::HolonomicDriveController controller, - std::function desiredRotation, - std::function)> - output, - std::initializer_list> requirements); - - /** - * 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. - * - *

Note 2: The final rotation of the robot will be set to the rotation of - * the final pose in the trajectory. The robot will not follow the rotations - * from the poses at each timestep. If alternate rotation behavior is desired, - * the other constructor with a supplier for rotation should be used. - * - * @param trajectory The trajectory to follow. - * @param pose A function that supplies the robot pose, - * provided by the odometry class. - * @param kinematics The kinematics for the robot drivetrain. - * @param controller The HolonomicDriveController for the drivetrain. - * @param output The raw output module states from the - * position controllers. - * @param requirements The subsystems to require. - */ - SwerveControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SwerveDriveKinematics kinematics, - frc::HolonomicDriveController controller, - std::function)> - output, - std::initializer_list> requirements); - - /** - * 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, - * provided by the odometry class. - * @param kinematics The kinematics for the robot drivetrain. - * @param controller The HolonomicDriveController for the robot. - * @param desiredRotation The angle that the drivetrain should be - * facing. This is sampled at each time step. - * @param output The raw output module states from the - * position controllers. - * @param requirements The subsystems to require. - */ - SwerveControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SwerveDriveKinematics kinematics, - frc::HolonomicDriveController controller, - std::function desiredRotation, - std::function)> - output, - std::span> requirements = {}); - - /** - * 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. - * - *

Note 2: The final rotation of the robot will be set to the rotation of - * the final pose in the trajectory. The robot will not follow the rotations - * from the poses at each timestep. If alternate rotation behavior is desired, - * the other constructor with a supplier for rotation should be used. - * - * @param trajectory The trajectory to follow. - * @param pose A function that supplies the robot pose, - * provided by the odometry class. - * @param kinematics The kinematics for the robot drivetrain. - * @param controller The HolonomicDriveController for the drivetrain. - * @param output The raw output module states from the - * position controllers. - * @param requirements The subsystems to require. - */ - SwerveControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SwerveDriveKinematics kinematics, - frc::HolonomicDriveController controller, - std::function)> - output, - std::span> requirements = {}); - - void Initialize() override; - - void Execute() override; - - void End(bool interrupted) override; - - bool IsFinished() override; - - private: - frc::Trajectory m_trajectory; - std::function m_pose; - frc::SwerveDriveKinematics m_kinematics; - frc::HolonomicDriveController m_controller; - std::function)> - m_outputStates; - - std::function m_desiredRotation; - - frc::Timer m_timer; - units::second_t m_prevTime; - frc::Rotation2d m_finalRotation; -}; -} // namespace frc2 - -#include "SwerveControllerCommand.inc" diff --git a/commands2/src/include/frc2/command/SwerveControllerCommand.inc b/commands2/src/include/frc2/command/SwerveControllerCommand.inc deleted file mode 100644 index 24fa3f13..00000000 --- a/commands2/src/include/frc2/command/SwerveControllerCommand.inc +++ /dev/null @@ -1,179 +0,0 @@ -// 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. - -#pragma once - -#include -#include - -#include "frc2/command/SwerveControllerCommand.h" - -namespace frc2 { - -template -SwerveControllerCommand::SwerveControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SwerveDriveKinematics kinematics, - frc2::PIDController xController, frc2::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function desiredRotation, - std::function)> output, - std::initializer_list> requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_kinematics(kinematics), - m_controller(xController, yController, thetaController), - m_desiredRotation(std::move(desiredRotation)), - m_outputStates(output) { - this->AddRequirements(requirements); -} - -template -SwerveControllerCommand::SwerveControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SwerveDriveKinematics kinematics, - frc2::PIDController xController, frc2::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function)> output, - std::initializer_list> requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_kinematics(kinematics), - m_controller(xController, yController, thetaController), - m_outputStates(output) { - this->AddRequirements(requirements); -} - -template -SwerveControllerCommand::SwerveControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SwerveDriveKinematics kinematics, - frc2::PIDController xController, frc2::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function desiredRotation, - std::function)> output, - std::span> requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_kinematics(kinematics), - m_controller(xController, yController, thetaController), - m_desiredRotation(std::move(desiredRotation)), - m_outputStates(output) { - this->AddRequirements(requirements); -} - -template -SwerveControllerCommand::SwerveControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SwerveDriveKinematics kinematics, - frc2::PIDController xController, frc2::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function)> output, - std::span> requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_kinematics(kinematics), - m_controller(xController, yController, thetaController), - m_outputStates(output) { - this->AddRequirements(requirements); -} - -template -SwerveControllerCommand::SwerveControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SwerveDriveKinematics kinematics, - frc::HolonomicDriveController controller, - std::function desiredRotation, - std::function)> output, - std::initializer_list> requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_kinematics(kinematics), - m_controller(std::move(controller)), - m_desiredRotation(std::move(desiredRotation)), - m_outputStates(output) { - this->AddRequirements(requirements); -} - -template -SwerveControllerCommand::SwerveControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SwerveDriveKinematics kinematics, - frc::HolonomicDriveController controller, - std::function)> output, - std::initializer_list> requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_kinematics(kinematics), - m_controller(std::move(controller)), - m_outputStates(output) { - this->AddRequirements(requirements); -} - -template -SwerveControllerCommand::SwerveControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SwerveDriveKinematics kinematics, - frc::HolonomicDriveController controller, - std::function desiredRotation, - std::function)> output, - std::span> requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_kinematics(kinematics), - m_controller(std::move(controller)), - m_desiredRotation(std::move(desiredRotation)), - m_outputStates(output) { - this->AddRequirements(requirements); -} - -template -SwerveControllerCommand::SwerveControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SwerveDriveKinematics kinematics, - frc::HolonomicDriveController controller, - std::function)> output, - std::span> requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_kinematics(kinematics), - m_controller(std::move(controller)), - m_outputStates(output) { - this->AddRequirements(requirements); -} - -template -void SwerveControllerCommand::Initialize() { - if (m_desiredRotation == nullptr) { - m_desiredRotation = [&] { - return m_trajectory.States().back().pose.Rotation(); - }; - } - m_timer.Restart(); -} - -template -void SwerveControllerCommand::Execute() { - auto curTime = m_timer.Get(); - auto m_desiredState = m_trajectory.Sample(curTime); - - auto targetChassisSpeeds = - m_controller.Calculate(m_pose(), m_desiredState, m_desiredRotation()); - auto targetModuleStates = - m_kinematics.ToSwerveModuleStates(targetChassisSpeeds); - - m_outputStates(targetModuleStates); -} - -template -void SwerveControllerCommand::End(bool interrupted) { - m_timer.Stop(); -} - -template -bool SwerveControllerCommand::IsFinished() { - return m_timer.HasElapsed(m_trajectory.TotalTime()); -} - -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/TrapezoidProfileCommand.h b/commands2/src/include/frc2/command/TrapezoidProfileCommand.h deleted file mode 100644 index 291dc5e9..00000000 --- a/commands2/src/include/frc2/command/TrapezoidProfileCommand.h +++ /dev/null @@ -1,81 +0,0 @@ -// 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. - -#pragma once - -#include -#include -#include - -#include -#include - -#include "frc2/command/CommandBase.h" -#include "frc2/command/CommandHelper.h" - -namespace frc2 { -/** - * A command that runs a TrapezoidProfile. Useful for smoothly controlling - * mechanism motion. - * - * @see TrapezoidProfile - */ -template -class TrapezoidProfileCommand - : public CommandBase { -public: - using Distance_t = units::unit_t; - using Velocity = - units::compound_unit>; - using Velocity_t = units::unit_t; - using State = typename frc::TrapezoidProfile::State; - - /** - * Creates a new TrapezoidProfileCommand that will execute the given - * TrapezoidalProfile. 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 requirements The list of requirements. - */ - TrapezoidProfileCommand(frc::TrapezoidProfile profile, - std::function output, - std::initializer_list> requirements) - : m_profile(profile), m_output(output) { - this->AddRequirements(requirements); - } - - /** - * Creates a new TrapezoidProfileCommand that will execute the given - * TrapezoidalProfile. 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 requirements The list of requirements. - */ - TrapezoidProfileCommand(frc::TrapezoidProfile profile, - std::function output, - std::span> requirements = {}) - : m_profile(profile), m_output(output) { - this->AddRequirements(requirements); - } - - void Initialize() override { m_timer.Restart(); } - - void Execute() override { m_output(m_profile.Calculate(m_timer.Get())); } - - void End(bool interrupted) override { m_timer.Stop(); } - - bool IsFinished() override { - return m_timer.HasElapsed(m_profile.TotalTime()); - } - - private: - frc::TrapezoidProfile m_profile; - std::function m_output; - - frc::Timer m_timer; -}; - -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/TrapezoidProfileSubsystem.h b/commands2/src/include/frc2/command/TrapezoidProfileSubsystem.h deleted file mode 100644 index 82bad16f..00000000 --- a/commands2/src/include/frc2/command/TrapezoidProfileSubsystem.h +++ /dev/null @@ -1,94 +0,0 @@ -// 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. - -#pragma once - -#include -#include - -#include "frc2/command/SubsystemBase.h" - -namespace frc2 { -/** - * 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. - */ -template -class TrapezoidProfileSubsystem : public SubsystemBase { -public: - using Distance_t = units::unit_t; - using Velocity = - units::compound_unit>; - using Velocity_t = units::unit_t; - using State = typename frc::TrapezoidProfile::State; - using Constraints = typename frc::TrapezoidProfile::Constraints; - - /** - * Creates a new TrapezoidProfileSubsystem. - * - * @param constraints The constraints (maximum velocity and acceleration) - * for the profiles. - * @param initialPosition The initial position of the controller mechanism - * when the subsystem is constructed. - * @param period The period of the main robot loop, in seconds. - */ - explicit TrapezoidProfileSubsystem(Constraints constraints, - Distance_t initialPosition = Distance_t{0}, - units::second_t period = 20_ms) - : m_constraints(constraints), - m_state{initialPosition, Velocity_t(0)}, - m_goal{initialPosition, Velocity_t{0}}, - m_period(period) {} - - void Periodic() override { - auto profile = - frc::TrapezoidProfile(m_constraints, m_goal, m_state); - m_state = profile.Calculate(m_period); - if (m_enabled) { - UseState(m_state); - } - } - - /** - * Sets the goal state for the subsystem. - * - * @param goal The goal state for the subsystem's motion profile. - */ - void SetGoal(State goal) { m_goal = 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. - */ - void SetGoal(Distance_t goal) { m_goal = State{goal, Velocity_t(0)}; } - - protected: - /** - * Users should override this to consume the current state of the motion - * profile. - * - * @param state The current state of the motion profile. - */ - virtual void UseState(State state) = 0; - - /** - * Enable the TrapezoidProfileSubsystem's output. - */ - void Enable() { m_enabled = true; } - - /** - * Disable the TrapezoidProfileSubsystem's output. - */ - void Disable() { m_enabled = false; } - - private: - Constraints m_constraints; - State m_state; - State m_goal; - units::second_t m_period; - bool m_enabled{false}; -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/WaitCommand.h b/commands2/src/include/frc2/command/WaitCommand.h deleted file mode 100644 index 0b0aed59..00000000 --- a/commands2/src/include/frc2/command/WaitCommand.h +++ /dev/null @@ -1,49 +0,0 @@ -// 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. - -#pragma once - -#include -#include - -#include "frc2/command/CommandBase.h" -#include "frc2/command/CommandHelper.h" - -namespace frc2 { -/** - * A command that does nothing but takes a specified amount of time to finish. - * Useful for CommandGroups. Can also be subclassed to make a command with an - * internal timer. - */ -class WaitCommand : public CommandBase { - public: - /** - * Creates a new WaitCommand. This command will do nothing, and end after the - * specified duration. - * - * @param duration the time to wait - */ - explicit WaitCommand(units::second_t duration); - - WaitCommand(WaitCommand&& other) = default; - - WaitCommand(const WaitCommand& other) = default; - - void Initialize() override; - - void End(bool interrupted) override; - - bool IsFinished() override; - - bool RunsWhenDisabled() const override; - - void InitSendable(wpi::SendableBuilder& builder) override; - - protected: - frc::Timer m_timer; - - private: - units::second_t m_duration; -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/WaitUntilCommand.h b/commands2/src/include/frc2/command/WaitUntilCommand.h deleted file mode 100644 index a6b974bf..00000000 --- a/commands2/src/include/frc2/command/WaitUntilCommand.h +++ /dev/null @@ -1,52 +0,0 @@ -// 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. - -#pragma once - -#include - -#include - -#include "frc2/command/CommandBase.h" -#include "frc2/command/CommandHelper.h" - -namespace frc2 { -/** - * A command that does nothing but ends after a specified match time or - * condition. Useful for CommandGroups. - */ -class WaitUntilCommand : public CommandBase { - public: - /** - * Creates a new WaitUntilCommand that ends after a given condition becomes - * true. - * - * @param condition the condition to determine when to end - */ - explicit WaitUntilCommand(std::function condition); - - /** - * 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 after which to end, in seconds - */ - explicit WaitUntilCommand(units::second_t time); - - WaitUntilCommand(WaitUntilCommand&& other) = default; - - WaitUntilCommand(const WaitUntilCommand& other) = default; - - bool IsFinished() override; - - bool RunsWhenDisabled() const override; - - private: - std::function m_condition; -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/WrapperCommand.h b/commands2/src/include/frc2/command/WrapperCommand.h deleted file mode 100644 index 8b4044ea..00000000 --- a/commands2/src/include/frc2/command/WrapperCommand.h +++ /dev/null @@ -1,77 +0,0 @@ -// 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. - -#pragma once - -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable : 4521) -#endif - -#include -#include - -#include "frc2/command/CommandBase.h" -#include "frc2/command/CommandHelper.h" - -namespace frc2 { -/** - * A class used internally to wrap commands while overriding a specific method; - * all other methods will call through to the wrapped command. - * - *

Wrapped commands may only be used through the wrapper, trying to directly - * schedule them or add them to a group will throw an exception. - */ -class WrapperCommand : public CommandBase { - public: - /** - * Wrap a command. - * - * @param command the command being wrapped. Trying to directly schedule this - * command or add it to a group will throw an exception. - */ - explicit WrapperCommand(std::unique_ptr&& command); - - /** - * Wrap a command. - * - * @param command the command being wrapped. Trying to directly schedule this - * command or add it to a group will throw an exception. - */ - template >>> - explicit WrapperCommand(T&& command) - : WrapperCommand(std::make_unique>( - std::forward(command))) {} - - WrapperCommand(WrapperCommand&& other) = default; - - // No copy constructors for command groups - WrapperCommand(const WrapperCommand& other) = delete; - - // Prevent template expansion from emulating copy ctor - WrapperCommand(WrapperCommand&) = delete; - - void Initialize() override; - - void Execute() override; - - bool IsFinished() override; - - void End(bool interrupted) override; - - bool RunsWhenDisabled() const override; - - InterruptionBehavior GetInterruptionBehavior() const override; - - wpi::SmallSet, 4> GetRequirements() const override; - - protected: - std::unique_ptr m_command; -}; -} // namespace frc2 - -#ifdef _WIN32 -#pragma warning(pop) -#endif diff --git a/commands2/src/include/frc2/command/button/Button.h b/commands2/src/include/frc2/command/button/Button.h deleted file mode 100644 index 922a0292..00000000 --- a/commands2/src/include/frc2/command/button/Button.h +++ /dev/null @@ -1,278 +0,0 @@ -// 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. - -#pragma once - -#include -#include -#include -#include - -#include - -#include "Trigger.h" -#include "frc2/command/CommandPtr.h" - -namespace frc2 { -class Command; -/** - * A class used to bind command scheduling to button presses. Can be composed - * with other buttons with the operators in Trigger. - * - * @see Trigger - */ -class Button : public Trigger { - public: - /** - * Create a new button that is pressed when the given condition is true. - * - * @param isPressed Whether the button is pressed. - * @deprecated Replace with Trigger - */ - WPI_DEPRECATED("Replace with Trigger") - explicit Button(std::function isPressed); - - /** - * Create a new button that is pressed active (default constructor) - activity - * can be further determined by subclass code. - * @deprecated Replace with Trigger - */ - WPI_DEPRECATED("Replace with Trigger") - Button() = default; - - /** - * Binds a command to start when the button is pressed. Takes a - * raw pointer, and so is non-owning; users are responsible for the lifespan - * of the command. - * - * @param command The command to bind. - * @return The trigger, for chained calls. - * @deprecated Replace with Trigger::OnTrue() - */ - WPI_DEPRECATED("Replace with Trigger#OnTrue()") - Button WhenPressed(std::shared_ptr command); - - /** - * Binds a command to start when the button is pressed. Transfers - * command ownership to the button scheduler, so the user does not have to - * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be - * *copied.* - * - * @param command The command to bind. - * @return The trigger, for chained calls. - * @deprecated Replace with Trigger::OnTrue() - */ - template >>> - WPI_DEPRECATED("Replace with Trigger#OnTrue()") - Button WhenPressed(T&& command) { - WhenActive(std::forward(command)); - return *this; - } - - /** - * Binds a runnable to execute when the button is pressed. - * - * @param toRun the runnable to execute. - * @param requirements the required subsystems. - * @deprecated Replace with Trigger::OnTrue(cmd::RunOnce()) - */ - /* - WPI_DEPRECATED("Replace with Trigger#OnTrue(cmd::RunOnce())") - Button WhenPressed(std::function toRun, - std::initializer_list> requirements); - */ - - /** - * Binds a runnable to execute when the button is pressed. - * - * @param toRun the runnable to execute. - * @param requirements the required subsystems. - * @deprecated Replace with Trigger::OnTrue(cmd::RunOnce()) - */ - WPI_DEPRECATED("Replace with Trigger#OnTrue(cmd::RunOnce())") - Button WhenPressed(std::function toRun, - std::span> requirements = {}); - - /** - * Binds a command to be started repeatedly while the button is pressed, and - * canceled when it is released. Takes a raw pointer, and so is non-owning; - * users are responsible for the lifespan of the command. - * - * @param command The command to bind. - * @return The button, for chained calls. - * @deprecated Replace with Trigger::WhileTrue(command.Repeatedly()) - */ - WPI_DEPRECATED("Replace with Trigger#WhileTrue(command.Repeatedly())") - Button WhileHeld(std::shared_ptr command); - - /** - * Binds a command to be started repeatedly while the button is pressed, and - * canceled when it is released. Transfers command ownership to the button - * scheduler, so the user does not have to worry about lifespan - rvalue refs - * will be *moved*, lvalue refs will be *copied.* - * - * @param command The command to bind. - * @return The button, for chained calls. - * @deprecated Replace with Trigger::WhileTrue(command.Repeatedly()) - */ - template >>> - WPI_DEPRECATED("Replace with Trigger#WhileTrue(command.Repeatedly())") - Button WhileHeld(T&& command) { - WhileActiveContinous(std::forward(command)); - return *this; - } - - /** - * Binds a runnable to execute repeatedly while the button is pressed. - * - * @param toRun the runnable to execute. - * @param requirements the required subsystems. - * @deprecated Replace with Trigger::WhileTrue(cmd::Run()) - */ - /* - WPI_DEPRECATED("Replace with Trigger#WhileTrue(cmd::Run())") - Button WhileHeld(std::function toRun, - std::initializer_list> requirements); - */ - - /** - * Binds a runnable to execute repeatedly while the button is pressed. - * - * @param toRun the runnable to execute. - * @param requirements the required subsystems. - * @deprecated Replace with Trigger::WhileTrue(cmd::Run()) - */ - WPI_DEPRECATED("Replace with Trigger#WhileTrue(cmd::Run())") - Button WhileHeld(std::function toRun, - std::span> requirements = {}); - - /** - * Binds a command to be started when the button is pressed, and canceled - * when it is released. Takes a raw pointer, and so is non-owning; users are - * responsible for the lifespan of the command. - * - * @param command The command to bind. - * @return The button, for chained calls. - * @deprecated Replace with Trigger::WhileTrue() - */ - WPI_DEPRECATED("Replace with Trigger#WhileTrue()") - Button WhenHeld(std::shared_ptr command); - - /** - * Binds a command to be started when the button is pressed, and canceled - * when it is released. Transfers command ownership to the button scheduler, - * so the user does not have to worry about lifespan - rvalue refs will be - * *moved*, lvalue refs will be *copied.* - * - * @param command The command to bind. - * @return The button, for chained calls. - * @deprecated Replace with Trigger::WhileTrue() - */ - template >>> - WPI_DEPRECATED("Replace with Trigger#WhileTrue()") - Button WhenHeld(T&& command) { - WhileActiveOnce(std::forward(command)); - return *this; - } - - /** - * Binds a command to start when the button is released. Takes a - * raw pointer, and so is non-owning; users are responsible for the lifespan - * of the command. - * - * @param command The command to bind. - * @return The button, for chained calls. - * @deprecated Replace with Trigger::OnFalse() - */ - WPI_DEPRECATED("Replace with Trigger#OnFalse()") - Button WhenReleased(std::shared_ptr command); - - /** - * Binds a command to start when the button is pressed. Transfers - * command ownership to the button scheduler, so the user does not have to - * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be - * *copied.* - * - * @param command The command to bind. - * @return The button, for chained calls. - * @deprecated Replace with Trigger::OnFalse() - */ - template >>> - WPI_DEPRECATED("Replace with Trigger#OnFalse()") - Button WhenReleased(T&& command) { - WhenInactive(std::forward(command)); - return *this; - } - - /** - * Binds a runnable to execute when the button is released. - * - * @param toRun the runnable to execute. - * @param requirements the required subsystems. - * @deprecated Replace with Trigger::OnFalse(cmd::RunOnce()) - */ - /* - WPI_DEPRECATED("Replace with Trigger#OnFalse(cmd::RunOnce())") - Button WhenReleased(std::function toRun, - std::initializer_list> requirements); - */ - - /** - * Binds a runnable to execute when the button is released. - * - * @param toRun the runnable to execute. - * @param requirements the required subsystems. - * @deprecated Replace with Trigger::OnFalse(cmd::RunOnce()) - */ - WPI_DEPRECATED("Replace with Trigger#OnFalse(cmd::RunOnce())") - Button WhenReleased(std::function toRun, - std::span> requirements = {}); - - /** - * Binds a command to start when the button is pressed, and be canceled when - * it is pressed again. Takes a raw pointer, and so is non-owning; users are - * responsible for the lifespan of the command. - * - * @param command The command to bind. - * @return The button, for chained calls. - * @deprecated Replace with Trigger::ToggleOnTrue() - */ - WPI_DEPRECATED("Replace with Trigger#ToggleOnTrue()") - Button ToggleWhenPressed(std::shared_ptr command); - - /** - * Binds a command to start when the button is pressed, and be canceled when - * it is pressed again. Transfers command ownership to the button scheduler, - * so the user does not have to worry about lifespan - rvalue refs will be - * *moved*, lvalue refs will be *copied.* - * - * @param command The command to bind. - * @return The button, for chained calls. - * @deprecated Replace with Trigger::ToggleOnTrue() - */ - template >>> - WPI_DEPRECATED("Replace with Trigger#ToggleOnTrue()") - Button ToggleWhenPressed(T&& command) { - ToggleWhenActive(std::forward(command)); - return *this; - } - - /** - * Binds a command to be canceled when the button is pressed. Takes a - * raw pointer, and so is non-owning; users are responsible for the lifespan - * and scheduling of the command. - * - * @param command The command to bind. - * @return The button, for chained calls. - * @deprecated Pass this as a command end condition with Until() instead. - */ - WPI_DEPRECATED("Pass this as a command end condition with Until() instead.") - Button CancelWhenPressed(std::shared_ptr command); -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/button/CommandGenericHID.h b/commands2/src/include/frc2/command/button/CommandGenericHID.h deleted file mode 100644 index 2d8e8df1..00000000 --- a/commands2/src/include/frc2/command/button/CommandGenericHID.h +++ /dev/null @@ -1,205 +0,0 @@ -// 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. - -#pragma once -#include - -#include "Trigger.h" -#include "frc2/command/CommandScheduler.h" - -namespace frc2 { -/** - * A subclass of {@link GenericHID} with {@link Trigger} factories for - * command-based. - * - * @see GenericHID - */ -class CommandGenericHID : public frc::GenericHID { - public: - using GenericHID::GenericHID; - - /** - * 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 the - * CommandScheduler's default loop. - * @return an event instance representing the button's digital signal attached - * to the given loop. - */ - Trigger Button(int button, - std::optional loop = std::nullopt) const; - - /** - * 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 - * (eg right is 90, upper-left is 315). - * - * @param loop the event loop instance to attach the event to. Defaults to - * {@link CommandScheduler::GetDefaultButtonLoop() the default command - * scheduler button loop}. - * @param angle POV angle in degrees, or -1 for the center / not pressed. - * @return a Trigger instance based around this angle of a POV on the HID. - */ - Trigger POV(int angle, - std::optional loop = std::nullopt) const; - - /** - * 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 - * (eg right is 90, upper-left is 315). - * - * @param loop the event loop instance to attach the event to. Defaults to - * {@link CommandScheduler::GetDefaultButtonLoop() the default command - * scheduler button loop}. - * @param pov index of the POV to read (starting at 0). Defaults to 0. - * @param angle POV angle in degrees, or -1 for the center / not pressed. - * @return a Trigger instance based around this angle of a POV on the HID. - */ - Trigger POV(int pov, int angle, - std::optional loop = std::nullopt) const; - - /** - * Constructs a Trigger instance based around the 0 degree angle (up) of the - * default (index 0) POV on the HID. - * - * @param loop the event loop instance to attach the event to. Defaults to - * {@link CommandScheduler::GetDefaultButtonLoop() the default command - * scheduler button loop}. - * @return a Trigger instance based around the 0 degree angle of a POV on the - * HID. - */ - Trigger POVUp(std::optional loop = std::nullopt) const; - - /** - * Constructs a Trigger instance based around the 45 degree angle (right up) - * of the default (index 0) POV on the HID. - * - * @param loop the event loop instance to attach the event to. Defaults to - * {@link CommandScheduler::GetDefaultButtonLoop() the default command - * scheduler button loop}. - * @return a Trigger instance based around the 45 degree angle of a POV on the - * HID. - */ - Trigger POVUpRight(std::optional loop = std::nullopt) const; - - /** - * Constructs a Trigger instance based around the 90 degree angle (right) of - * the default (index 0) POV on the HID. - * - * @param loop the event loop instance to attach the event to. Defaults to - * {@link CommandScheduler::GetDefaultButtonLoop() the default command - * scheduler button loop}. - * @return a Trigger instance based around the 90 degree angle of a POV on the - * HID. - */ - Trigger POVRight(std::optional loop = std::nullopt) const; - - /** - * Constructs a Trigger instance based around the 135 degree angle (right - * down) of the default (index 0) POV on the HID. - * - * @return a Trigger instance based around the 135 degree angle of a POV on - * the HID. - */ - Trigger POVDownRight( - std::optional loop = std::nullopt) const; - - /** - * Constructs a Trigger instance based around the 180 degree angle (down) of - * the default (index 0) POV on the HID. - * - * @param loop the event loop instance to attach the event to. Defaults to - * {@link CommandScheduler::GetDefaultButtonLoop() the default command - * scheduler button loop}. - * @return a Trigger instance based around the 180 degree angle of a POV on - * the HID. - */ - Trigger POVDown(std::optional loop = std::nullopt) const; - - /** - * Constructs a Trigger instance based around the 225 degree angle (down left) - * of the default (index 0) POV on the HID. - * - * @param loop the event loop instance to attach the event to. Defaults to - * {@link CommandScheduler::GetDefaultButtonLoop() the default command - * scheduler button loop}. - * @return a Trigger instance based around the 225 degree angle of a POV on - * the HID. - */ - Trigger POVDownLeft(std::optional loop = std::nullopt) const; - - /** - * Constructs a Trigger instance based around the 270 degree angle (left) of - * the default (index 0) POV on the HID. - * - * @param loop the event loop instance to attach the event to. Defaults to - * {@link CommandScheduler::GetDefaultButtonLoop() the default command - * scheduler button loop}. - * @return a Trigger instance based around the 270 degree angle of a POV on - * the HID. - */ - Trigger POVLeft(std::optional loop = std::nullopt) const; - - /** - * Constructs a Trigger instance based around the 315 degree angle (left up) - * of the default (index 0) POV on the HID. - * - * @param loop the event loop instance to attach the event to. Defaults to - * {@link CommandScheduler::GetDefaultButtonLoop() the default command - * scheduler button loop}. - * @return a Trigger instance based around the 315 degree angle of a POV on - * the HID. - */ - Trigger POVUpLeft(std::optional loop = std::nullopt) const; - - /** - * Constructs a Trigger instance based around the center (not pressed) - * position of the default (index 0) POV on the HID. - * - * @param loop the event loop instance to attach the event to. Defaults to - * {@link CommandScheduler::GetDefaultButtonLoop() the default command - * scheduler button loop}. - * @return a Trigger instance based around the center position of a POV on the - * HID. - */ - Trigger POVCenter(std::optional loop = std::nullopt) const; - - /** - * Constructs a Trigger instance that is true when the axis value is less than - * {@code threshold}, attached to {@link - * CommandScheduler::GetDefaultButtonLoop() the default command scheduler - * button 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 event to. Defaults to - * {@link CommandScheduler::GetDefaultButtonLoop() the default command - * scheduler button loop}. - * @return a Trigger instance that is true when the axis value is less than - * the provided threshold. - */ - Trigger AxisLessThan( - int axis, double threshold, - std::optional loop = std::nullopt) const; - - /** - * Constructs a Trigger instance that is true when the axis value is greater - * than {@code threshold}, attached to {@link - * CommandScheduler::GetDefaultButtonLoop() the default command scheduler - * button 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 event to. Defaults to - * {@link CommandScheduler::GetDefaultButtonLoop() the default command - * scheduler button loop}. - * @return a Trigger instance that is true when the axis value is greater than - * the provided threshold. - */ - Trigger AxisGreaterThan( - int axis, double threshold, - std::optional loop = std::nullopt) const; -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/button/CommandJoystick.h b/commands2/src/include/frc2/command/button/CommandJoystick.h deleted file mode 100644 index b7f26aca..00000000 --- a/commands2/src/include/frc2/command/button/CommandJoystick.h +++ /dev/null @@ -1,55 +0,0 @@ -// 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. - -#pragma once -#include - -#include "Trigger.h" -#include "frc2/command/CommandScheduler.h" - -namespace frc2 { -/** - * A version of {@link Joystick} with {@link Trigger} factories for - * command-based. - * - * @see Joystick - */ -class CommandJoystick : public frc::Joystick { - public: - using Joystick::Joystick; - - /** - * 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 the - * CommandScheduler's default loop. - * @return an event instance representing the button's digital signal attached - * to the given loop. - */ - class Trigger Button( - int button, std::optional loop = std::nullopt) const; - - /** - * Constructs an event instance around the trigger button's digital signal. - * - * @param loop the event loop instance to attach the event to. Defaults to the - * CommandScheduler's default loop. - * @return an event instance representing the trigger button's digital signal - * attached to the given loop. - */ - class Trigger Trigger( - std::optional loop = std::nullopt) const; - - /** - * Constructs an event instance around the top button's digital signal. - * - * @param loop the event loop instance to attach the event to. Defaults to the - * CommandScheduler's default loop. - * @return an event instance representing the top button's digital signal - * attached to the given loop. - */ - class Trigger Top(std::optional loop = std::nullopt) const; -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/button/CommandPS4Controller.h b/commands2/src/include/frc2/command/button/CommandPS4Controller.h deleted file mode 100644 index c22f98df..00000000 --- a/commands2/src/include/frc2/command/button/CommandPS4Controller.h +++ /dev/null @@ -1,166 +0,0 @@ -// 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. - -#pragma once -#include - -#include "Trigger.h" -#include "frc2/command/CommandScheduler.h" - -#include - -namespace frc2 { -/** - * A version of {@link PS4Controller} with {@link Trigger} factories for - * command-based. - * - * @see PS4Controller - */ -class CommandPS4Controller : public frc::PS4Controller { - public: - using PS4Controller::PS4Controller; - - /** - * 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 the - * CommandScheduler's default loop. - * @return an event instance representing the button's digital signal attached - * to the given loop. - */ - Trigger Button(int button, - std::optional loop = std::nullopt) const; - - /** - * Constructs an event instance around the square button's digital signal. - * - * @param loop the event loop instance to attach the event to. Defaults to the - * CommandScheduler's default loop. - * @return an event instance representing the square button's digital signal - * attached to the given loop. - */ - Trigger Square(std::optional loop = std::nullopt) const; - - /** - * Constructs an event instance around the cross button's digital signal. - * - * @param loop the event loop instance to attach the event to. Defaults to the - * CommandScheduler's default loop. - * @return an event instance representing the cross button's digital signal - * attached to the given loop. - */ - Trigger Cross(std::optional loop = std::nullopt) const; - - /** - * Constructs an event instance around the circle button's digital signal. - * - * @param loop the event loop instance to attach the event to. Defaults to the - * CommandScheduler's default loop. - * @return an event instance representing the circle button's digital signal - * attached to the given loop. - */ - Trigger Circle(std::optional loop = std::nullopt) const; - - /** - * Constructs an event instance around the triangle button's digital signal. - * - * @param loop the event loop instance to attach the event to. Defaults to the - * CommandScheduler's default loop. - * @return an event instance representing the triangle button's digital signal - * attached to the given loop. - */ - Trigger Triangle(std::optional loop = std::nullopt) const; - - /** - * Constructs an event instance around the L1 button's digital signal. - * - * @param loop the event loop instance to attach the event to. Defaults to the - * CommandScheduler's default loop. - * @return an event instance representing the L1 button's digital signal - * attached to the given loop. - */ - Trigger L1(std::optional loop = std::nullopt) const; - - /** - * Constructs an event instance around the R1 button's digital signal. - * - * @param loop the event loop instance to attach the event to. Defaults to the - * CommandScheduler's default loop. - * @return an event instance representing the R1 button's digital signal - * attached to the given loop. - */ - Trigger R1(std::optional loop = std::nullopt) const; - - /** - * Constructs an event instance around the L2 button's digital signal. - * - * @param loop the event loop instance to attach the event to. Defaults to the - * CommandScheduler's default loop. - * @return an event instance representing the L2 button's digital signal - * attached to the given loop. - */ - Trigger L2(std::optional loop = std::nullopt) const; - - /** - * Constructs an event instance around the R2 button's digital signal. - * - * @param loop the event loop instance to attach the event to. Defaults to the - * CommandScheduler's default loop. - * @return an event instance representing the R2 button's digital signal - * attached to the given loop. - */ - Trigger R2(std::optional loop = std::nullopt) const; - - /** - * Constructs an event instance around the options button's digital signal. - * - * @param loop the event loop instance to attach the event to. Defaults to the - * CommandScheduler's default loop. - * @return an event instance representing the options button's digital signal - * attached to the given loop. - */ - Trigger Options(std::optional loop = std::nullopt) const; - - /** - * Constructs an event instance around the L3 button's digital signal. - * - * @param loop the event loop instance to attach the event to. Defaults to the - * CommandScheduler's default loop. - * @return an event instance representing the L3 button's digital signal - * attached to the given loop. - */ - Trigger L3(std::optional loop = std::nullopt) const; - - /** - * Constructs an event instance around the R3 button's digital signal. - * - * @param loop the event loop instance to attach the event to. Defaults to the - * CommandScheduler's default loop. - * @return an event instance representing the R3 button's digital signal - * attached to the given loop. - */ - Trigger R3(std::optional loop = std::nullopt) const; - - /** - * Constructs an event instance around the PS button's digital signal. - * - * @param loop the event loop instance to attach the event to. Defaults to the - * CommandScheduler's default loop. - * @return an event instance representing the PS button's digital signal - * attached to the given loop. - */ - Trigger PS(std::optional loop = std::nullopt) const; - - /** - * Constructs an event instance around the touchpad's digital signal. - * - * @param loop the event loop instance to attach the event to. Defaults to the - * CommandScheduler's default loop. - * @return an event instance representing the touchpad's digital signal - * attached to the given loop. - */ - Trigger Touchpad(std::optional loop = std::nullopt) const; -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/button/CommandXboxController.h b/commands2/src/include/frc2/command/button/CommandXboxController.h deleted file mode 100644 index 7ef63c33..00000000 --- a/commands2/src/include/frc2/command/button/CommandXboxController.h +++ /dev/null @@ -1,167 +0,0 @@ -// 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. - -#pragma once -#include - -#include "Trigger.h" -#include "frc2/command/CommandScheduler.h" - -namespace frc2 { -/** - * A version of {@link XboxController} with {@link Trigger} factories for - * command-based. - * - * @see XboxController - */ -class CommandXboxController : public frc::XboxController { - public: - using XboxController::XboxController; - - /** - * 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 the - * CommandScheduler's default loop. - * @return an event instance representing the button's digital signal attached - * to the given loop. - */ - Trigger Button(int button, - std::optional loop = std::nullopt) const; - - /** - * Constructs an event instance around the left bumper's digital signal. - * - * @param loop the event loop instance to attach the event to. Defaults to the - * CommandScheduler's default loop. - * @return an event instance representing the left bumper's digital signal - * attached to the given loop. - */ - Trigger LeftBumper(std::optional loop = std::nullopt) const; - - /** - * Constructs an event instance around the right bumper's digital signal. - * - * @param loop the event loop instance to attach the event to. Defaults to the - * CommandScheduler's default loop. - * @return an event instance representing the right bumper's digital signal - * attached to the given loop. - */ - Trigger RightBumper(std::optional loop = std::nullopt) const; - - /** - * Constructs an event instance around the left stick's digital signal. - * - * @param loop the event loop instance to attach the event to. Defaults to the - * CommandScheduler's default loop. - * @return an event instance representing the left stick's digital signal - * attached to the given loop. - */ - Trigger LeftStick(std::optional loop = std::nullopt) const; - - /** - * Constructs an event instance around the right stick's digital signal. - * - * @param loop the event loop instance to attach the event to. Defaults to the - * CommandScheduler's default loop. - * @return an event instance representing the right stick's digital signal - * attached to the given loop. - */ - Trigger RightStick(std::optional loop = std::nullopt) const; - - /** - * Constructs an event instance around the A button's digital signal. - * - * @param loop the event loop instance to attach the event to. Defaults to the - * CommandScheduler's default loop. - * @return an event instance representing the A button's digital signal - * attached to the given loop. - */ - Trigger A(std::optional loop = std::nullopt) const; - - /** - * Constructs an event instance around the B button's digital signal. - * - * @param loop the event loop instance to attach the event to. Defaults to the - * CommandScheduler's default loop. - * @return an event instance representing the B button's digital signal - * attached to the given loop. - */ - Trigger B(std::optional loop = std::nullopt) const; - - /** - * Constructs an event instance around the X button's digital signal. - * - * @param loop the event loop instance to attach the event to. Defaults to the - * CommandScheduler's default loop. - * @return an event instance representing the X button's digital signal - * attached to the given loop. - */ - Trigger X(std::optional loop = std::nullopt) const; - - /** - * Constructs an event instance around the Y button's digital signal. - * - * @param loop the event loop instance to attach the event to. Defaults to the - * CommandScheduler's default loop. - * @return an event instance representing the Y button's digital signal - * attached to the given loop. - */ - Trigger Y(std::optional loop = std::nullopt) const; - - /** - * Constructs an event instance around the back button's digital signal. - * - * @param loop the event loop instance to attach the event to. Defaults to the - * CommandScheduler's default loop. - * @return an event instance representing the back button's digital signal - * attached to the given loop. - */ - Trigger Back(std::optional loop = std::nullopt) const; - - /** - * Constructs an event instance around the start button's digital signal. - * - * @param loop the event loop instance to attach the event to. Defaults to the - * CommandScheduler's default loop. - * @return an event instance representing the start button's digital signal - * attached to the given loop. - */ - Trigger Start(std::optional loop = std::nullopt) const; - - /** - * 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. Defaults to 0.5. - * @param loop the event loop instance to attach the Trigger to. Defaults to - * the CommandScheduler's default loop. - * @return a Trigger instance that is true when the left trigger's axis - * exceeds the provided threshold, attached to the given loop - */ - Trigger LeftTrigger(double threshold = 0.5, - std::optional loop = std::nullopt) const; - - /** - * 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. Defaults to 0.5. - * @param loop the event loop instance to attach the Trigger to. Defaults to - * the CommandScheduler's default loop. - * @return a Trigger instance that is true when the right trigger's axis - * exceeds the provided threshold, attached to the given loop - */ - Trigger RightTrigger( - double threshold = 0.5, - std::optional loop = std::nullopt) const; -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/button/JoystickButton.h b/commands2/src/include/frc2/command/button/JoystickButton.h deleted file mode 100644 index e318f2d5..00000000 --- a/commands2/src/include/frc2/command/button/JoystickButton.h +++ /dev/null @@ -1,33 +0,0 @@ -// 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. - -#pragma once -#include -#include - -#include "Button.h" - -namespace frc2 { -/** - * A class used to bind command scheduling to joystick button presses. Can be - * composed with other buttons with the operators in Trigger. - * - * @see Trigger - */ -class JoystickButton : public Button { - public: - /** - * Creates a JoystickButton that commands can be bound to. - * - * @param joystick The joystick on which the button is located. - * @param buttonNumber The number of the button on the joystick. - */ - WPI_IGNORE_DEPRECATED - explicit JoystickButton(std::shared_ptr joystick, int buttonNumber) - : Button([joystick, buttonNumber] { - return joystick->GetRawButton(buttonNumber); - }) {} - WPI_UNIGNORE_DEPRECATED -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/button/NetworkButton.h b/commands2/src/include/frc2/command/button/NetworkButton.h deleted file mode 100644 index d9eb4578..00000000 --- a/commands2/src/include/frc2/command/button/NetworkButton.h +++ /dev/null @@ -1,63 +0,0 @@ -// 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. - -#pragma once - -#include -#include - -#include -#include -#include - -#include "Button.h" - -namespace frc2 { -/** - * A Button that uses a NetworkTable boolean field. - */ -class NetworkButton : public Button { - public: - /** - * Creates a NetworkButton that commands can be bound to. - * - * @param topic The boolean topic that contains the value. - */ - explicit NetworkButton(nt::BooleanTopic topic); - - /** - * Creates a NetworkButton that commands can be bound to. - * - * @param sub The boolean subscriber that provides the value. - */ - explicit NetworkButton(nt::BooleanSubscriber sub); - - /** - * 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. - */ - NetworkButton(std::shared_ptr table, - std::string_view field); - - /** - * 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. - */ - NetworkButton(std::string_view table, std::string_view field); - - /** - * 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. - */ - NetworkButton(nt::NetworkTableInstance inst, std::string_view table, - std::string_view field); -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/button/POVButton.h b/commands2/src/include/frc2/command/button/POVButton.h deleted file mode 100644 index b93ac664..00000000 --- a/commands2/src/include/frc2/command/button/POVButton.h +++ /dev/null @@ -1,34 +0,0 @@ -// 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. - -#pragma once -#include -#include - -#include "Button.h" - -namespace frc2 { -/** - * A class used to bind command scheduling to joystick POV presses. Can be - * composed with other buttons with the operators in Trigger. - * - * @see Trigger - */ -class POVButton : public Button { - public: - /** - * Creates a POVButton that commands can be bound to. - * - * @param joystick The joystick on which the button is located. - * @param angle The angle of the POV corresponding to a button press. - * @param povNumber The number of the POV on the joystick. - */ - WPI_IGNORE_DEPRECATED - POVButton(std::shared_ptr joystick, int angle, int povNumber = 0) - : Button([joystick, angle, povNumber] { - return joystick->GetPOV(povNumber) == angle; - }) {} - WPI_UNIGNORE_DEPRECATED -}; -} // namespace frc2 diff --git a/commands2/src/include/frc2/command/button/Trigger.h b/commands2/src/include/frc2/command/button/Trigger.h deleted file mode 100644 index b0bf3f09..00000000 --- a/commands2/src/include/frc2/command/button/Trigger.h +++ /dev/null @@ -1,615 +0,0 @@ -// 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. - -#pragma once - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "frc2/command/Command.h" -#include "frc2/command/CommandScheduler.h" - -namespace frc2 { -class Command; -/** - * 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. - * - *

Triggers can easily be composed for advanced functionality using the - * {@link #operator!}, {@link #operator||}, {@link #operator&&} operators. - * - *

This class is provided by the NewCommands VendorDep - */ -class Trigger { - public: - /** - * Creates a new trigger based on the given condition. - * - *

Polled by the default scheduler button loop. - * - * @param condition the condition represented by this trigger - */ - explicit Trigger(std::function condition) - : Trigger{CommandScheduler::GetInstance().GetDefaultButtonLoop(), - std::move(condition)} {} - - /** - * 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 - */ - Trigger(frc::EventLoop* loop, std::function condition) - : m_loop{loop}, m_condition{std::move(condition)} {} - - /** - * Create a new trigger that is always `false`. - */ - Trigger() : Trigger([] { return false; }) {} - - Trigger(const Trigger& other); - - /** - * Starts the given command whenever the condition changes from `false` to - * `true`. - * - *

Takes a raw pointer, and so is non-owning; users are responsible for the - * lifespan of the command. - * - * @param command the command to start - * @return this trigger, so calls can be chained - */ - Trigger OnTrue(std::shared_ptr command); - - /** - * Starts the given command whenever the condition changes from `false` to - * `true`. Moves command ownership to the button scheduler. - * - * @param command The command to bind. - * @return The trigger, for chained calls. - */ - /* - Trigger OnTrue(CommandPtr&& command); - */ - - /** - * Starts the given command whenever the condition changes from `true` to - * `false`. - * - *

Takes a raw pointer, and so is non-owning; users are responsible for the - * lifespan of the command. - * - * @param command the command to start - * @return this trigger, so calls can be chained - */ - Trigger OnFalse(std::shared_ptr command); - - /** - * Starts the given command whenever the condition changes from `true` to - * `false`. - * - * @param command The command to bind. - * @return The trigger, for chained calls. - */ - /* - Trigger OnFalse(CommandPtr&& command); - */ - - /** - * 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 RepeatCommand. - * - *

Takes a raw pointer, and so is non-owning; users are responsible for the - * lifespan of the command. - * - * @param command the command to start - * @return this trigger, so calls can be chained - */ - Trigger WhileTrue(std::shared_ptr command); - - /** - * Starts the given command when the condition changes to `true` and cancels - * it when the condition changes to `false`. Moves command ownership to the - * button scheduler. - * - *

Doesn't re-start the command if it ends while the condition is still - * `true`. If the command should restart, see RepeatCommand. - * - * @param command The command to bind. - * @return The trigger, for chained calls. - */ - /* - Trigger WhileTrue(CommandPtr&& command); - */ - - /** - * 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 - * `true`. If the command should restart, see RepeatCommand. - * - *

Takes a raw pointer, and so is non-owning; users are responsible for the - * lifespan of the command. - * - * @param command the command to start - * @return this trigger, so calls can be chained - */ - Trigger WhileFalse(std::shared_ptr command); - - /** - * Starts the given command when the condition changes to `false` and cancels - * it when the condition changes to `true`. Moves command ownership to the - * button scheduler. - * - *

Doesn't re-start the command if it ends while the condition is still - * `false`. If the command should restart, see RepeatCommand. - * - * @param command The command to bind. - * @return The trigger, for chained calls. - */ - /* - Trigger WhileFalse(CommandPtr&& command); - */ - - /** - * Toggles a command when the condition changes from `false` to `true`. - * - *

Takes a raw pointer, and so is non-owning; users are responsible for the - * lifespan of the command. - * - * @param command the command to toggle - * @return this trigger, so calls can be chained - */ - Trigger ToggleOnTrue(std::shared_ptr command); - - /** - * Toggles a command when the condition changes from `false` to `true`. - * - *

Takes a raw pointer, and so is non-owning; users are responsible for the - * lifespan of the command. - * - * @param command the command to toggle - * @return this trigger, so calls can be chained - */ - /* - Trigger ToggleOnTrue(CommandPtr&& command); - */ - - /** - * Toggles a command when the condition changes from `true` to the low - * state. - * - *

Takes a raw pointer, and so is non-owning; users are responsible for the - * lifespan of the command. - * - * @param command the command to toggle - * @return this trigger, so calls can be chained - */ - Trigger ToggleOnFalse(std::shared_ptr command); - - /** - * Toggles a command when the condition changes from `true` to `false`. - * - *

Takes a raw pointer, and so is non-owning; users are responsible for the - * lifespan of the command. - * - * @param command the command to toggle - * @return this trigger, so calls can be chained - */ - /* - Trigger ToggleOnFalse(CommandPtr&& command); - */ - - /** - * Binds a command to start when the trigger becomes active. Takes a - * raw pointer, and so is non-owning; users are responsible for the lifespan - * of the command. - * - * @param command The command to bind. - * @return The trigger, for chained calls. - * @deprecated Use OnTrue(Command) instead - */ - WPI_DEPRECATED("Use OnTrue(Command) instead") - Trigger WhenActive(std::shared_ptr command); - - /** - * Binds a command to start when the trigger becomes active. Transfers - * command ownership to the button scheduler, so the user does not have to - * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be - * *copied.* - * - * @param command The command to bind. - * @return The trigger, for chained calls. - * @deprecated Use OnTrue(Command) instead - */ - /* - template >>> - WPI_DEPRECATED("Use OnTrue(Command) instead") - Trigger WhenActive(T&& command) { - m_loop->Bind([condition = m_condition, previous = m_condition(), - command = std::make_unique>( - std::forward(command))]() mutable { - bool current = condition(); - - if (!previous && current) { - command->Schedule(); - } - - previous = current; - }); - return *this; - } - */ - - /** - * Binds a runnable to execute when the trigger becomes active. - * - * @param toRun the runnable to execute. - * @param requirements the required subsystems. - * @deprecated Use OnTrue(Command) instead and construct the InstantCommand - * manually - */ - /* - WPI_DEPRECATED( - "Use OnTrue(Command) instead and construct the InstantCommand manually") - Trigger WhenActive(std::function toRun, - std::initializer_list> requirements); - */ - - /** - * Binds a runnable to execute when the trigger becomes active. - * - * @param toRun the runnable to execute. - * @param requirements the required subsystems. - * @deprecated Use OnTrue(Command) instead and construct the InstantCommand - * manually - */ - WPI_DEPRECATED( - "Use OnTrue(Command) instead and construct the InstantCommand manually") - Trigger WhenActive(std::function toRun, - std::span> requirements = {}); - - /** - * Binds a command to be started repeatedly while the trigger is active, and - * canceled when it becomes inactive. Takes a raw pointer, and so is - * non-owning; users are responsible for the lifespan of the command. - * - * @param command The command to bind. - * @return The trigger, for chained calls. - * @deprecated Use WhileTrue(Command) with RepeatCommand, or bind - command::Schedule with IfHigh(std::function). - */ - WPI_DEPRECATED( - "Use WhileTrue(Command) with RepeatCommand, or bind command::Schedule " - "with IfHigh(std::function).") - Trigger WhileActiveContinous(std::shared_ptr command); - - /** - * Binds a command to be started repeatedly while the trigger is active, and - * canceled when it becomes inactive. Transfers command ownership to the - * button scheduler, so the user does not have to worry about lifespan - - * rvalue refs will be *moved*, lvalue refs will be *copied.* - * - * @param command The command to bind. - * @return The trigger, for chained calls. - * @deprecated Use WhileTrue(Command) with RepeatCommand, or bind - command::Schedule with IfHigh(std::function). - */ - /* - template >>> - WPI_DEPRECATED( - "Use WhileTrue(Command) with RepeatCommand, or bind command::Schedule " - "with IfHigh(std::function).") - Trigger WhileActiveContinous(T&& command) { - m_loop->Bind([condition = m_condition, previous = m_condition(), - command = std::make_unique>( - std::forward(command))]() mutable { - bool current = condition(); - - if (current) { - command->Schedule(); - } else if (previous && !current) { - command->Cancel(); - } - - previous = current; - }); - - return *this; - } - */ - - /** - * Binds a runnable to execute repeatedly while the trigger is active. - * - * @param toRun the runnable to execute. - * @param requirements the required subsystems. - * @deprecated Use WhileTrue(Command) and construct a RunCommand manually - */ - /* - WPI_DEPRECATED("Use WhileTrue(Command) and construct a RunCommand manually") - Trigger WhileActiveContinous(std::function toRun, - std::initializer_list> requirements); - */ - - /** - * Binds a runnable to execute repeatedly while the trigger is active. - * - * @param toRun the runnable to execute. - * @param requirements the required subsystems. - * @deprecated Use WhileTrue(Command) and construct a RunCommand manually - */ - WPI_DEPRECATED("Use WhileTrue(Command) and construct a RunCommand manually") - Trigger WhileActiveContinous(std::function toRun, - std::span> requirements = {}); - - /** - * Binds a command to be started when the trigger becomes active, and - * canceled when it becomes inactive. Takes a raw pointer, and so is - * non-owning; users are responsible for the lifespan of the command. - * - * @param command The command to bind. - * @return The trigger, for chained calls. - * @deprecated Use WhileTrue(Command) instead. - */ - WPI_DEPRECATED("Use WhileTrue(Command) instead.") - Trigger WhileActiveOnce(std::shared_ptr command); - - /** - * Binds a command to be started when the trigger becomes active, and - * canceled when it becomes inactive. Transfers command ownership to the - * button scheduler, so the user does not have to worry about lifespan - - * rvalue refs will be *moved*, lvalue refs will be *copied.* - * - * @param command The command to bind. - * @return The trigger, for chained calls. - * @deprecated Use WhileTrue(Command) instead. - */ - /* - template >>> - WPI_DEPRECATED("Use WhileTrue(Command) instead.") - Trigger WhileActiveOnce(T&& command) { - m_loop->Bind([condition = m_condition, previous = m_condition(), - command = std::make_unique>( - std::forward(command))]() mutable { - bool current = condition(); - - if (!previous && current) { - command->Schedule(); - } else if (previous && !current) { - command->Cancel(); - } - - previous = current; - }); - return *this; - } - */ - - /** - * Binds a command to start when the trigger becomes inactive. Takes a - * raw pointer, and so is non-owning; users are responsible for the lifespan - * of the command. - * - * @param command The command to bind. - * @return The trigger, for chained calls. - * @deprecated Use OnFalse(Command) instead. - */ - WPI_DEPRECATED("Use OnFalse(Command) instead.") - Trigger WhenInactive(std::shared_ptr command); - - /** - * Binds a command to start when the trigger becomes inactive. Transfers - * command ownership to the button scheduler, so the user does not have to - * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be - * *copied.* - * - * @param command The command to bind. - * @return The trigger, for chained calls. - * @deprecated Use OnFalse(Command) instead. - */ - /* - template >>> - WPI_DEPRECATED("Use OnFalse(Command) instead.") - Trigger WhenInactive(T&& command) { - m_loop->Bind([condition = m_condition, previous = m_condition(), - command = std::make_unique>( - std::forward(command))]() mutable { - bool current = condition(); - - if (previous && !current) { - command->Schedule(); - } - - previous = current; - }); - return *this; - } - */ - - /** - * Binds a runnable to execute when the trigger becomes inactive. - * - * @param toRun the runnable to execute. - * @param requirements the required subsystems. - * @deprecated Use OnFalse(Command) instead and construct the InstantCommand - * manually - */ - /* - WPI_DEPRECATED( - "Use OnFalse(Command) instead and construct the InstantCommand manually") - Trigger WhenInactive(std::function toRun, - std::initializer_list> requirements); - */ - - /** - * Binds a runnable to execute when the trigger becomes inactive. - * - * @param toRun the runnable to execute. - * @param requirements the required subsystems. - * @deprecated Use OnFalse(Command) instead and construct the InstantCommand - * manually - */ - WPI_DEPRECATED( - "Use OnFalse(Command) instead and construct the InstantCommand manually") - Trigger WhenInactive(std::function toRun, - std::span> requirements = {}); - - /** - * Binds a command to start when the trigger becomes active, and be canceled - * when it again becomes active. Takes a raw pointer, and so is non-owning; - * users are responsible for the lifespan of the command. - * - * @param command The command to bind. - * @return The trigger, for chained calls. - * @deprecated Use ToggleOnTrue(Command) instead. - */ - WPI_DEPRECATED("Use ToggleOnTrue(Command) instead.") - Trigger ToggleWhenActive(std::shared_ptr command); - - /** - * Binds a command to start when the trigger becomes active, and be canceled - * when it again becomes active. Transfers command ownership to the button - * scheduler, so the user does not have to worry about lifespan - rvalue refs - * will be *moved*, lvalue refs will be *copied.* - * - * @param command The command to bind. - * @return The trigger, for chained calls. - * @deprecated Use ToggleOnTrue(Command) instead. - */ - /* - template >>> - WPI_DEPRECATED("Use ToggleOnTrue(Command) instead.") - Trigger ToggleWhenActive(T&& command) { - m_loop->Bind([condition = m_condition, previous = m_condition(), - command = std::make_unique>( - std::forward(command))]() mutable { - bool current = condition(); - - if (!previous && current) { - if (command->IsScheduled()) { - command->Cancel(); - } else { - command->Schedule(); - } - } - - previous = current; - }); - - return *this; - } - */ - - /** - * Binds a command to be canceled when the trigger becomes active. Takes a - * raw pointer, and so is non-owning; users are responsible for the lifespan - * and scheduling of the command. - * - * @param command The command to bind. - * @return The trigger, for chained calls. - * @deprecated Pass this as a command end condition with Until() instead. - */ - WPI_DEPRECATED("Pass this as a command end condition with Until() instead.") - Trigger CancelWhenActive(std::shared_ptr command); - - /** - * Composes two triggers with logical AND. - * - * @return A trigger which is active when both component triggers are active. - */ - Trigger operator&&(std::function rhs) { - return Trigger(m_loop, [condition = m_condition, rhs = std::move(rhs)] { - return condition() && rhs(); - }); - } - - /** - * Composes two triggers with logical AND. - * - * @return A trigger which is active when both component triggers are active. - */ - Trigger operator&&(Trigger rhs) { - return Trigger(m_loop, [condition = m_condition, rhs] { - return condition() && rhs.m_condition(); - }); - } - - /** - * Composes two triggers with logical OR. - * - * @return A trigger which is active when either component trigger is active. - */ - Trigger operator||(std::function rhs) { - return Trigger(m_loop, [condition = m_condition, rhs = std::move(rhs)] { - return condition() || rhs(); - }); - } - - /** - * Composes two triggers with logical OR. - * - * @return A trigger which is active when either component trigger is active. - */ - Trigger operator||(Trigger rhs) { - return Trigger(m_loop, [condition = m_condition, rhs] { - return condition() || rhs.m_condition(); - }); - } - - /** - * Composes a trigger with logical NOT. - * - * @return A trigger which is active when the component trigger is inactive, - * and vice-versa. - */ - Trigger operator!() { - return Trigger(m_loop, [condition = m_condition] { return !condition(); }); - } - - /** - * Returns whether or not the trigger is currently active - */ - bool Get() { - return m_condition(); - } - - - /** - * 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 debounceTime The debounce period. - * @param type The debounce type. - * @return The debounced trigger. - */ - Trigger Debounce(units::second_t debounceTime, - frc::Debouncer::DebounceType type = - frc::Debouncer::DebounceType::kRising); - - private: - frc::EventLoop* m_loop; - std::function m_condition; -}; -} // namespace frc2 diff --git a/commands2/src/main.cpp b/commands2/src/main.cpp deleted file mode 100644 index 603dea7f..00000000 --- a/commands2/src/main.cpp +++ /dev/null @@ -1,34 +0,0 @@ -#include "rpygen_wrapper.hpp" - -#include - -#ifndef __FRC_ROBORIO__ -#include - -void commandsOnHalShutdown(void*) -{ - frc2::CommandScheduler::ResetInstance(); - - // re-register the callback so that HAL_Shutdown can be called multiple times - HAL_OnShutdown(NULL, commandsOnHalShutdown); -} - -#endif - - -RPYBUILD_PYBIND11_MODULE(m) -{ - initWrapper(m); - - // ensure that the command data is released when python shuts down - // even if HAL_Shutdown isn't called - static int unused; // the capsule needs something to reference - py::capsule cleanup(&unused, [](void *) { - frc2::CommandScheduler::ResetInstance(); - }); - m.add_object("_cleanup", cleanup); - -#ifndef __FRC_ROBORIO__ - HAL_OnShutdown(nullptr, commandsOnHalShutdown); -#endif -} \ No newline at end of file diff --git a/commands2/startendcommand.py b/commands2/startendcommand.py new file mode 100644 index 00000000..1ea896af --- /dev/null +++ b/commands2/startendcommand.py @@ -0,0 +1,33 @@ +from __future__ import annotations + +from typing import Any, Callable + +from .commandgroup import * +from .functionalcommand import FunctionalCommand +from .subsystem import Subsystem + + +class StartEndCommand(FunctionalCommand): + """ + A command that runs a given runnable when it is initialized, and another runnable 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 Command#withTimeout(double) or {@link + Command#until(java.util.function.BooleanSupplier)} to give it one. + """ + + def __init__( + self, + onInit: Callable[[], Any], + onEnd: Callable[[], Any], + *requirements: Subsystem, + ): + """ + Creates a new StartEndCommand. Will run the given runnables when the command starts and when it + ends. + + :param onInit: the Runnable to run on command init + :param onEnd: the Runnable to run on command end + :param requirements: the subsystems required by this command""" + super().__init__( + onInit, lambda: None, lambda _: onEnd(), lambda: False, *requirements + ) diff --git a/commands2/subsystem.py b/commands2/subsystem.py new file mode 100644 index 00000000..a3ba81ca --- /dev/null +++ b/commands2/subsystem.py @@ -0,0 +1,180 @@ +# 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 + +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 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 {@link + CommandScheduler#registerSubsystem(Subsystem...)} method in order for the {@link + Subsystem#periodic()} method to be called. It is recommended that this method be called from the + constructor of users' Subsystem implementations. The SubsystemBase class offers a simple + base for user implementations that handles this. + """ + + def __new__(cls, *arg, **kwargs) -> "Subsystem": + instance = super().__new__(cls) + super().__init__(instance) + SendableRegistry.addLW(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 + edu.wpi.first.wpilibj.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 Command#isFinished() method should + always return false. Will automatically register this subsystem with the {@link + 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 null 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 null 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 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 getName(self) -> str: + return SendableRegistry.getName(self) + + def setName(self, name: str) -> None: + SendableRegistry.setName(self, name) + + def getSubsystem(self) -> str: + return SendableRegistry.getSubsystem(self) + + def addChild(self, name: str, child: Sendable) -> None: + SendableRegistry.addLW(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/commands2/timedcommandrobot.py b/commands2/timedcommandrobot.py new file mode 100644 index 00000000..4a4b3bc5 --- /dev/null +++ b/commands2/timedcommandrobot.py @@ -0,0 +1,15 @@ +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/commands2/util.py b/commands2/util.py new file mode 100644 index 00000000..d60da8c2 --- /dev/null +++ b/commands2/util.py @@ -0,0 +1,24 @@ +from __future__ import annotations + +from typing import Iterable, Tuple, Union + +from .command import Command + + +def flatten_args_commands( + *commands: Union[Command, Iterable[Command]] +) -> Tuple[Command]: + flattened_commands = [] + 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/commands2/waitcommand.py b/commands2/waitcommand.py new file mode 100644 index 00000000..4f7ed04d --- /dev/null +++ b/commands2/waitcommand.py @@ -0,0 +1,33 @@ +from __future__ import annotations + +from wpilib import Timer + +from .command import Command +from .commandgroup import * + + +class WaitCommand(Command): + """ + A command that does nothing but takes a specified amount of time to finish.""" + + def __init__(self, seconds: float): + """ + 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() + + def initialize(self): + self._timer.reset() + self._timer.start() + + def end(self, interrupted: bool): + self._timer.stop() + + def isFinished(self) -> bool: + return self._timer.hasElapsed(self._duration) + + def runsWhenDisabled(self) -> bool: + return True diff --git a/commands2/waituntilcommand.py b/commands2/waituntilcommand.py new file mode 100644 index 00000000..34839c9f --- /dev/null +++ b/commands2/waituntilcommand.py @@ -0,0 +1,74 @@ +from __future__ import annotations + +from typing import Callable, overload + +from wpilib import Timer + +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: float): + """ + 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: + 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: float) + +Invoked with: {format_args_kwargs(self, *args, **kwargs)} +""" + ) + + def isFinished(self) -> bool: + return self._condition() + + def runsWhenDisabled(self) -> bool: + return True diff --git a/commands2/wrappercommand.py b/commands2/wrappercommand.py new file mode 100644 index 00000000..c61ffe60 --- /dev/null +++ b/commands2/wrappercommand.py @@ -0,0 +1,79 @@ +from __future__ import annotations + +from typing import Set + +from .command import Command, InterruptionBehavior +from .commandgroup import * +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 + + 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 {@link + #andThen(Command...)} 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/devtools/upstream.sh b/devtools/upstream.sh deleted file mode 100755 index 2d238684..00000000 --- a/devtools/upstream.sh +++ /dev/null @@ -1,44 +0,0 @@ -#!/bin/bash -e -# -# Assumes that the allwpilib repo is checked out to the correct branch -# that needs to be added to this repo -# -# This script is only intended to be used by maintainers of this -# repository, users should not need to use it. -# - -abspath() { - # $1 : relative filename - echo "$(cd "$(dirname "$1")" && pwd)/$(basename "$1")" -} - -if [ "$1" == "" ]; then - echo "Usage: $0 /path/to/allwpilib" - exit 1 -fi - -ALLWPILIB=$(abspath "$1") - -cd $(dirname "$0")/.. - -if [ ! -z "$(git status --porcelain --untracked-files=no)" ]; then - echo "Working tree is dirty, exiting" - exit 1 -fi - -ORIG_BRANCH="$(git symbolic-ref --short HEAD 2>/dev/null)" - - -pushd "$ALLWPILIB" - -# TODO: this takes awhile -git subtree split --prefix=wpilibNewCommands/src/main/native/ -b cmd-v2-upstream - -popd - - -git checkout upstream - -git subtree pull --prefix commands2/src $ALLWPILIB cmd-v2-upstream - -git checkout $ORIG_BRANCH \ No newline at end of file diff --git a/docs/conf.py b/docs/conf.py index cfea4c4f..0f2e83a8 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -5,7 +5,6 @@ # Imports # -import sys import os from os.path import abspath, dirname diff --git a/gen/Button.yml b/gen/Button.yml deleted file mode 100644 index aa61752d..00000000 --- a/gen/Button.yml +++ /dev/null @@ -1,44 +0,0 @@ ---- - -extra_includes: -- frc2/command/Command.h -- frc2/command/Subsystem.h - -classes: - Button: - subpackage: button - force_no_trampoline: true - methods: - Button: - overloads: - std::function: - "": - WhenPressed: - overloads: - std::shared_ptr: - T&&: - ignore: true - std::function, std::span>: - WhileHeld: - overloads: - std::shared_ptr: - T&&: - ignore: true - std::function, std::span>: - WhenHeld: - overloads: - std::shared_ptr: - T&&: - ignore: true - WhenReleased: - overloads: - std::shared_ptr: - T&&: - ignore: true - std::function, std::span>: - ToggleWhenPressed: - overloads: - std::shared_ptr: - T&&: - ignore: true - CancelWhenPressed: diff --git a/gen/Command.yml b/gen/Command.yml deleted file mode 100644 index 8aedd289..00000000 --- a/gen/Command.yml +++ /dev/null @@ -1,57 +0,0 @@ ---- - -extra_includes: -- frc2/command/InstantCommand.h -- frc2/command/ParallelCommandGroup.h -- frc2/command/ParallelRaceGroup.h -- frc2/command/ParallelDeadlineGroup.h -- frc2/command/SequentialCommandGroup.h -- frc2/command/PerpetualCommand.h -- frc2/command/ProxyScheduleCommand.h -- frc2/command/WaitCommand.h -- frc2/command/WaitUntilCommand.h -- src/helpers.h - -inline_code: | - #include - -functions: - GetTypeName: - ignore: true - RequirementsDisjoint: - Command_Schedule: - ignore: true -classes: - Command: - force_type_casters: - - std::span - - std::function - - units::second_t - shared_ptr: true - attributes: - m_isComposed: - enums: - InterruptionBehavior: - methods: - Command: - Initialize: - Execute: - End: - IsFinished: - GetRequirements: - Schedule: - cpp_code: | - [](std::shared_ptr self) { Command_Schedule(self); } - Cancel: - IsScheduled: - HasRequirement: - IsComposed: - SetComposed: - IsGrouped: - SetGrouped: - RunsWhenDisabled: - GetInterruptionBehavior: - GetName: - SetName: - ToPtr: - ignore: true diff --git a/gen/CommandBase.yml b/gen/CommandBase.yml deleted file mode 100644 index c0589889..00000000 --- a/gen/CommandBase.yml +++ /dev/null @@ -1,46 +0,0 @@ ---- - -extra_includes: -- src/helpers.h - -classes: - CommandBase: - shared_ptr: true - attributes: - m_requirements: - # this is supposed to be protected, but had to make it public - rename: _m_requirements - ignored_bases: - - wpi::SendableHelper - methods: - AddRequirements: - overloads: - std::shared_ptr: - std::initializer_list>: - ignore: true - std::span>: - wpi::SmallSet, 4>: - GetRequirements: - SetName: - GetName: - GetSubsystem: - SetSubsystem: - InitSendable: - virtual_xform: | - [&](py::function fn) { - auto builderHandle = py::cast(builder, py::return_value_policy::reference); - fn(builderHandle); - } - CommandBase: - cpp_code: | - []() { - return std::make_shared(); - } - - -inline_code: | - cls_CommandBase - .def("addRequirements", [](CommandBase * self, py::args requirements) { - auto reqs = pyargs2SharedSubsystemList(requirements); - self->AddRequirements(reqs); - }); \ No newline at end of file diff --git a/gen/CommandGenericHID.yml b/gen/CommandGenericHID.yml deleted file mode 100644 index f36789ab..00000000 --- a/gen/CommandGenericHID.yml +++ /dev/null @@ -1,27 +0,0 @@ ---- - -classes: - CommandGenericHID: - subpackage: button - # virtual base issue: robotpy-build/166 - force_no_trampoline: true - force_no_default_constructor: true - methods: - Button: - POV: - overloads: - int, std::optional [const]: - int, int, std::optional [const]: - POVUp: - POVUpRight: - POVRight: - POVDownRight: - POVDown: - POVDownLeft: - POVLeft: - POVUpLeft: - POVCenter: - AxisLessThan: - AxisGreaterThan: - inline_code: | - .def(py::init()) \ No newline at end of file diff --git a/gen/CommandGroupBase.yml b/gen/CommandGroupBase.yml deleted file mode 100644 index b366d9a5..00000000 --- a/gen/CommandGroupBase.yml +++ /dev/null @@ -1,19 +0,0 @@ ---- - -extra_includes: -- frc2/command/Command.h -- frc2/command/Subsystem.h - -classes: - CommandGroupBase: - shared_ptr: true - methods: - RequireUngrouped: - overloads: - Command&: - Command*: - ignore: true - std::span>: - std::initializer_list>: - ignore: true - AddCommands: diff --git a/gen/CommandHelper.yml b/gen/CommandHelper.yml deleted file mode 100644 index 08e3440a..00000000 --- a/gen/CommandHelper.yml +++ /dev/null @@ -1,8 +0,0 @@ ---- - -classes: - CommandHelper: - shared_ptr: true - methods: - CommandHelper: - TransferOwnership: diff --git a/gen/CommandJoystick.yml b/gen/CommandJoystick.yml deleted file mode 100644 index d57d34f0..00000000 --- a/gen/CommandJoystick.yml +++ /dev/null @@ -1,14 +0,0 @@ ---- - -classes: - CommandJoystick: - subpackage: button - # virtual base issue: robotpy-build/166 - force_no_trampoline: true - force_no_default_constructor: true - methods: - Button: - Trigger: - Top: - inline_code: | - .def(py::init()) \ No newline at end of file diff --git a/gen/CommandPS4Controller.yml b/gen/CommandPS4Controller.yml deleted file mode 100644 index 93817f1b..00000000 --- a/gen/CommandPS4Controller.yml +++ /dev/null @@ -1,25 +0,0 @@ ---- - -classes: - CommandPS4Controller: - subpackage: button - # virtual base issue: robotpy-build/166 - force_no_trampoline: true - force_no_default_constructor: true - methods: - Button: - Square: - Cross: - Circle: - Triangle: - L1: - R1: - L2: - R2: - Options: - L3: - R3: - PS: - Touchpad: - inline_code: | - .def(py::init()) diff --git a/gen/CommandScheduler.yml b/gen/CommandScheduler.yml deleted file mode 100644 index d64a5f09..00000000 --- a/gen/CommandScheduler.yml +++ /dev/null @@ -1,96 +0,0 @@ ---- - -extra_includes: -- frc2/command/Command.h -- frc2/command/Subsystem.h -- wpi/sendable/SendableBuilder.h -- networktables/NTSendableBuilder.h -- src/helpers.h - -classes: - CommandScheduler: - ignored_bases: - - wpi::SendableHelper - methods: - GetInstance: - return_value_policy: reference - no_release_gil: true - ResetInstance: - no_release_gil: true - SetPeriod: - GetActiveButtonLoop: - SetActiveButtonLoop: - GetDefaultButtonLoop: - ClearButtons: - Schedule: - overloads: - std::shared_ptr: - std::span>: - std::initializer_list>: - ignore: true - Run: - RegisterSubsystem: - overloads: - frc2::Subsystem*: - std::initializer_list: - ignore: true - std::span: - UnregisterSubsystem: - overloads: - frc2::Subsystem*: - std::initializer_list: - ignore: true - std::span: - RemoveDefaultCommand: - SetDefaultCommand: - template_impls: - - ["std::shared_ptr"] - GetDefaultCommand: - Cancel: - overloads: - std::shared_ptr: - frc2::Command*: - ignore: true - frc2::CommandPtr&: - ignore: true - std::span>: - std::initializer_list>: - ignore: true - CancelAll: - IsScheduled: - overloads: - std::span> [const]: - std::initializer_list> [const]: - ignore: true - std::shared_ptr [const]: - frc2::Command* [const]: - ignore: true - frc2::CommandPtr& [const]: - ignore: true - Requiring: - overloads: - std::shared_ptr [const]: - frc2::Subsystem* [const]: - ignore: true - Disable: - Enable: - OnCommandInitialize: - OnCommandExecute: - OnCommandInterrupt: - OnCommandFinish: - RequireUngrouped: - overloads: - const std::shared_ptr: - std::span>: - std::initializer_list>: - ignore: true - InitSendable: - inline_code: | - .def("registerSubsystem", [](CommandScheduler * self, py::args args){ - auto vargs = pyargs2SubsystemList(args); - self->RegisterSubsystem(vargs); - }) - .def("unregisterSubsystem", [](CommandScheduler * self, py::args args){ - auto vargs = pyargs2SubsystemList(args); - self->UnregisterSubsystem(vargs); - }) diff --git a/gen/CommandXboxController.yml b/gen/CommandXboxController.yml deleted file mode 100644 index 9a5fb861..00000000 --- a/gen/CommandXboxController.yml +++ /dev/null @@ -1,24 +0,0 @@ ---- - -classes: - CommandXboxController: - subpackage: button - # virtual base issue: robotpy-build/166 - force_no_trampoline: true - force_no_default_constructor: true - methods: - Button: - LeftBumper: - RightBumper: - LeftStick: - RightStick: - A: - B: - X: - Y: - Back: - Start: - LeftTrigger: - RightTrigger: - inline_code: | - .def(py::init()) \ No newline at end of file diff --git a/gen/Commands.yml b/gen/Commands.yml deleted file mode 100644 index e7bcc259..00000000 --- a/gen/Commands.yml +++ /dev/null @@ -1,77 +0,0 @@ ---- - -extra_includes: -- frc2/command/Command.h -- frc2/command/Subsystem.h -- src/helpers.h -- src/SelectCommandKey.h - -typealias: -- frc2::Command -- frc2::Subsystem - -functions: - None: - rename: nothing - subpackage: cmd - RunOnce: - overloads: - std::function, std::initializer_list>: - ignore: true - std::function, std::span>: - subpackage: cmd - Run: - overloads: - std::function, std::initializer_list>: - ignore: true - std::function, std::span>: - subpackage: cmd - StartEnd: - overloads: - std::function, std::function, std::initializer_list>: - ignore: true - std::function, std::function, std::span>: - subpackage: cmd - RunEnd: - overloads: - std::function, std::function, std::initializer_list>: - ignore: true - std::function, std::function, std::span>: - subpackage: cmd - Print: - subpackage: cmd - Wait: - subpackage: cmd - WaitUntil: - subpackage: cmd - Either: - subpackage: cmd - Select: - subpackage: cmd - template_impls: - - [SelectCommandKey] - MakeVector: - ignore: true - Sequence: - subpackage: cmd - RepeatingSequence: - subpackage: cmd - Parallel: - subpackage: cmd - Race: - subpackage: cmd - Deadline: - subpackage: cmd -inline_code: | - pkg_cmd.def("sequence", [](py::args cmds) { - return frc2::cmd::Sequence(pyargs2cmdList(cmds)); - }); - pkg_cmd.def("repeatingSequence", [](py::args cmds) { - return frc2::cmd::RepeatingSequence(pyargs2cmdList(cmds)); - }); - pkg_cmd.def("parallel", [](py::args cmds) { - return frc2::cmd::Parallel(pyargs2cmdList(cmds)); - }); - pkg_cmd.def("race", [](py::args cmds) { - return frc2::cmd::Race(pyargs2cmdList(cmds)); - }); \ No newline at end of file diff --git a/gen/ConditionalCommand.yml b/gen/ConditionalCommand.yml deleted file mode 100644 index 74c7ba43..00000000 --- a/gen/ConditionalCommand.yml +++ /dev/null @@ -1,26 +0,0 @@ ---- - -extra_includes: -- frc2/command/Command.h -- frc2/command/Subsystem.h - -classes: - ConditionalCommand: - shared_ptr: true - methods: - ConditionalCommand: - overloads: - T1&&, T2&&, std::function: - ignore: true - std::shared_ptr, std::shared_ptr, std::function: - Initialize: - Execute: - End: - IsFinished: - RunsWhenDisabled: - InitSendable: - virtual_xform: | - [&](py::function fn) { - auto builderHandle = py::cast(builder, py::return_value_policy::reference); - fn(builderHandle); - } diff --git a/gen/FunctionalCommand.yml b/gen/FunctionalCommand.yml deleted file mode 100644 index 259ccafc..00000000 --- a/gen/FunctionalCommand.yml +++ /dev/null @@ -1,36 +0,0 @@ ---- - -extra_includes: -- frc2/command/Command.h -- frc2/command/Subsystem.h -- src/helpers.h - -classes: - FunctionalCommand: - shared_ptr: true - methods: - FunctionalCommand: - overloads: - ? std::function, std::function, std::function, std::function, std::initializer_list> - : ignore: true - ? std::function, std::function, std::function, std::function, std::span> - : - Initialize: - Execute: - End: - IsFinished: - -inline_code: | - cls_FunctionalCommand - .def(py::init([]( - std::function onInit, - std::function onExecute, - std::function onEnd, - std::function isFinished, - py::args requirements) { - auto reqs = pyargs2SharedSubsystemList(requirements); - return std::make_shared(onInit, onExecute, onEnd, isFinished, reqs); - })) - ; \ No newline at end of file diff --git a/gen/InstantCommand.yml b/gen/InstantCommand.yml deleted file mode 100644 index 8e40fd21..00000000 --- a/gen/InstantCommand.yml +++ /dev/null @@ -1,28 +0,0 @@ ---- - -extra_includes: -- frc2/command/Command.h -- frc2/command/Subsystem.h -- src/helpers.h - -classes: - InstantCommand: - shared_ptr: true - methods: - InstantCommand: - overloads: - std::function, std::initializer_list>: - ignore: true - std::function, std::span>: - "": - Initialize: - IsFinished: - -inline_code: | - cls_InstantCommand - .def(py::init([](std::function toRun, py::args requirements) { - auto reqs = pyargs2SharedSubsystemList(requirements); - return std::make_shared(toRun, reqs); - }) - ) - ; \ No newline at end of file diff --git a/gen/JoystickButton.yml b/gen/JoystickButton.yml deleted file mode 100644 index 5744687c..00000000 --- a/gen/JoystickButton.yml +++ /dev/null @@ -1,9 +0,0 @@ ---- - -classes: - JoystickButton: - shared_ptr: true - force_no_trampoline: true - subpackage: button - methods: - JoystickButton: diff --git a/gen/MecanumControllerCommand.yml b/gen/MecanumControllerCommand.yml deleted file mode 100644 index f03b5744..00000000 --- a/gen/MecanumControllerCommand.yml +++ /dev/null @@ -1,72 +0,0 @@ ---- - -extra_includes: -- frc2/command/Command.h -- frc2/command/Subsystem.h - -classes: - MecanumControllerCommand: - shared_ptr: true - force_type_casters: - - units::volt_t - methods: - MecanumControllerCommand: - overloads: - ? frc::Trajectory, std::function, frc::SimpleMotorFeedforward, - frc::MecanumDriveKinematics, frc2::PIDController, frc2::PIDController, - frc::ProfiledPIDController, std::function, units::meters_per_second_t, std::function, frc2::PIDController, frc2::PIDController, frc2::PIDController, frc2::PIDController, - std::function, std::initializer_list> - : ignore: true - ? frc::Trajectory, std::function, frc::SimpleMotorFeedforward, - frc::MecanumDriveKinematics, frc2::PIDController, frc2::PIDController, - frc::ProfiledPIDController, units::meters_per_second_t, - std::function, frc2::PIDController, - frc2::PIDController, frc2::PIDController, frc2::PIDController, std::function, std::initializer_list> - : ignore: true - ? frc::Trajectory, std::function, frc::SimpleMotorFeedforward, - frc::MecanumDriveKinematics, frc2::PIDController, frc2::PIDController, - frc::ProfiledPIDController, std::function, units::meters_per_second_t, std::function, frc2::PIDController, frc2::PIDController, frc2::PIDController, frc2::PIDController, - std::function, std::span> - : - ? frc::Trajectory, std::function, frc::SimpleMotorFeedforward, - frc::MecanumDriveKinematics, frc2::PIDController, frc2::PIDController, - frc::ProfiledPIDController, units::meters_per_second_t, - std::function, frc2::PIDController, - frc2::PIDController, frc2::PIDController, frc2::PIDController, std::function, std::span> - : - ? frc::Trajectory, std::function, frc::MecanumDriveKinematics, - frc2::PIDController, frc2::PIDController, frc::ProfiledPIDController, - std::function, units::meters_per_second_t, std::function, std::initializer_list> - : ignore: true - ? frc::Trajectory, std::function, frc::MecanumDriveKinematics, - frc2::PIDController, frc2::PIDController, frc::ProfiledPIDController, - units::meters_per_second_t, std::function, std::initializer_list> - : ignore: true - ? frc::Trajectory, std::function, frc::MecanumDriveKinematics, - frc2::PIDController, frc2::PIDController, frc::ProfiledPIDController, - std::function, units::meters_per_second_t, std::function, std::span> - : - ? frc::Trajectory, std::function, frc::MecanumDriveKinematics, - frc2::PIDController, frc2::PIDController, frc::ProfiledPIDController, - units::meters_per_second_t, std::function, std::span> - : - Initialize: - Execute: - End: - IsFinished: diff --git a/gen/NetworkButton.yml b/gen/NetworkButton.yml deleted file mode 100644 index 79727830..00000000 --- a/gen/NetworkButton.yml +++ /dev/null @@ -1,16 +0,0 @@ ---- - -classes: - NetworkButton: - force_no_trampoline: true - subpackage: button - methods: - NetworkButton: - overloads: - nt::BooleanTopic: - ignore: true # TODO - nt::BooleanSubscriber: - ignore: true # TODO - std::shared_ptr, std::string_view: - std::string_view, std::string_view: - nt::NetworkTableInstance, std::string_view, std::string_view: \ No newline at end of file diff --git a/gen/NotifierCommand.yml b/gen/NotifierCommand.yml deleted file mode 100644 index d782f021..00000000 --- a/gen/NotifierCommand.yml +++ /dev/null @@ -1,17 +0,0 @@ ---- - -extra_includes: -- frc2/command/Command.h -- frc2/command/Subsystem.h - -classes: - NotifierCommand: - shared_ptr: true - methods: - NotifierCommand: - overloads: - std::function, units::second_t, std::initializer_list>: - ignore: true - std::function, units::second_t, std::span>: - Initialize: - End: diff --git a/gen/PIDCommand.yml b/gen/PIDCommand.yml deleted file mode 100644 index e4a24f12..00000000 --- a/gen/PIDCommand.yml +++ /dev/null @@ -1,48 +0,0 @@ ---- - -extra_includes: -- frc2/command/Command.h -- frc2/command/Subsystem.h - -classes: - PIDCommand: - shared_ptr: true - attributes: - m_controller: - rename: "_controller" - methods: - PIDCommand: - overloads: - ? PIDController, std::function, std::function, std::function, std::initializer_list> - : ignore: true - ? PIDController, std::function, std::function, std::function, std::span> - : - ? PIDController, std::function, double, std::function, std::initializer_list> - : ignore: true - ? PIDController, std::function, double, std::function, std::span> - : - Initialize: - Execute: - End: - GetController: - function>&&: - Types ...&&: - cpp_code: | - [](py::args cmds) { - return std::make_shared(std::move(pyargs2cmdList(cmds))); - } - param_override: - commands: - ignore: true - AddCommands: - overloads: - std::vector>&&: - Types ...&&: - ignore: true - Initialize: - Execute: - End: - IsFinished: - RunsWhenDisabled: - GetInterruptionBehavior: - -inline_code: | - cls_ParallelCommandGroup - .def("addCommands", [](ParallelCommandGroup * self, py::args cmds) { - self->AddCommands(std::move(pyargs2cmdList(cmds))); - }) - .def("alongWith", [](std::shared_ptr self, py::args cmds) -> std::shared_ptr { - self->AddCommands(std::move(pyargs2cmdList(cmds))); - return self; - }) - ; \ No newline at end of file diff --git a/gen/ParallelDeadlineGroup.yml b/gen/ParallelDeadlineGroup.yml deleted file mode 100644 index ccf12847..00000000 --- a/gen/ParallelDeadlineGroup.yml +++ /dev/null @@ -1,50 +0,0 @@ ---- - -extra_includes: -- frc2/command/Command.h -- frc2/command/Subsystem.h -- src/helpers.h - -classes: - ParallelDeadlineGroup: - shared_ptr: true - methods: - ParallelDeadlineGroup: - overloads: - std::shared_ptr, std::vector>&&: - T&&, Types ...&&: - cpp_code: | - [](std::shared_ptr cmd, py::args cmds) { - return std::make_shared(cmd, std::move(pyargs2cmdList(cmds))); - } - param_override: - commands: - ignore: true - AddCommands: - overloads: - Types ...&&: - ignore: true - std::vector>&&: - Initialize: - Execute: - End: - IsFinished: - RunsWhenDisabled: - GetInterruptionBehavior: - InitSendable: - virtual_xform: | - [&](py::function fn) { - auto builderHandle = py::cast(builder, py::return_value_policy::reference); - fn(builderHandle); - } - -inline_code: | - cls_ParallelDeadlineGroup - .def("addCommands", [](ParallelDeadlineGroup * self, py::args cmds) { - self->AddCommands(std::move(pyargs2cmdList(cmds))); - }) - .def("deadlineWith", [](std::shared_ptr self, py::args cmds) -> std::shared_ptr { - self->AddCommands(std::move(pyargs2cmdList(cmds))); - return self; - }) - ; \ No newline at end of file diff --git a/gen/ParallelRaceGroup.yml b/gen/ParallelRaceGroup.yml deleted file mode 100644 index 2ad7ec67..00000000 --- a/gen/ParallelRaceGroup.yml +++ /dev/null @@ -1,44 +0,0 @@ ---- - -extra_includes: -- frc2/command/Command.h -- frc2/command/Subsystem.h -- src/helpers.h - -classes: - ParallelRaceGroup: - shared_ptr: true - methods: - ParallelRaceGroup: - overloads: - std::vector>&&: - Types ...&&: - cpp_code: | - [](py::args cmds) { - return std::make_shared(std::move(pyargs2cmdList(cmds))); - } - param_override: - commands: - ignore: true - AddCommands: - overloads: - std::vector>&&: - Types ...&&: - ignore: true - Initialize: - Execute: - End: - IsFinished: - RunsWhenDisabled: - GetInterruptionBehavior: - -inline_code: | - cls_ParallelRaceGroup - .def("addCommands", [](ParallelRaceGroup * self, py::args cmds) { - self->AddCommands(std::move(pyargs2cmdList(cmds))); - }) - .def("raceWith", [](std::shared_ptr self, py::args cmds) -> std::shared_ptr { - self->AddCommands(std::move(pyargs2cmdList(cmds))); - return self; - }) - ; \ No newline at end of file diff --git a/gen/PerpetualCommand.yml b/gen/PerpetualCommand.yml deleted file mode 100644 index b7bc2412..00000000 --- a/gen/PerpetualCommand.yml +++ /dev/null @@ -1,23 +0,0 @@ ---- - -extra_includes: -- frc2/command/Command.h -- frc2/command/Subsystem.h - -classes: - PerpetualCommand: - shared_ptr: true - methods: - PerpetualCommand: - overloads: - std::shared_ptr: - T&&: - ignore: true - Initialize: - Execute: - End: - Perpetually: - cpp_code: | - [](std::shared_ptr self) { - return self; - } diff --git a/gen/PrintCommand.yml b/gen/PrintCommand.yml deleted file mode 100644 index e7384d94..00000000 --- a/gen/PrintCommand.yml +++ /dev/null @@ -1,12 +0,0 @@ ---- - -extra_includes: -- frc2/command/Command.h -- frc2/command/Subsystem.h - -classes: - PrintCommand: - shared_ptr: true - methods: - PrintCommand: - RunsWhenDisabled: diff --git a/gen/ProfiledPIDCommand.yml b/gen/ProfiledPIDCommand.yml deleted file mode 100644 index 0dc67ac6..00000000 --- a/gen/ProfiledPIDCommand.yml +++ /dev/null @@ -1,75 +0,0 @@ ---- - -extra_includes: -- frc2/command/Command.h -- frc2/command/Subsystem.h - -classes: - ProfiledPIDCommand: - shared_ptr: true - template_params: - - Distance - typealias: - - Distance_t = units::unit_t - - State = typename frc::TrapezoidProfile::State - attributes: - m_controller: - rename: _controller - methods: - ProfiledPIDCommand: - overloads: - ? frc::ProfiledPIDController, std::function, std::function, std::function, std::initializer_list> - : ignore: true - ? frc::ProfiledPIDController, std::function, std::function, std::function, std::span> - : - ? frc::ProfiledPIDController, std::function, std::function, std::function, std::initializer_list> - : ignore: true - ? frc::ProfiledPIDController, std::function, std::function, std::function, std::span> - : - ? frc::ProfiledPIDController, std::function, typename frc::TrapezoidProfile::State, - std::function, std::initializer_list> - : ignore: true - ? frc::ProfiledPIDController, std::function, typename frc::TrapezoidProfile::State, - std::function, std::span> - : - ? frc::ProfiledPIDController, std::function, units::unit_t, - std::function, std::initializer_list> - : ignore: true - ? frc::ProfiledPIDController, std::function, units::unit_t, - std::function, std::span> - : - Initialize: - Execute: - End: - GetController: - function::m_measurement; - using frc2::ProfiledPIDCommand::m_goal; - using frc2::ProfiledPIDCommand::m_useOutput; - - -templates: - ProfiledPIDCommand: - qualname: frc2::ProfiledPIDCommand - params: - - units::dimensionless::scalar \ No newline at end of file diff --git a/gen/ProfiledPIDSubsystem.yml b/gen/ProfiledPIDSubsystem.yml deleted file mode 100644 index 969c183f..00000000 --- a/gen/ProfiledPIDSubsystem.yml +++ /dev/null @@ -1,31 +0,0 @@ ---- - -classes: - ProfiledPIDSubsystem: - shared_ptr: true - template_params: - - Distance - typealias: - - Distance_t = units::unit_t - attributes: - m_controller: - methods: - ProfiledPIDSubsystem: - Periodic: - SetGoal: - overloads: - 'typename frc::TrapezoidProfile::State': - units::unit_t: - Enable: - Disable: - IsEnabled: - GetController: - GetMeasurement: - UseOutput: - - -templates: - ProfiledPIDSubsystem: - qualname: frc2::ProfiledPIDSubsystem - params: - - units::dimensionless::scalar diff --git a/gen/ProxyCommand.yml b/gen/ProxyCommand.yml deleted file mode 100644 index 36058cd5..00000000 --- a/gen/ProxyCommand.yml +++ /dev/null @@ -1,19 +0,0 @@ ---- - -classes: - ProxyCommand: - methods: - ProxyCommand: - overloads: - std::function ( )>: - std::shared_ptr: - Initialize: - End: - Execute: - IsFinished: - InitSendable: - virtual_xform: | - [&](py::function fn) { - auto builderHandle = py::cast(builder, py::return_value_policy::reference); - fn(builderHandle); - } diff --git a/gen/ProxyScheduleCommand.yml b/gen/ProxyScheduleCommand.yml deleted file mode 100644 index 3d9bf271..00000000 --- a/gen/ProxyScheduleCommand.yml +++ /dev/null @@ -1,26 +0,0 @@ ---- - -extra_includes: -- frc2/command/Command.h -- frc2/command/Subsystem.h -- src/helpers.h - -classes: - ProxyScheduleCommand: - methods: - ProxyScheduleCommand: - overloads: - std::span>: - std::shared_ptr: - Initialize: - End: - Execute: - IsFinished: - -inline_code: | - cls_ProxyScheduleCommand - .def(py::init([](py::args cmds) { - auto cmdList = pyargs2cmdList(cmds); - return std::make_shared(cmdList); - })) - ; \ No newline at end of file diff --git a/gen/RamseteCommand.yml b/gen/RamseteCommand.yml deleted file mode 100644 index 36c45f3a..00000000 --- a/gen/RamseteCommand.yml +++ /dev/null @@ -1,41 +0,0 @@ ---- - -classes: - RamseteCommand: - shared_ptr: true - force_type_casters: - - units::meters_per_second_t - - units::volt_t - methods: - RamseteCommand: - overloads: - ? frc::Trajectory, std::function, frc::RamseteController, - frc::SimpleMotorFeedforward, frc::DifferentialDriveKinematics, - std::function, frc2::PIDController, - frc2::PIDController, std::function, std::initializer_list> - : ignore: true - ? frc::Trajectory, std::function, frc::RamseteController, - frc::SimpleMotorFeedforward, frc::DifferentialDriveKinematics, - std::function, frc2::PIDController, - frc2::PIDController, std::function, std::span> - : - ? frc::Trajectory, std::function, frc::RamseteController, - frc::DifferentialDriveKinematics, std::function, std::initializer_list> - : ignore: true - ? frc::Trajectory, std::function, frc::RamseteController, - frc::DifferentialDriveKinematics, std::function, std::span> - : - Initialize: - Execute: - End: - IsFinished: - InitSendable: - virtual_xform: | - [&](py::function fn) { - auto builderHandle = py::cast(builder, py::return_value_policy::reference); - fn(builderHandle); - } diff --git a/gen/RepeatCommand.yml b/gen/RepeatCommand.yml deleted file mode 100644 index 8a5c479a..00000000 --- a/gen/RepeatCommand.yml +++ /dev/null @@ -1,22 +0,0 @@ ---- - -classes: - RepeatCommand: - methods: - RepeatCommand: - overloads: - std::shared_ptr: - T&&: - ignore: true - Initialize: - Execute: - IsFinished: - End: - RunsWhenDisabled: - GetInterruptionBehavior: - InitSendable: - virtual_xform: | - [&](py::function fn) { - auto builderHandle = py::cast(builder, py::return_value_policy::reference); - fn(builderHandle); - } diff --git a/gen/RunCommand.yml b/gen/RunCommand.yml deleted file mode 100644 index 70cccdcd..00000000 --- a/gen/RunCommand.yml +++ /dev/null @@ -1,25 +0,0 @@ ---- - -extra_includes: -- frc2/command/Command.h -- frc2/command/Subsystem.h -- src/helpers.h - -classes: - RunCommand: - shared_ptr: true - methods: - RunCommand: - overloads: - std::function, std::initializer_list>: - ignore: true - std::function, std::span>: - Execute: - -inline_code: | - cls_RunCommand - .def(py::init([](std::function toRun, py::args requirements) { - auto reqs = pyargs2SharedSubsystemList(requirements); - return std::make_shared(toRun, reqs); - })) - ; \ No newline at end of file diff --git a/gen/ScheduleCommand.yml b/gen/ScheduleCommand.yml deleted file mode 100644 index 89edebd7..00000000 --- a/gen/ScheduleCommand.yml +++ /dev/null @@ -1,26 +0,0 @@ ---- - -extra_includes: -- frc2/command/Command.h -- frc2/command/Subsystem.h -- src/helpers.h - -classes: - ScheduleCommand: - shared_ptr: true - methods: - ScheduleCommand: - overloads: - std::span>: - std::shared_ptr: - Initialize: - IsFinished: - RunsWhenDisabled: - -inline_code: | - cls_ScheduleCommand - .def(py::init([](py::args cmds) { - auto cmdList = pyargs2cmdList(cmds); - return std::make_shared(cmdList); - })) - ; \ No newline at end of file diff --git a/gen/SelectCommand.yml b/gen/SelectCommand.yml deleted file mode 100644 index ccbc8fa2..00000000 --- a/gen/SelectCommand.yml +++ /dev/null @@ -1,41 +0,0 @@ ---- - -extra_includes: -- frc2/command/Command.h -- frc2/command/Subsystem.h -- src/SelectCommandKey.h - -functions: - Initialize: - ignore: true -classes: - SelectCommand: - shared_ptr: true - template_params: - - Key - methods: - SelectCommand: - overloads: - std::function, std::pair...: - ignore: true - std::function, std::vector>>&&: - std::function ( )>: - Initialize: - Execute: - End: - IsFinished: - RunsWhenDisabled: - GetInterruptionBehavior: - InitSendable: - virtual_xform: | - [&](py::function fn) { - auto builderHandle = py::cast(builder, py::return_value_policy::reference); - fn(builderHandle); - } - - -templates: - SelectCommand: - qualname: frc2::SelectCommand - params: - - SelectCommandKey diff --git a/gen/SequentialCommandGroup.yml b/gen/SequentialCommandGroup.yml deleted file mode 100644 index d96bdfb8..00000000 --- a/gen/SequentialCommandGroup.yml +++ /dev/null @@ -1,58 +0,0 @@ ---- - -extra_includes: -- frc2/command/Command.h -- frc2/command/Subsystem.h -- src/helpers.h - -attributes: - invalid_index: -functions: - SequentialCommandGroup_BeforeStarting: - ignore: true - SequentialCommandGroup_AndThen: - ignore: true -classes: - SequentialCommandGroup: - shared_ptr: true - force_type_casters: - - std::span - - std::function - attributes: - m_commands: - methods: - SequentialCommandGroup: - overloads: - std::vector>&&: - Types ...&&: - cpp_code: | - [](py::args cmds) { - return std::make_shared(std::move(pyargs2cmdList(cmds))); - } - param_override: - commands: - ignore: true - AddCommands: - overloads: - std::vector>&&: - Types ...&&: - ignore: true - Initialize: - Execute: - End: - IsFinished: - RunsWhenDisabled: - GetInterruptionBehavior: - InitSendable: - virtual_xform: | - [&](py::function fn) { - auto builderHandle = py::cast(builder, py::return_value_policy::reference); - fn(builderHandle); - } - -inline_code: | - cls_SequentialCommandGroup - .def("addCommands", [](SequentialCommandGroup * self, py::args cmds) { - self->AddCommands(std::move(pyargs2cmdList(cmds))); - }) - ; \ No newline at end of file diff --git a/gen/StartEndCommand.yml b/gen/StartEndCommand.yml deleted file mode 100644 index f10558cd..00000000 --- a/gen/StartEndCommand.yml +++ /dev/null @@ -1,28 +0,0 @@ ---- - -extra_includes: -- frc2/command/Command.h -- frc2/command/Subsystem.h -- src/helpers.h - -classes: - StartEndCommand: - shared_ptr: true - methods: - StartEndCommand: - overloads: - std::function, std::function, std::initializer_list>: - ignore: true - std::function, std::function, std::span>: - Initialize: - End: - -inline_code: | - cls_StartEndCommand - .def(py::init([](std::function onInit, std::function onEnd, - py::args requirements) { - auto reqs = pyargs2SharedSubsystemList(requirements); - return std::make_shared(onInit, onEnd, reqs); - }) - ) - ; \ No newline at end of file diff --git a/gen/Subsystem.yml b/gen/Subsystem.yml deleted file mode 100644 index 1166c1b6..00000000 --- a/gen/Subsystem.yml +++ /dev/null @@ -1,36 +0,0 @@ ---- - -extra_includes: -- frc2/command/Command.h -- frc2/command/Commands.h - -classes: - Subsystem: - shared_ptr: true - methods: - Periodic: - SimulationPeriodic: - SetDefaultCommand: - GetDefaultCommand: - GetCurrentCommand: - Register: - RunOnce: - cpp_code: | - [](std::shared_ptr self, std::function action) { - return frc2::cmd::RunOnce(std::move(action), {self}); - } - Run: - cpp_code: | - [](std::shared_ptr self, std::function action) { - return frc2::cmd::Run(std::move(action), {self}); - } - StartEnd: - cpp_code: | - [](std::shared_ptr self, std::function start, std::function end) { - return frc2::cmd::StartEnd(std::move(start), std::move(end), {self}); - } - RunEnd: - cpp_code: | - [](std::shared_ptr self, std::function start, std::function end) { - return frc2::cmd::RunEnd(std::move(start), std::move(end), {self}); - } diff --git a/gen/SubsystemBase.yml b/gen/SubsystemBase.yml deleted file mode 100644 index 4b7668db..00000000 --- a/gen/SubsystemBase.yml +++ /dev/null @@ -1,20 +0,0 @@ ---- - -classes: - SubsystemBase: - shared_ptr: true - ignored_bases: - - wpi::SendableHelper - methods: - InitSendable: - virtual_xform: | - [&](py::function fn) { - auto builderHandle = py::cast(builder, py::return_value_policy::reference); - fn(builderHandle); - } - GetName: - SetName: - GetSubsystem: - SetSubsystem: - AddChild: - SubsystemBase: diff --git a/gen/SwerveControllerCommand.yml b/gen/SwerveControllerCommand.yml deleted file mode 100644 index bca2a7e0..00000000 --- a/gen/SwerveControllerCommand.yml +++ /dev/null @@ -1,72 +0,0 @@ ---- - -extra_includes: -- frc2/command/Command.h -- frc2/command/Subsystem.h - -classes: - SwerveControllerCommand: - shared_ptr: true - template_params: - - size_t NumModules - methods: - SwerveControllerCommand: - overloads: - ? frc::Trajectory, std::function, frc::SwerveDriveKinematics, - frc2::PIDController, frc2::PIDController, frc::ProfiledPIDController, - std::function, std::function )>, std::initializer_list> - : ignore: true - ? frc::Trajectory, std::function, frc::SwerveDriveKinematics, - frc2::PIDController, frc2::PIDController, frc::ProfiledPIDController, - std::function )>, - std::initializer_list> - : ignore: true - ? frc::Trajectory, std::function, frc::SwerveDriveKinematics, - frc2::PIDController, frc2::PIDController, frc::ProfiledPIDController, - std::function, std::function )>, std::span> - : - ? frc::Trajectory, std::function, frc::SwerveDriveKinematics, - frc2::PIDController, frc2::PIDController, frc::ProfiledPIDController, - std::function )>, - std::span> - : - ? frc::Trajectory, std::function, frc::SwerveDriveKinematics, - frc::HolonomicDriveController, std::function, std::function )>, std::initializer_list> - : ignore: true - ? frc::Trajectory, std::function, frc::SwerveDriveKinematics, - frc::HolonomicDriveController, std::function )>, std::initializer_list> - : ignore: true - ? frc::Trajectory, std::function, frc::SwerveDriveKinematics, - frc::HolonomicDriveController, std::function, std::function )>, std::span> - : - ? frc::Trajectory, std::function, frc::SwerveDriveKinematics, - frc::HolonomicDriveController, std::function )>, std::span> - : - Initialize: - Execute: - End: - IsFinished: - -templates: - Swerve2ControllerCommand: - qualname: frc2::SwerveControllerCommand - params: - - 2 - Swerve3ControllerCommand: - qualname: frc2::SwerveControllerCommand - params: - - 3 - Swerve4ControllerCommand: - qualname: frc2::SwerveControllerCommand - params: - - 4 - Swerve6ControllerCommand: - qualname: frc2::SwerveControllerCommand - params: - - 6 \ No newline at end of file diff --git a/gen/TimedCommandRobot.yml b/gen/TimedCommandRobot.yml deleted file mode 100644 index b56eb94f..00000000 --- a/gen/TimedCommandRobot.yml +++ /dev/null @@ -1,7 +0,0 @@ ---- - -classes: - TimedCommandRobot: - methods: - TimedCommandRobot: - RobotPeriodic: diff --git a/gen/TrapezoidProfileCommand.yml b/gen/TrapezoidProfileCommand.yml deleted file mode 100644 index b2d19b12..00000000 --- a/gen/TrapezoidProfileCommand.yml +++ /dev/null @@ -1,38 +0,0 @@ ---- - -extra_includes: -- frc2/command/Command.h -- frc2/command/Subsystem.h - -classes: - TrapezoidProfileCommand: - shared_ptr: true - template_params: - - Distance - typealias: - - State = typename frc::TrapezoidProfile::State - methods: - TrapezoidProfileCommand: - overloads: - frc::TrapezoidProfile, std::function, std::initializer_list>: - ignore: true - frc::TrapezoidProfile, std::function, std::span>: - param_override: - output: - x_type: std::function - Initialize: - Execute: - End: - IsFinished: - -templates: - TrapezoidProfileCommand: - qualname: frc2::TrapezoidProfileCommand - params: - - units::dimensionless::scalar - - # needed for HolonomicDriveController - TrapezoidProfileCommandRadians: - qualname: frc2::TrapezoidProfileCommand - params: - - units::radian \ No newline at end of file diff --git a/gen/TrapezoidProfileSubsystem.yml b/gen/TrapezoidProfileSubsystem.yml deleted file mode 100644 index f7315a63..00000000 --- a/gen/TrapezoidProfileSubsystem.yml +++ /dev/null @@ -1,34 +0,0 @@ ---- - -classes: - TrapezoidProfileSubsystem: - shared_ptr: true - template_params: - - Distance - typealias: - - Distance_t = units::unit_t - methods: - TrapezoidProfileSubsystem: - param_override: - period: - default: 20_ms - Periodic: - SetGoal: - overloads: - 'typename frc::TrapezoidProfile::State': - units::unit_t: - UseState: - Enable: - Disable: - -templates: - TrapezoidProfileSubsystem: - qualname: frc2::TrapezoidProfileSubsystem - params: - - units::dimensionless::scalar - - # needed for HolonomicDriveController - TrapezoidProfileSubsystemRadians: - qualname: frc2::TrapezoidProfileSubsystem - params: - - units::radian \ No newline at end of file diff --git a/gen/Trigger.yml b/gen/Trigger.yml deleted file mode 100644 index c82f7c74..00000000 --- a/gen/Trigger.yml +++ /dev/null @@ -1,106 +0,0 @@ ---- - -extra_includes: -- frc2/command/Command.h - -classes: - Trigger: - methods: - Trigger: - overloads: - std::function: - frc::EventLoop*, std::function: - "": - OnTrue: - overloads: - std::shared_ptr: - CommandPtr&&: - ignore: true - OnFalse: - overloads: - std::shared_ptr: - CommandPtr&&: - ignore: true - WhileTrue: - overloads: - std::shared_ptr: - CommandPtr&&: - ignore: true - WhileFalse: - overloads: - std::shared_ptr: - CommandPtr&&: - ignore: true - ToggleOnTrue: - overloads: - std::shared_ptr: - CommandPtr&&: - ignore: true - ToggleOnFalse: - overloads: - std::shared_ptr: - CommandPtr&&: - ignore: true - WhenActive: - overloads: - std::shared_ptr: - T&&: - ignore: true - std::function, std::span>: - WhileActiveContinous: - overloads: - std::shared_ptr: - T&&: - ignore: true - std::function, std::span>: - WhileActiveOnce: - overloads: - std::shared_ptr: - T&&: - ignore: true - WhenInactive: - overloads: - std::shared_ptr: - T&&: - ignore: true - std::function, std::span>: - ToggleWhenActive: - overloads: - std::shared_ptr: - T&&: - ignore: true - CancelWhenActive: - Rising: - Falling: - Get: - rename: __bool__ - Debounce: - GetEvent: - inline_code: | - .def("and_", [](Trigger * self, Trigger * other) { - return *self && *other; - }, py::arg("other"), - "Composes this trigger with another trigger, returning a new trigger that is active when both\n" - "triggers are active.\n" - "\n" - ":param trigger: the trigger to compose with\n" - "\n" - ":returns: the trigger that is active when both triggers are active\n") - .def("or_", [](Trigger * self, Trigger * other) { - return *self || *other; - }, py::arg("other"), - "Composes this trigger with another trigger, returning a new trigger that is active when either\n" - "triggers are active.\n" - "\n" - ":param trigger: the trigger to compose with\n" - "\n" - ":returns: the trigger that is active when both triggers are active\n") - .def("not_", [](Trigger * self) { - return !*self; - }, - "Creates a new trigger that is active when this trigger is inactive, i.e. that acts as the\n" - "negation of this trigger.\n" - "\n" - ":param trigger: the trigger to compose with\n" - "\n" - ":returns: the trigger that is active when both triggers are active\n") diff --git a/gen/WaitCommand.yml b/gen/WaitCommand.yml deleted file mode 100644 index 0c35f110..00000000 --- a/gen/WaitCommand.yml +++ /dev/null @@ -1,23 +0,0 @@ ---- - -extra_includes: -- frc2/command/Command.h -- frc2/command/Subsystem.h - -classes: - WaitCommand: - shared_ptr: true - attributes: - m_timer: - methods: - WaitCommand: - Initialize: - End: - IsFinished: - RunsWhenDisabled: - InitSendable: - virtual_xform: | - [&](py::function fn) { - auto builderHandle = py::cast(builder, py::return_value_policy::reference); - fn(builderHandle); - } diff --git a/gen/WaitUntilCommand.yml b/gen/WaitUntilCommand.yml deleted file mode 100644 index 06434e41..00000000 --- a/gen/WaitUntilCommand.yml +++ /dev/null @@ -1,16 +0,0 @@ ---- - -extra_includes: -- frc2/command/Command.h -- frc2/command/Subsystem.h - -classes: - WaitUntilCommand: - shared_ptr: true - methods: - WaitUntilCommand: - overloads: - std::function: - units::second_t: - IsFinished: - RunsWhenDisabled: diff --git a/pyproject.toml b/pyproject.toml deleted file mode 100644 index 8856394a..00000000 --- a/pyproject.toml +++ /dev/null @@ -1,139 +0,0 @@ -[tool.robotpy-build.metadata] -name = "robotpy-commands-v2" -description = "WPILib command framework v2" -author = "RobotPy Development Team" -author_email = "robotpy@googlegroups.com" -url = "https://github.com/robotpy/robotpy-commands-v2" -license = "BSD-3-Clause" -install_requires = [ - "wpilib~=2023.4.3", -] - -[build-system] -requires = [ - "robotpy-build<2024.0.0,>=2023.1.1", - "wpilib~=2023.4.3", -] - -[tool.robotpy-build] -base_package = "commands2" - -[tool.robotpy-build.wrappers."commands2"] -name = "commands_v2" -extension = "_impl" -sources = [ - "commands2/src/main.cpp", - "commands2/src/helpers.cpp", - - "commands2/src/cpp/frc2/command/button/Button.cpp", - "commands2/src/cpp/frc2/command/button/CommandGenericHID.cpp", - "commands2/src/cpp/frc2/command/button/CommandJoystick.cpp", - "commands2/src/cpp/frc2/command/button/CommandPS4Controller.cpp", - "commands2/src/cpp/frc2/command/button/CommandXboxController.cpp", - "commands2/src/cpp/frc2/command/button/NetworkButton.cpp", - "commands2/src/cpp/frc2/command/button/Trigger.cpp", - - "commands2/src/cpp/frc2/command/Command.cpp", - "commands2/src/cpp/frc2/command/CommandBase.cpp", - "commands2/src/cpp/frc2/command/CommandGroupBase.cpp", - # "commands2/src/cpp/frc2/command/CommandPtr.cpp", - "commands2/src/cpp/frc2/command/Commands.cpp", - "commands2/src/cpp/frc2/command/CommandScheduler.cpp", - "commands2/src/cpp/frc2/command/ConditionalCommand.cpp", - "commands2/src/cpp/frc2/command/FunctionalCommand.cpp", - "commands2/src/cpp/frc2/command/InstantCommand.cpp", - "commands2/src/cpp/frc2/command/MecanumControllerCommand.cpp", - "commands2/src/cpp/frc2/command/NotifierCommand.cpp", - "commands2/src/cpp/frc2/command/ParallelCommandGroup.cpp", - "commands2/src/cpp/frc2/command/ParallelDeadlineGroup.cpp", - "commands2/src/cpp/frc2/command/ParallelRaceGroup.cpp", - "commands2/src/cpp/frc2/command/PerpetualCommand.cpp", - "commands2/src/cpp/frc2/command/PIDCommand.cpp", - "commands2/src/cpp/frc2/command/PIDSubsystem.cpp", - "commands2/src/cpp/frc2/command/PrintCommand.cpp", - "commands2/src/cpp/frc2/command/ProxyCommand.cpp", - "commands2/src/cpp/frc2/command/ProxyScheduleCommand.cpp", - "commands2/src/cpp/frc2/command/RamseteCommand.cpp", - "commands2/src/cpp/frc2/command/RepeatCommand.cpp", - "commands2/src/cpp/frc2/command/RunCommand.cpp", - "commands2/src/cpp/frc2/command/ScheduleCommand.cpp", - "commands2/src/cpp/frc2/command/SequentialCommandGroup.cpp", - "commands2/src/cpp/frc2/command/StartEndCommand.cpp", - "commands2/src/cpp/frc2/command/Subsystem.cpp", - "commands2/src/cpp/frc2/command/SubsystemBase.cpp", - "commands2/src/cpp/frc2/command/WaitCommand.cpp", - "commands2/src/cpp/frc2/command/WaitUntilCommand.cpp", - # "commands2/src/cpp/frc2/command/WrapperCommand.cpp", -] -depends = [ - "wpilib_core", "wpilibc_interfaces", "wpilibc", - "wpimath_cpp", "wpimath_controls", "wpimath_geometry", "wpimath_filter", - "wpimath_kinematics", "wpimath_spline", - "wpiHal", "wpiutil", "ntcore", -] - -extra_includes = [ - "commands2/src/include" -] - -generation_data = "gen" - -[tool.robotpy-build.wrappers."commands2".autogen_headers] - -# frc2/command -Command = "frc2/command/Command.h" -CommandBase = "frc2/command/CommandBase.h" -CommandGroupBase = "frc2/command/CommandGroupBase.h" -# CommandHelper = "frc2/command/CommandHelper.h" -# CommandPtr = "frc2/command/CommandPtr.h" -CommandScheduler = "frc2/command/CommandScheduler.h" -Commands = "frc2/command/Commands.h" -ConditionalCommand = "frc2/command/ConditionalCommand.h" -# DenseMapInfo = "frc2/command/DenseMapInfo.h" -FunctionalCommand = "frc2/command/FunctionalCommand.h" -InstantCommand = "frc2/command/InstantCommand.h" -MecanumControllerCommand = "frc2/command/MecanumControllerCommand.h" -NotifierCommand = "frc2/command/NotifierCommand.h" -PIDCommand = "frc2/command/PIDCommand.h" -PIDSubsystem = "frc2/command/PIDSubsystem.h" -ParallelCommandGroup = "frc2/command/ParallelCommandGroup.h" -ParallelDeadlineGroup = "frc2/command/ParallelDeadlineGroup.h" -ParallelRaceGroup = "frc2/command/ParallelRaceGroup.h" -PerpetualCommand = "frc2/command/PerpetualCommand.h" -PrintCommand = "frc2/command/PrintCommand.h" -ProfiledPIDCommand = "frc2/command/ProfiledPIDCommand.h" -ProfiledPIDSubsystem = "frc2/command/ProfiledPIDSubsystem.h" -ProxyCommand = "frc2/command/ProxyCommand.h" -ProxyScheduleCommand = "frc2/command/ProxyScheduleCommand.h" -RamseteCommand = "frc2/command/RamseteCommand.h" -RepeatCommand = "frc2/command/RepeatCommand.h" -RunCommand = "frc2/command/RunCommand.h" -ScheduleCommand = "frc2/command/ScheduleCommand.h" -# TODO -SelectCommand = "frc2/command/SelectCommand.h" -SequentialCommandGroup = "frc2/command/SequentialCommandGroup.h" -# SetUtilities = "frc2/command/SetUtilities.h" -StartEndCommand = "frc2/command/StartEndCommand.h" -Subsystem = "frc2/command/Subsystem.h" -SubsystemBase = "frc2/command/SubsystemBase.h" -SwerveControllerCommand = "frc2/command/SwerveControllerCommand.h" -TrapezoidProfileCommand = "frc2/command/TrapezoidProfileCommand.h" -TrapezoidProfileSubsystem = "frc2/command/TrapezoidProfileSubsystem.h" -WaitCommand = "frc2/command/WaitCommand.h" -WaitUntilCommand = "frc2/command/WaitUntilCommand.h" -# WrapperCommand = "frc2/command/WrapperCommand.h" - -# frc2/command/button -Button = "frc2/command/button/Button.h" -CommandGenericHID = "frc2/command/button/CommandGenericHID.h" -CommandJoystick = "frc2/command/button/CommandJoystick.h" -CommandPS4Controller = "frc2/command/button/CommandPS4Controller.h" -CommandXboxController = "frc2/command/button/CommandXboxController.h" -JoystickButton = "frc2/command/button/JoystickButton.h" -NetworkButton = "frc2/command/button/NetworkButton.h" -POVButton = "frc2/command/button/POVButton.h" -Trigger = "frc2/command/button/Trigger.h" - -# custom stuff -TimedCommandRobot = "src/TimedCommandRobot.h" - diff --git a/setup.py b/setup.py index 3542d0c9..b1c069cb 100644 --- a/setup.py +++ b/setup.py @@ -1,5 +1,18 @@ #!/usr/bin/env python3 -from robotpy_build.setup import setup +import setuptools -setup() +setuptools.setup( + name="robotpy-commands-v2", + use_scm_version=True, + setup_requires=["setuptools_scm"], + author="RobotPy Development Team", + author_email="robotpy@googlegroups.com", + description="WPILib command framework v2", + url="https://github.com/robotpy/robotpy-commands-v2", + packages=["commands2"], + install_requires=["wpilib<2025,>=2024.0.0b2", "typing_extensions>=4.1.0,<5"], + license="BSD-3-Clause", + python_requires=">=3.8", + include_package_data=True, +) diff --git a/tests/compositiontestbase.py b/tests/compositiontestbase.py new file mode 100644 index 00000000..953769a3 --- /dev/null +++ b/tests/compositiontestbase.py @@ -0,0 +1,146 @@ +from typing import Generic, TypeVar + +import commands2 +import pytest + +# T = TypeVar("T", bound=commands2.Command) +# T = commands2.Command + +from util import * +if not IS_OLD_COMMANDS: + + 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 + + + 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 + +else: + class SingleCompositionTestBase: ... + class MultiCompositionTestBase: ... diff --git a/tests/conftest.py b/tests/conftest.py index 9c193904..96f18263 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -1,6 +1,6 @@ -import pytest - import commands2 +import pytest +from ntcore import NetworkTableInstance from wpilib.simulation import DriverStationSim @@ -10,3 +10,11 @@ def scheduler(): 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/tests/run_tests.py b/tests/run_tests.py index 2ffc6595..35ac1d70 100755 --- a/tests/run_tests.py +++ b/tests/run_tests.py @@ -1,12 +1,20 @@ #!/usr/bin/env python3 import os -from os.path import abspath, dirname -import sys import subprocess +import sys +from os.path import abspath, dirname if __name__ == "__main__": root = abspath(dirname(__file__)) os.chdir(root) subprocess.check_call([sys.executable, "-m", "pytest"]) + + + print("---------------------------------") + print("Testing with C++ Wrapped Commands") + subprocess.check_call([sys.executable, "-m", "pip", "uninstall", "-y", "robotpy-commands-v2"]) + subprocess.check_call([sys.executable, "-m", "pip", "install", "robotpy-commands-v2"]) + + subprocess.check_call([sys.executable, "-m", "pytest"]) diff --git a/tests/test_button.py b/tests/test_button.py deleted file mode 100644 index 1b100655..00000000 --- a/tests/test_button.py +++ /dev/null @@ -1,202 +0,0 @@ -import commands2 -import commands2.button - -from util import Counter - - -class MyButton(commands2.button.Button): - 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 - - -class MyCommand(commands2.CommandBase): - executed = 0 - - ended = 0 - canceled = 0 - - def execute(self) -> None: - self.executed += 1 - - def end(self, interrupted: bool) -> None: - self.ended += 1 - if interrupted: - self.canceled += 1 - - -def test_when_pressed(scheduler: commands2.CommandScheduler): - cmd1 = MyCommand() - button = MyButton() - button.setPressed(False) - - button.whenPressed(cmd1) - scheduler.run() - - assert not cmd1.executed - assert not scheduler.isScheduled(cmd1) - - button.setPressed(True) - scheduler.run() - scheduler.run() - - assert cmd1.executed - assert scheduler.isScheduled(cmd1) - - -def test_when_released(scheduler: commands2.CommandScheduler): - cmd1 = MyCommand() - button = MyButton() - button.setPressed(True) - - button.whenReleased(cmd1) - scheduler.run() - - assert not cmd1.executed - assert not scheduler.isScheduled(cmd1) - - button.setPressed(False) - scheduler.run() - scheduler.run() - - assert cmd1.executed - assert scheduler.isScheduled(cmd1) - - -def test_while_held(scheduler: commands2.CommandScheduler): - cmd1 = MyCommand() - button = MyButton() - button.setPressed(False) - - button.whileHeld(cmd1) - scheduler.run() - - assert not cmd1.executed - assert not scheduler.isScheduled(cmd1) - - button.setPressed(True) - scheduler.run() - scheduler.run() - - assert cmd1.executed == 2 - assert scheduler.isScheduled(cmd1) - - button.setPressed(False) - scheduler.run() - - assert cmd1.executed == 2 - assert not scheduler.isScheduled(cmd1) - - -def test_when_held(scheduler: commands2.CommandScheduler): - cmd1 = MyCommand() - button = MyButton() - button.setPressed(False) - - button.whenHeld(cmd1) - scheduler.run() - - assert not cmd1.executed - assert not scheduler.isScheduled(cmd1) - - button.setPressed(True) - scheduler.run() - scheduler.run() - - assert cmd1.executed == 2 - assert scheduler.isScheduled(cmd1) - - button.setPressed(False) - scheduler.run() - - assert cmd1.executed == 2 - assert not scheduler.isScheduled(cmd1) - - -def test_toggle_when_pressed(scheduler: commands2.CommandScheduler): - cmd1 = MyCommand() - button = MyButton() - button.setPressed(False) - - button.toggleWhenPressed(cmd1) - scheduler.run() - - assert not cmd1.executed - assert not scheduler.isScheduled(cmd1) - - button.setPressed(True) - scheduler.run() - - assert cmd1.executed - assert scheduler.isScheduled(cmd1) - - -def test_cancel_when_pressed(scheduler: commands2.CommandScheduler): - cmd1 = MyCommand() - button = MyButton() - button.setPressed(False) - - scheduler.schedule(cmd1) - - button.cancelWhenPressed(cmd1) - scheduler.run() - - assert cmd1.executed == 1 - assert cmd1.ended == 0 - assert cmd1.canceled == 0 - assert scheduler.isScheduled(cmd1) - - button.setPressed(True) - scheduler.run() - scheduler.run() - - assert cmd1.executed == 1 - assert cmd1.ended == 1 - assert cmd1.canceled == 1 - assert not scheduler.isScheduled(cmd1) - - -def test_function_bindings(scheduler: commands2.CommandScheduler): - buttonWhenPressed = MyButton() - buttonWhileHeld = MyButton() - buttonWhenReleased = MyButton() - - buttonWhenPressed.setPressed(False) - buttonWhileHeld.setPressed(True) - buttonWhenReleased.setPressed(True) - - counter = Counter() - - buttonWhenPressed.whenPressed(counter.increment) - buttonWhileHeld.whileHeld(counter.increment) - buttonWhenReleased.whenReleased(counter.increment) - - scheduler.run() - buttonWhenPressed.setPressed(True) - buttonWhenReleased.setPressed(False) - scheduler.run() - - assert counter.value == 4 - - -def test_button_composition(scheduler: commands2.CommandScheduler): - button1 = MyButton() - button2 = MyButton() - - button1.setPressed(True) - button2.setPressed(False) - - # TODO: not sure if this is a great idea? - assert button1 - assert not button2 - - assert not button1.and_(button2) - assert button1.or_(button2) - assert not button1.not_() - assert button1.and_(button2.not_()) diff --git a/tests/test_command_decorators.py b/tests/test_command_decorators.py index 369bd2dc..0831d6cb 100644 --- a/tests/test_command_decorators.py +++ b/tests/test_command_decorators.py @@ -1,145 +1,221 @@ -from util import ConditionHolder, ManualSimTime -import commands2 +from typing import TYPE_CHECKING +from util import * # type: ignore -def test_with_timeout(scheduler: commands2.CommandScheduler): - with ManualSimTime() as tm: - cmd = commands2.WaitCommand(10) - timeout = cmd.withTimeout(2) +if TYPE_CHECKING: + from .util import * - scheduler.schedule(timeout) - scheduler.run() - - assert not scheduler.isScheduled(cmd) - assert scheduler.isScheduled(timeout) +import commands2 +import pytest - tm.step(3) +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 scheduler.isScheduled(timeout) - - -def test_with_interrupt(scheduler: commands2.CommandScheduler): - cond = ConditionHolder() + assert not command1.isScheduled() + assert timeout.isScheduled() + sim.step(3) + scheduler.run() + assert not timeout.isScheduled() - cmd = commands2.WaitCommand(10) - timeout = cmd.withInterrupt(cond.getCondition) - scheduler.schedule(timeout) +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() - assert not scheduler.isScheduled(cmd) - assert scheduler.isScheduled(timeout) +@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") +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() - cond.setTrue() +@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") +def test_ignoringDisable(scheduler: commands2.CommandScheduler): + command = commands2.RunCommand(lambda: None).ignoringDisable(True) + DriverStationSim.setEnabled(False) + DriverStationSim.notifyNewData() + scheduler.schedule(command) scheduler.run() - assert not scheduler.isScheduled(cmd) + assert command.isScheduled() -def test_before_starting(scheduler: commands2.CommandScheduler): - cond = ConditionHolder() +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 - cmd = commands2.InstantCommand().beforeStarting(cond.setTrue) - scheduler.schedule(cmd) +@pytest.mark.skip +def test_andThenLambda(scheduler: commands2.CommandScheduler): + ... - assert cond.getCondition() +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() - scheduler.run() - - assert not scheduler.isScheduled(cmd) - + assert condition == True -def test_and_then_fn(scheduler: commands2.CommandScheduler): - cond = ConditionHolder() - cmd = commands2.InstantCommand().andThen(cond.setTrue) +def test_deadlineWith(scheduler: commands2.CommandScheduler): + condition = OOBoolean(False) + condition.set(False) - scheduler.schedule(cmd) + dictator = commands2.WaitUntilCommand(condition) + endsBefore = commands2.InstantCommand() + endsAfter = commands2.WaitUntilCommand(lambda: False) - assert not cond.getCondition() + group = dictator.deadlineWith(endsBefore, endsAfter) + scheduler.schedule(group) scheduler.run() + assert group.isScheduled() + condition.set(True) scheduler.run() - - assert not scheduler.isScheduled(cmd) - assert cond.getCondition() + assert not group.isScheduled() -def test_and_then_commands(scheduler: commands2.CommandScheduler): - cond = ConditionHolder() +def test_alongWith(scheduler: commands2.CommandScheduler): + condition = OOBoolean() + condition.set(False) - cmd1 = commands2.InstantCommand() - cmd2 = commands2.InstantCommand(cond.setTrue) + command1 = commands2.WaitUntilCommand(condition) + command2 = commands2.InstantCommand() - scheduler.schedule(cmd1.andThen(cmd2)) - - assert not cond.getCondition() + group = command1.alongWith(command2) + scheduler.schedule(group) scheduler.run() + assert group.isScheduled() + condition.set(True) + scheduler.run() + assert not group.isScheduled() - assert cond.getCondition() - - -def test_deadline_with(scheduler: commands2.CommandScheduler): - cond = ConditionHolder() - dictator = commands2.WaitUntilCommand(cond.getCondition) - endsBefore = commands2.InstantCommand() - endsAfter = commands2.WaitUntilCommand(lambda: False) +def test_raceWith(scheduler: commands2.CommandScheduler): + command1 = commands2.WaitUntilCommand(lambda: False) + command2 = commands2.InstantCommand() - group = dictator.deadlineWith(endsBefore, endsAfter) + group = command1.raceWith(command2) scheduler.schedule(group) scheduler.run() + assert not group.isScheduled() - assert scheduler.isScheduled(group) - cond.setTrue() - scheduler.run() +# def test_perpetually(scheduler: commands2.CommandScheduler): +# command = commands2.InstantCommand() +# perpetual = command.perpetually() +# scheduler.schedule(perpetual) +# scheduler.run() +# scheduler.run() +# scheduler.run() +# assert perpetual.isScheduled() - assert not scheduler.isScheduled(group) +@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") +def test_unless(scheduler: commands2.CommandScheduler): + unlessCondition = OOBoolean(True) + hasRunCondition = OOBoolean(False) -def test_along_with(scheduler: commands2.CommandScheduler): - cond = ConditionHolder(False) + command = commands2.InstantCommand(lambda: hasRunCondition.set(True)).unless( + unlessCondition + ) - cmd1 = commands2.WaitUntilCommand(cond.getCondition) - cmd2 = commands2.InstantCommand() + scheduler.schedule(command) + scheduler.run() + assert hasRunCondition == False + unlessCondition.set(False) + scheduler.schedule(command) + scheduler.run() + assert hasRunCondition == True - group = cmd1.alongWith(cmd2) - scheduler.schedule(group) - scheduler.run() +@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") +def test_onlyIf(scheduler: commands2.CommandScheduler): + onlyIfCondition = OOBoolean(False) + hasRunCondition = OOBoolean(False) - assert scheduler.isScheduled(group) + command = commands2.InstantCommand(lambda: hasRunCondition.set(True)).onlyIf( + onlyIfCondition + ) - cond.setTrue() + scheduler.schedule(command) scheduler.run() - - assert not scheduler.isScheduled(group) + assert hasRunCondition == False + onlyIfCondition.set(True) + scheduler.schedule(command) + scheduler.run() + assert hasRunCondition == True -def test_race_with(scheduler: commands2.CommandScheduler): - cmd1 = commands2.WaitUntilCommand(lambda: False) - cmd2 = commands2.InstantCommand() +@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") +def test_finallyDo(scheduler: commands2.CommandScheduler): + first = OOInteger(0) + second = OOInteger(0) - group = cmd1.raceWith(cmd2) + 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(group) + scheduler.schedule(command) + assert first == 0 + assert second == 0 scheduler.run() - - assert not scheduler.isScheduled(group) + assert first == 1 + assert second == 2 -def test_perpetually(scheduler: commands2.CommandScheduler): - cmd = commands2.InstantCommand().perpetually() +@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") +def test_handleInterrupt(scheduler: commands2.CommandScheduler): + first = OOInteger(0) + second = OOInteger(0) - scheduler.schedule(cmd) + 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() - scheduler.run() - - assert scheduler.isScheduled(cmd) + assert first == 0 + assert second == 0 + scheduler.cancel(command) + assert first == 1 + assert second == 2 + + +@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") +def test_withName(scheduler: commands2.CommandScheduler): + command = commands2.InstantCommand() + name = "Named" + named = command.withName(name) + assert named.getName() == name diff --git a/tests/test_command_group_adds.py b/tests/test_command_group_adds.py deleted file mode 100644 index 58ee700b..00000000 --- a/tests/test_command_group_adds.py +++ /dev/null @@ -1,75 +0,0 @@ -import commands2 -from commands2 import WaitCommand - -import pytest - - -class MySubsystem(commands2.SubsystemBase): - def __init__(self, param) -> None: - super().__init__() - self.param = param - - -class MyCommand(commands2.CommandBase): - def __init__(self, param) -> None: - super().__init__() - self.addRequirements(MySubsystem(param)) - - -def get_params(g): - return sorted(s.param for s in g.getRequirements()) - - -@pytest.mark.parametrize( - "ccls", - [ - commands2.ParallelCommandGroup, - lambda *args: commands2.ParallelDeadlineGroup(WaitCommand(1), *args), - commands2.ParallelRaceGroup, - commands2.SequentialCommandGroup, - ], -) -def test_command_group_constructors(ccls): - # individual param form - g = ccls(MyCommand(1)) - assert get_params(g) == [1] - - g = ccls(MyCommand(1), MyCommand(2)) - assert get_params(g) == [1, 2] - - # list form - g = ccls([MyCommand(1)]) - assert get_params(g) == [1] - - g = ccls([MyCommand(1), MyCommand(2)]) - assert get_params(g) == [1, 2] - - # tuple form - g = ccls((MyCommand(1),)) - assert get_params(g) == [1] - - g = ccls((MyCommand(1), MyCommand(2))) - assert get_params(g) == [1, 2] - - -@pytest.mark.parametrize( - "ccls", - [ - commands2.ParallelCommandGroup, - lambda *args: commands2.ParallelDeadlineGroup(WaitCommand(1), *args), - commands2.ParallelRaceGroup, - commands2.SequentialCommandGroup, - ], -) -def test_command_group_add_commands(ccls): - g = ccls() - - # individual param form - g.addCommands(WaitCommand(1)) - g.addCommands(WaitCommand(1), WaitCommand(2)) - - # list form - g.addCommands([WaitCommand(1)]) - g.addCommands([WaitCommand(1), WaitCommand(2)]) - - # tuple form diff --git a/tests/test_command_group_errors.py b/tests/test_command_group_errors.py deleted file mode 100644 index 49197020..00000000 --- a/tests/test_command_group_errors.py +++ /dev/null @@ -1,35 +0,0 @@ -import commands2 -import pytest - - -def test_multiple_groups(): - cmd1 = commands2.InstantCommand() - cmd2 = commands2.InstantCommand() - - _ = commands2.ParallelCommandGroup([cmd1, cmd2]) - with pytest.raises(RuntimeError): - commands2.ParallelCommandGroup(cmd1, cmd2) - - -def test_externally_scheduled(scheduler: commands2.CommandScheduler): - cmd1 = commands2.InstantCommand() - cmd2 = commands2.InstantCommand() - - _ = commands2.SequentialCommandGroup([cmd1, cmd2]) - with pytest.raises( - RuntimeError, - match="Illegal use of Command: Commands that have been composed may not be added to another composition or scheduled individually!", - ): - scheduler.schedule(cmd1) - - -def test_redecorated(): - cmd = commands2.InstantCommand() - - _ = cmd.withTimeout(10).withInterrupt(lambda: False) - - with pytest.raises(RuntimeError): - cmd.withTimeout(10) - - cmd.setGrouped(False) - _ = cmd.withTimeout(10) diff --git a/tests/test_command_requirements.py b/tests/test_command_requirements.py new file mode 100644 index 00000000..2faafb72 --- /dev/null +++ b/tests/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() + +@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") +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/tests/test_command_schedule.py b/tests/test_command_schedule.py new file mode 100644 index 00000000..02eac27f --- /dev/null +++ b/tests/test_command_schedule.py @@ -0,0 +1,110 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +from ntcore import NetworkTableInstance +from wpilib import SmartDashboard + +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) + + +def test_smartDashboardCancelTest(scheduler: commands2.CommandScheduler, nt_instance: NetworkTableInstance): + SmartDashboard.putData("Scheduler", scheduler) + SmartDashboard.updateValues() + + command = commands2.Command() + scheduler.schedule(command) + scheduler.run() + SmartDashboard.updateValues() + assert scheduler.isScheduled(command) + + table = nt_instance.getTable("SmartDashboard") + table.getEntry("Scheduler/Cancel").setIntegerArray([id(command)]) + SmartDashboard.updateValues() + scheduler.run() + assert not scheduler.isScheduled(command) + diff --git a/tests/test_command_sendable_button.py b/tests/test_command_sendable_button.py new file mode 100644 index 00000000..2a6fbe5b --- /dev/null +++ b/tests/test_command_sendable_button.py @@ -0,0 +1,102 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +from ntcore import NetworkTableInstance +from wpilib import SmartDashboard + +import pytest + +def test_trueAndNotScheduledSchedules(scheduler: commands2.CommandScheduler, nt_instance: NetworkTableInstance): + schedule = OOInteger(0) + cancel = OOInteger(0) + + command = commands2.cmd.startEnd(schedule.incrementAndGet, cancel.incrementAndGet) + publish = nt_instance.getBooleanTopic("/SmartDashboard/command/running").publish() + SmartDashboard.putData("command", command) + SmartDashboard.updateValues() + # + commands2.CommandScheduler.getInstance().run() + SmartDashboard.updateValues() + assert not command.isScheduled() + assert schedule == 0 + assert cancel == 0 + + publish.set(True) + SmartDashboard.updateValues() + commands2.CommandScheduler.getInstance().run() + assert command.isScheduled() + assert schedule == 1 + assert cancel == 0 + +def test_trueAndScheduleNoOp(scheduler: commands2.CommandScheduler, nt_instance: NetworkTableInstance): + schedule = OOInteger(0) + cancel = OOInteger(0) + + command = commands2.cmd.startEnd(schedule.incrementAndGet, cancel.incrementAndGet) + publish = nt_instance.getBooleanTopic("/SmartDashboard/command/running").publish() + SmartDashboard.putData("command", command) + SmartDashboard.updateValues() + # + command.schedule() + commands2.CommandScheduler.getInstance().run() + SmartDashboard.updateValues() + assert command.isScheduled() + assert schedule == 1 + assert cancel == 0 + + publish.set(True) + SmartDashboard.updateValues() + commands2.CommandScheduler.getInstance().run() + assert command.isScheduled() + assert schedule == 1 + assert cancel == 0 + +def test_falseAndNotScheduledNoOp(scheduler: commands2.CommandScheduler, nt_instance: NetworkTableInstance): + schedule = OOInteger(0) + cancel = OOInteger(0) + + command = commands2.cmd.startEnd(schedule.incrementAndGet, cancel.incrementAndGet) + publish = nt_instance.getBooleanTopic("/SmartDashboard/command/running").publish() + SmartDashboard.putData("command", command) + SmartDashboard.updateValues() + # + commands2.CommandScheduler.getInstance().run() + SmartDashboard.updateValues() + assert not command.isScheduled() + assert schedule == 0 + assert cancel == 0 + + publish.set(False) + SmartDashboard.updateValues() + commands2.CommandScheduler.getInstance().run() + assert not command.isScheduled() + assert schedule == 0 + assert cancel == 0 + +def test_falseAndScheduleCancel(scheduler: commands2.CommandScheduler, nt_instance: NetworkTableInstance): + schedule = OOInteger(0) + cancel = OOInteger(0) + + command = commands2.cmd.startEnd(schedule.incrementAndGet, cancel.incrementAndGet) + publish = nt_instance.getBooleanTopic("/SmartDashboard/command/running").publish() + SmartDashboard.putData("command", command) + SmartDashboard.updateValues() + # + command.schedule() + commands2.CommandScheduler.getInstance().run() + SmartDashboard.updateValues() + assert command.isScheduled() + assert schedule == 1 + assert cancel == 0 + + publish.set(False) + SmartDashboard.updateValues() + commands2.CommandScheduler.getInstance().run() + assert not command.isScheduled() + assert schedule == 1 + assert cancel == 1 \ No newline at end of file diff --git a/tests/test_commandgroup_error.py b/tests/test_commandgroup_error.py new file mode 100644 index 00000000..36008f1a --- /dev/null +++ b/tests/test_commandgroup_error.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_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) + + +@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") +def test_redecoratedCommandError(scheduler: commands2.CommandScheduler): + command = commands2.InstantCommand() + command.withTimeout(10).until(lambda: False) + with pytest.raises(commands2.IllegalCommandUse): + command.withTimeout(10) + scheduler.removeComposedCommands([command]) + command.withTimeout(10) diff --git a/tests/test_conditional_command.py b/tests/test_conditional_command.py new file mode 100644 index 00000000..bf069d17 --- /dev/null +++ b/tests/test_conditional_command.py @@ -0,0 +1,159 @@ +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) + + +@pytest.mark.parametrize( + "name,expected,command1,command2,selector", + [ + ( + "AllCancelSelf", + commands2.InterruptionBehavior.kCancelSelf, + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelSelf + ), + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelSelf + ), + lambda: True, + ), + ( + "AllCancelIncoming", + commands2.InterruptionBehavior.kCancelIncoming, + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelIncoming + ), + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelIncoming + ), + lambda: True, + ), + ( + "OneCancelSelfOneIncoming", + commands2.InterruptionBehavior.kCancelSelf, + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelSelf + ), + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelIncoming + ), + lambda: True, + ), + ( + "OneCancelIncomingOneSelf", + commands2.InterruptionBehavior.kCancelSelf, + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelIncoming + ), + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelSelf + ), + lambda: True, + ), + ], +) +def test_interruptible( + name, + expected, + command1, + command2, + selector, +): + command = commands2.ConditionalCommand(command1, command2, selector) + assert command.getInterruptionBehavior() == expected + + +@pytest.mark.parametrize( + "name,expected,command1,command2,selector", + [ + ( + "AllFalse", + False, + commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), + commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), + lambda: True, + ), + ( + "AllTrue", + True, + commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), + commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), + lambda: True, + ), + ( + "OneTrueOneFalse", + False, + commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), + commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), + lambda: True, + ), + ( + "OneFalseOneTrue", + False, + commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), + commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), + lambda: True, + ), + ], +) +def test_runsWhenDisabled( + name, + expected, + command1, + command2, + selector, +): + command = commands2.ConditionalCommand(command1, command2, selector) + assert command.runsWhenDisabled() == expected \ No newline at end of file diff --git a/tests/test_default_command.py b/tests/test_default_command.py new file mode 100644 index 00000000..211ecae4 --- /dev/null +++ b/tests/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/tests/test_deferred_command.py b/tests/test_deferred_command.py new file mode 100644 index 00000000..521b7466 --- /dev/null +++ b/tests/test_deferred_command.py @@ -0,0 +1,69 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + +#parameterized on true and false +@pytest.mark.parametrize( + "interrupted", + [ + True, + False, + ], +) +def test_deferredFunctions(interrupted): + inner_command = commands2.Command() + start_spying_on(inner_command) + command = commands2.DeferredCommand(lambda: inner_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(2)).isFinished() + + command.end(interrupted) + verify(inner_command).end(interrupted) + +def test_deferredSupplierOnlyCalledDuringInit(scheduler: commands2.CommandScheduler): + class Supplier: + def get(self): + return commands2.cmd.none() + supplier = Supplier() + start_spying_on(supplier) + + command = commands2.DeferredCommand(supplier) + verify(supplier, never()).get() + + scheduler.schedule(command) + verify(supplier, times(1)).get() + scheduler.run() + + scheduler.schedule(command) + verify(supplier, times(2)).get() + +def test_deferredRequirements(): + subsystem = commands2.Subsystem() + command = commands2.DeferredCommand(commands2.cmd.none(), subsystem) + + assert subsystem in command.getRequirements() + +def test_deferredNullCommand(): + command = commands2.DeferredCommand(lambda: None) + + command.initialize() + command.execute() + command.isFinished() + command.end(False) diff --git a/tests/test_functional_command.py b/tests/test_functional_command.py new file mode 100644 index 00000000..c6481cab --- /dev/null +++ b/tests/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/tests/test_instant_command.py b/tests/test_instant_command.py index 7520cc60..96d3089e 100644 --- a/tests/test_instant_command.py +++ b/tests/test_instant_command.py @@ -1,14 +1,21 @@ +from typing import TYPE_CHECKING + import commands2 -from util import ConditionHolder +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest -def test_instant_command(scheduler: commands2.CommandScheduler): - cond = ConditionHolder() +def test_instantCommandSchedule(scheduler: commands2.CommandScheduler): + cond = OOBoolean() - cmd = commands2.InstantCommand(cond.setTrue) + command = commands2.InstantCommand(lambda: cond.set(True)) - scheduler.schedule(cmd) + scheduler.schedule(command) scheduler.run() - assert cond.getCondition() - assert not scheduler.isScheduled(cmd) + assert cond + assert not scheduler.isScheduled(command) diff --git a/tests/test_networkbutton.py b/tests/test_networkbutton.py new file mode 100644 index 00000000..b18243a1 --- /dev/null +++ b/tests/test_networkbutton.py @@ -0,0 +1,28 @@ +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() + 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/tests/test_notifier_command.py b/tests/test_notifier_command.py new file mode 100644 index 00000000..3022c20c --- /dev/null +++ b/tests/test_notifier_command.py @@ -0,0 +1,23 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +@pytest.mark.skip(reason="NotifierCommand is broken") +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/tests/test_parallelcommandgroup.py b/tests/test_parallelcommandgroup.py new file mode 100644 index 00000000..04355af7 --- /dev/null +++ b/tests/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/tests/test_paralleldeadlinegroup.py b/tests/test_paralleldeadlinegroup.py new file mode 100644 index 00000000..9136509b --- /dev/null +++ b/tests/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/tests/test_parallelracegroup.py b/tests/test_parallelracegroup.py new file mode 100644 index 00000000..58e2ace4 --- /dev/null +++ b/tests/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/tests/test_perpetualcommand.py b/tests/test_perpetualcommand.py new file mode 100644 index 00000000..00100e05 --- /dev/null +++ b/tests/test_perpetualcommand.py @@ -0,0 +1,18 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +# def test_perpetualCommandSchedule(scheduler: commands2.CommandScheduler): +# command = commands2.PerpetualCommand(commands2.InstantCommand()) + +# scheduler.schedule(command) +# scheduler.run() + +# assert scheduler.isScheduled(command) diff --git a/tests/test_printcommand.py b/tests/test_printcommand.py new file mode 100644 index 00000000..6d28cad6 --- /dev/null +++ b/tests/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/tests/test_proxycommand.py b/tests/test_proxycommand.py new file mode 100644 index 00000000..5f1add10 --- /dev/null +++ b/tests/test_proxycommand.py @@ -0,0 +1,40 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") +def test_proxyCommandSchedule(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + start_spying_on(command1) + + scheduleCommand = commands2.ProxyCommand(command1) + + scheduler.schedule(scheduleCommand) + + verify(command1).schedule() + + +@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") +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/tests/test_repeatcommand.py b/tests/test_repeatcommand.py new file mode 100644 index 00000000..e2ff3be9 --- /dev/null +++ b/tests/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/tests/test_robotdisabledcommand.py b/tests/test_robotdisabledcommand.py new file mode 100644 index 00000000..d34915f3 --- /dev/null +++ b/tests/test_robotdisabledcommand.py @@ -0,0 +1,157 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +if IS_OLD_COMMANDS: + import commands2.cmd + +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/tests/test_run_command.py b/tests/test_run_command.py deleted file mode 100644 index 47ee92c7..00000000 --- a/tests/test_run_command.py +++ /dev/null @@ -1,14 +0,0 @@ -import commands2 -from util import Counter - - -def test_run_command(scheduler: commands2.CommandScheduler): - counter = Counter() - cmd = commands2.RunCommand(counter.increment) - - scheduler.schedule(cmd) - scheduler.run() - scheduler.run() - scheduler.run() - - assert counter.value == 3 diff --git a/tests/test_runcommand.py b/tests/test_runcommand.py new file mode 100644 index 00000000..6bd4fcff --- /dev/null +++ b/tests/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/tests/test_schedulecommand.py b/tests/test_schedulecommand.py new file mode 100644 index 00000000..401f4e0d --- /dev/null +++ b/tests/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/tests/test_scheduler.py b/tests/test_scheduler.py new file mode 100644 index 00000000..09d8eb5d --- /dev/null +++ b/tests/test_scheduler.py @@ -0,0 +1,140 @@ +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_schedulerInterruptNoCauseLambda(scheduler: commands2.CommandScheduler): + counter = OOInteger() + + @scheduler.onCommandInterrupt + def _(interrupted, cause): + assert cause is None + counter.incrementAndGet() + + command = commands2.cmd.run(lambda: None) + + scheduler.schedule(command) + scheduler.cancel(command) + + assert counter == 1 + +def schedulerInterruptCauseLambda(scheduler: commands2.CommandScheduler): + counter = OOInteger() + + subsystem = commands2.Subsystem() + command = commands2.cmd.run(lambda: None) + interruptor = subsystem.runOnce(lambda: None) + + @scheduler.onCommandInterrupt + def _(interrupted, cause): + assert cause is not None + assert cause == interruptor + counter.incrementAndGet() + + scheduler.schedule(command) + scheduler.schedule(interruptor) + + assert counter == 1 + +def test_schedulerInterruptCauseLambdaInRunLoop(scheduler: commands2.CommandScheduler): + counter = OOInteger() + + subsystem = commands2.Subsystem() + command = commands2.cmd.run(lambda: None) + interruptor = subsystem.runOnce(lambda: None) + + interruptorScheduler = commands2.cmd.runOnce(lambda: scheduler.schedule(interruptor)) + + @scheduler.onCommandInterrupt + def _(interrupted, cause): + assert cause is not None + assert cause == interruptor + counter.incrementAndGet() + + scheduler.schedule(command) + scheduler.schedule(interruptorScheduler) + + scheduler.run() + + assert counter == 1 + +def test_registerSubsystem(scheduler: commands2.CommandScheduler): + counter = OOInteger() + subsystem = commands2.Subsystem() + subsystem.periodic = lambda: [None, counter.incrementAndGet()][0] + + scheduler.registerSubsystem(subsystem) + + scheduler.run() + assert counter == 1 + +def test_unregisterSubsystem(scheduler: commands2.CommandScheduler): + counter = OOInteger() + subsystem = commands2.Subsystem() + subsystem.periodic = lambda: [None, counter.incrementAndGet()][0] + + scheduler.registerSubsystem(subsystem) + scheduler.unregisterSubsystem(subsystem) + + scheduler.run() + assert counter == 0 + + +def test_schedulerCancelAll(scheduler: commands2.CommandScheduler): + counter = OOInteger() + + scheduler.onCommandInterrupt(lambda _: counter.incrementAndGet()) + @scheduler.onCommandInterrupt + def _(command, interruptor): + assert interruptor is None + + 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/tests/test_schedulingrecursion.py b/tests/test_schedulingrecursion.py new file mode 100644 index 00000000..957d02d0 --- /dev/null +++ b/tests/test_schedulingrecursion.py @@ -0,0 +1,344 @@ +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() + start_spying_on(selfCancels) + 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) + assert selfCancels.end.times_called == 1 + scheduler.run() + assert hasOtherRun == True + + +@pytest.mark.parametrize( + "interruptionBehavior", + [ + commands2.InterruptionBehavior.kCancelIncoming, + commands2.InterruptionBehavior.kCancelSelf, + ], +) +def test_cancelFromInitializeAction( + interruptionBehavior: commands2.InterruptionBehavior, + scheduler: commands2.CommandScheduler, +): + hasOtherRun = OOBoolean() + requirement = commands2.Subsystem() + + selfCancels = commands2.Command() + start_spying_on(selfCancels) + selfCancels.addRequirements(requirement) + selfCancels.getInterruptionBehavior = lambda: interruptionBehavior + + other = commands2.RunCommand(lambda: hasOtherRun.set(True), requirement) + + scheduler.onCommandInitialize(lambda _: scheduler.cancel(selfCancels)) + scheduler.schedule(selfCancels) + scheduler.run() + scheduler.schedule(other) + + assert not scheduler.isScheduled(selfCancels) + assert scheduler.isScheduled(other) + assert selfCancels.end.times_called == 1 + 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() + start_spying_on(selfCancels) + 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) + assert selfCancels.end.times_called == 1 + scheduler.run() + assert hasOtherRun == True + + +def test_cancelFromEnd(scheduler: commands2.CommandScheduler): + selfCancels = commands2.Command() + start_spying_on(selfCancels) + + selfCancels.end = lambda interrupted: scheduler.cancel(selfCancels) + + scheduler.schedule(selfCancels) + + scheduler.cancel(selfCancels) + assert selfCancels.end.times_called == 1 + assert not scheduler.isScheduled(selfCancels) + + +def test_cancelFromInterruptAction(scheduler: commands2.CommandScheduler): + selfCancels = commands2.RunCommand(lambda: None) + counter = OOInteger() + + @scheduler.onCommandInterrupt + def _(command): + counter.incrementAndGet() + scheduler.cancel(selfCancels) + + scheduler.schedule(selfCancels) + + scheduler.cancel(selfCancels) + assert counter == 1 + assert not scheduler.isScheduled(selfCancels) + +def test_cancelFromEndLoop(scheduler: commands2.CommandScheduler): + dCancelsAll = commands2.Command() + dCancelsAll.end = lambda interrupted: scheduler.cancelAll() + dCancelsAll.isFinished = lambda: True + + cCancelsD = commands2.Command() + cCancelsD.end = lambda interrupted: scheduler.cancel(dCancelsAll) + cCancelsD.isFinished = lambda: True + + bCancelsC = commands2.Command() + bCancelsC.end = lambda interrupted: scheduler.cancel(cCancelsD) + bCancelsC.isFinished = lambda: True + + aCancelsB = commands2.Command() + aCancelsB.end = lambda interrupted: scheduler.cancel(bCancelsC) + aCancelsB.isFinished = lambda: True + + start_spying_on(aCancelsB) + start_spying_on(bCancelsC) + start_spying_on(cCancelsD) + start_spying_on(dCancelsAll) + + scheduler.schedule(aCancelsB) + scheduler.schedule(bCancelsC) + scheduler.schedule(cCancelsD) + scheduler.schedule(dCancelsAll) + + scheduler.cancel(aCancelsB) + assert aCancelsB.end.times_called == 1 + assert bCancelsC.end.times_called == 1 + assert cCancelsD.end.times_called == 1 + assert dCancelsAll.end.times_called == 1 + assert not scheduler.isScheduled(aCancelsB) + assert not scheduler.isScheduled(bCancelsC) + assert not scheduler.isScheduled(cCancelsD) + assert not scheduler.isScheduled(dCancelsAll) + +def test_cancelFromEndLoopWhileInRunLoop(scheduler: commands2.CommandScheduler): + dCancelsAll = commands2.Command() + dCancelsAll.end = lambda interrupted: scheduler.cancelAll() + dCancelsAll.isFinished = lambda: True + + cCancelsD = commands2.Command() + cCancelsD.end = lambda interrupted: scheduler.cancel(dCancelsAll) + cCancelsD.isFinished = lambda: True + + bCancelsC = commands2.Command() + bCancelsC.end = lambda interrupted: scheduler.cancel(cCancelsD) + bCancelsC.isFinished = lambda: True + + aCancelsB = commands2.Command() + aCancelsB.end = lambda interrupted: scheduler.cancel(bCancelsC) + aCancelsB.isFinished = lambda: True + + start_spying_on(aCancelsB) + start_spying_on(bCancelsC) + start_spying_on(cCancelsD) + start_spying_on(dCancelsAll) + + scheduler.schedule(aCancelsB) + scheduler.schedule(bCancelsC) + scheduler.schedule(cCancelsD) + scheduler.schedule(dCancelsAll) + + scheduler.run() + assert aCancelsB.end.times_called == 1 + assert bCancelsC.end.times_called == 1 + assert cCancelsD.end.times_called == 1 + assert dCancelsAll.end.times_called == 1 + assert not scheduler.isScheduled(aCancelsB) + assert not scheduler.isScheduled(bCancelsC) + assert not scheduler.isScheduled(cCancelsD) + assert not scheduler.isScheduled(dCancelsAll) + +def test_multiCancelFromEnd(scheduler: commands2.CommandScheduler): + b = commands2.Command() + b.isFinished = lambda: True + start_spying_on(b) + + aCancelB = commands2.Command() + @patch_via_decorator(aCancelB) + def end(self, interrupted): + scheduler.cancel(b) + scheduler.cancel(self) + + scheduler.schedule(aCancelB) + scheduler.schedule(b) + + scheduler.cancel(aCancelB) + assert aCancelB.end.times_called == 2 + assert not scheduler.isScheduled(aCancelB) + assert not scheduler.isScheduled(b) + +def test_scheduleFromEndCancel(scheduler: commands2.CommandScheduler): + requirement = commands2.Subsystem() + other = commands2.InstantCommand(lambda: None, requirement) + selfCancels = commands2.Command() + start_spying_on(selfCancels) + selfCancels.addRequirements(requirement) + selfCancels.end = lambda interrupted: scheduler.schedule(other) + selfCancels.isFinished = lambda: False + + scheduler.schedule(selfCancels) + scheduler.cancel(selfCancels) + assert selfCancels.end.times_called == 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): + scheduler.schedule(other) + + scheduler.schedule(selfCancels) + + scheduler.schedule(other) + assert counter == 1 + assert not scheduler.isScheduled(selfCancels) + assert scheduler.isScheduled(other) + +def test_scheduleFromEndInterruptAction(scheduler: commands2.CommandScheduler): + counter = OOInteger() + requirement = commands2.Subsystem() + other = commands2.InstantCommand(lambda: None, requirement) + selfCancels = commands2.InstantCommand(lambda: None, requirement) + + @scheduler.onCommandInterrupt + def _(command): + 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) + +def test_cancelDefaultCommandFromEnd( + scheduler: commands2.CommandScheduler, +): + counter = OOInteger() + requirement = commands2.Subsystem() + defaultCommand = commands2.Command() + defaultCommand.addRequirements(requirement) + defaultCommand.end = lambda interrupted: counter.incrementAndGet() # type: ignore + + other = commands2.InstantCommand(lambda: None, requirement) + + cancelDefaultCommand = commands2.Command() + + @patch_via_decorator(cancelDefaultCommand) + def end(self, interrupted): + counter.incrementAndGet() + scheduler.schedule(other) + + scheduler.schedule(cancelDefaultCommand) + scheduler.setDefaultCommand(requirement, defaultCommand) + + scheduler.run() + scheduler.cancel(cancelDefaultCommand) + + assert counter == 2 + assert not scheduler.isScheduled(defaultCommand) + assert scheduler.isScheduled(other) diff --git a/tests/test_select_command.py b/tests/test_select_command.py deleted file mode 100644 index 91fc5d96..00000000 --- a/tests/test_select_command.py +++ /dev/null @@ -1,69 +0,0 @@ -import enum -import commands2 -from util import ConditionHolder - - -def test_select_command_int(scheduler: commands2.CommandScheduler): - c = ConditionHolder() - - def _assert_false(): - assert False - - cmd1 = commands2.RunCommand(_assert_false) - cmd2 = commands2.RunCommand(c.setTrue) - cmd3 = commands2.RunCommand(_assert_false) - - sc = commands2.SelectCommand(lambda: 2, [(1, cmd1), (2, cmd2), (3, cmd3)]) - - scheduler.schedule(sc) - scheduler.run() - - assert c.cond - - -def test_select_command_str(scheduler: commands2.CommandScheduler): - c = ConditionHolder() - - def _assert_false(): - assert False - - cmd1 = commands2.RunCommand(_assert_false) - cmd2 = commands2.RunCommand(c.setTrue) - cmd3 = commands2.RunCommand(_assert_false) - - sc = commands2.SelectCommand(lambda: "2", [("1", cmd1), ("2", cmd2), ("3", cmd3)]) - - scheduler.schedule(sc) - scheduler.run() - - assert c.cond - - -def test_select_command_enum(scheduler: commands2.CommandScheduler): - c = ConditionHolder() - - def _assert_false(): - assert False - - class Selector(enum.Enum): - ONE = enum.auto() - TWO = enum.auto() - THREE = enum.auto() - - cmd1 = commands2.RunCommand(_assert_false) - cmd2 = commands2.RunCommand(c.setTrue) - cmd3 = commands2.RunCommand(_assert_false) - - sc = commands2.SelectCommand( - lambda: Selector.TWO, - [ - (Selector.ONE, cmd1), - (Selector.TWO, cmd2), - (Selector.THREE, cmd3), - ], - ) - - scheduler.schedule(sc) - scheduler.run() - - assert c.cond diff --git a/tests/test_selectcommand.py b/tests/test_selectcommand.py new file mode 100644 index 00000000..3ceb2a72 --- /dev/null +++ b/tests/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/tests/test_sequentialcommandgroup.py b/tests/test_sequentialcommandgroup.py new file mode 100644 index 00000000..a6ecfb96 --- /dev/null +++ b/tests/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/tests/test_shutdown.py b/tests/test_shutdown.py deleted file mode 100644 index 92545d99..00000000 --- a/tests/test_shutdown.py +++ /dev/null @@ -1,17 +0,0 @@ -import commands2 -import hal -from util import Counter - - -def test_run_command(scheduler: commands2.CommandScheduler): - counter = Counter() - cmd = commands2.RunCommand(counter.increment) - - scheduler.schedule(cmd) - scheduler.run() - assert counter.value == 1 - - hal.shutdown() - - scheduler.run() - assert counter.value == 1 diff --git a/tests/test_start_end_command.py b/tests/test_start_end_command.py deleted file mode 100644 index ca8f6eb2..00000000 --- a/tests/test_start_end_command.py +++ /dev/null @@ -1,27 +0,0 @@ -from commands2 import StartEndCommand -from util import CommandTestHelper - - -class StartEnd: - def __init__(self) -> None: - self.counter = 0 - - def start(self): - self.counter += 1 - - def end(self): - self.counter += 1 - - -def test_start_end(): - with CommandTestHelper() as helper: - se = StartEnd() - - cmd = StartEndCommand(se.start, se.end, []) - - helper.scheduler.schedule(cmd) - helper.scheduler.run() - helper.scheduler.run() - helper.scheduler.cancel(cmd) - - assert se.counter == 2 diff --git a/tests/test_startendcommand.py b/tests/test_startendcommand.py new file mode 100644 index 00000000..700412f7 --- /dev/null +++ b/tests/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/tests/test_trigger.py b/tests/test_trigger.py new file mode 100644 index 00000000..876ae9da --- /dev/null +++ b/tests/test_trigger.py @@ -0,0 +1,219 @@ +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_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/tests/test_wait_until_command.py b/tests/test_wait_until_command.py deleted file mode 100644 index c029cbab..00000000 --- a/tests/test_wait_until_command.py +++ /dev/null @@ -1,17 +0,0 @@ -import commands2 -from util import ConditionHolder - - -def test_wait_until(scheduler: commands2.CommandScheduler): - cond = ConditionHolder() - - cmd = commands2.WaitUntilCommand(cond.getCondition) - - scheduler.schedule(cmd) - scheduler.run() - - assert scheduler.isScheduled(cmd) - cond.setTrue() - - scheduler.run() - assert not scheduler.isScheduled(cmd) diff --git a/tests/test_waitcommand.py b/tests/test_waitcommand.py new file mode 100644 index 00000000..125a2e31 --- /dev/null +++ b/tests/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/tests/test_waituntilcommand.py b/tests/test_waituntilcommand.py new file mode 100644 index 00000000..2762a1f6 --- /dev/null +++ b/tests/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/tests/util.py b/tests/util.py index 896ff9e8..069d8319 100644 --- a/tests/util.py +++ b/tests/util.py @@ -1,8 +1,68 @@ +from typing import Any, Dict, TypeVar, Type, Union + +import inspect + import commands2 from wpilib.simulation import DriverStationSim, pauseTiming, resumeTiming, stepTiming -# from unittest.mock import MagicMock +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] + + +################### +# Compat for wrapped commands +IS_OLD_COMMANDS = False +try: + if commands2.__version__ == "2023.4.3.0": # type: ignore + IS_OLD_COMMANDS = True +except AttributeError: + pass + +if IS_OLD_COMMANDS: + + # not needed for pure but works in pure + import commands2.button + + # In Java, Trigger is in the same package as Button + # I did rexport it in commands so using + # the incorrect `commands2.Trigger` instead of `commands2.button.Trigger` works + commands2.button.Trigger = commands2.Trigger + + # I moved this so its not a nested Enum. + # The old one is still there for compat + commands2.InterruptionBehavior = commands2.Command.InterruptionBehavior + + commands2.Command = commands2.CommandBase + + for name in dir(commands2): + if name == "CommandScheduler": + continue + value = getattr(commands2, name) + if inspect.isclass(value): + setattr(commands2, name, full_subclass_of(value)) + + commands2.IllegalCommandUse = RuntimeError + + +################### class ManualSimTime: def __enter__(self) -> "ManualSimTime": @@ -16,37 +76,239 @@ def step(self, delta: float): stepTiming(delta) -class CommandTestHelper: - def __init__(self) -> None: - self.scheduler = commands2.CommandScheduler.getInstance() +class OOInteger: + def __init__(self, value: int = 0) -> None: + self.value = value - def __enter__(self): - commands2.CommandScheduler.resetInstance() - DriverStationSim.setEnabled(True) - return self + def get(self) -> int: + return self.value - def __exit__(self, *args): - pass + def set(self, value: int): + self.value = value + def incrementAndGet(self) -> int: + self.value += 1 + return self.value -class Counter: - def __init__(self) -> None: - self.value = 0 + def addAndGet(self, value: int) -> int: + self.value += value + return self.value - def increment(self): - self.value += 1 + def __eq__(self, value: float) -> bool: + return self.value == value + def __lt__(self, value: float) -> bool: + return self.value < value -class ConditionHolder: - def __init__(self, cond: bool = False) -> None: - self.cond = cond + def __call__(self) -> int: + return self.value - def getCondition(self) -> bool: - return self.cond - def setTrue(self): - self.cond = True +class OOBoolean: + def __init__(self, value: bool = False) -> None: + self.value = value + def get(self) -> bool: + return self.value -class TestSubsystem(commands2.SubsystemBase): - pass + 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 + + +########################################## +# 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 only: ... + +# class atLeast: +# def __init__(self, times: int) -> None: +# self.times = times + +# def atLeastOnce() -> atLeast: +# return atLeast(1) + +# class atMost: +# def __init__(self, times: int) -> None: +# self.times = times + +# def atMostOnce() -> atMost: +# return atMost(1) + +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