Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[robotpy] Mirror robotpy's commands-v2 (#8369)

Browse Source 
Project import generated by Copybara.

GitOrigin-RevId: 715c8e8372d936f447f2937aab6b1a22dc619126
This commit is contained in:
PJ Reiniger
2025-11-14 00:55:54 -05:00
committed by GitHub
parent 6e6f8dd7cc
commit 1a99a348cb
84 changed files with 8974 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

88
commandsv2/src/main/python/commands2/__init__.py Normal file
View File

@@ -0,0 +1,88 @@
from .command import Command, InterruptionBehavior
from . import button
from . import cmd
from . import typing
from .commandscheduler import CommandScheduler
from .conditionalcommand import ConditionalCommand
from .deferredcommand import DeferredCommand
from .exceptions import IllegalCommandUse
from .functionalcommand import FunctionalCommand
from .instantcommand import InstantCommand
from .notifiercommand import NotifierCommand
from .parallelcommandgroup import ParallelCommandGroup
from .paralleldeadlinegroup import ParallelDeadlineGroup
from .parallelracegroup import ParallelRaceGroup
from .pidcommand import PIDCommand
from .pidsubsystem import PIDSubsystem
from .printcommand import PrintCommand
from .profiledpidcommand import ProfiledPIDCommand
from .profiledpidsubsystem import ProfiledPIDSubsystem
from .proxycommand import ProxyCommand
from .repeatcommand import RepeatCommand
from .runcommand import RunCommand
from .schedulecommand import ScheduleCommand
from .selectcommand import SelectCommand
from .sequentialcommandgroup import SequentialCommandGroup
from .startendcommand import StartEndCommand
from .subsystem import Subsystem
from .swervecontrollercommand import SwerveControllerCommand
from .timedcommandrobot import TimedCommandRobot
from .trapezoidprofilecommand import TrapezoidProfileCommand
from .trapezoidprofilesubsystem import TrapezoidProfileSubsystem
from .waitcommand import WaitCommand
from .waituntilcommand import WaitUntilCommand
from .wrappercommand import WrapperCommand
from typing import TYPE_CHECKING
__all__ = [
"button",
"cmd",
"Command",
"CommandScheduler",
"ConditionalCommand",
"DeferredCommand",
"FunctionalCommand",
"IllegalCommandUse",
"InstantCommand",
"InterruptionBehavior",
"NotifierCommand",
"ParallelCommandGroup",
"ParallelDeadlineGroup",
"ParallelRaceGroup",
"PIDCommand",
"PIDSubsystem",
"PrintCommand",
"ProfiledPIDCommand",
"ProfiledPIDSubsystem",
"ProxyCommand",
"RepeatCommand",
"RunCommand",
"ScheduleCommand",
"SelectCommand",
"SequentialCommandGroup",
"StartEndCommand",
"Subsystem",
"SwerveControllerCommand",
"TimedCommandRobot",
"TrapezoidProfileCommand",
"TrapezoidProfileSubsystem",
"WaitCommand",
"WaitUntilCommand",
"WrapperCommand",
]
if not TYPE_CHECKING:
def __getattr__(attr):
if attr == "SubsystemBase":
import warnings
warnings.warn(
"SubsystemBase is deprecated", DeprecationWarning, stacklevel=2
)
return Subsystem
raise AttributeError(f"module {__name__!r} has no attribute {attr!r}")

19
commandsv2/src/main/python/commands2/button/__init__.py Normal file
View File

@@ -0,0 +1,19 @@
from .commandgenerichid import CommandGenericHID
from .commandjoystick import CommandJoystick
from .commandps4controller import CommandPS4Controller
from .commandxboxcontroller import CommandXboxController
from .joystickbutton import JoystickButton
from .networkbutton import NetworkButton
from .povbutton import POVButton
from .trigger import Trigger
__all__ = [
"Trigger",
"CommandGenericHID",
"CommandJoystick",
"CommandPS4Controller",
"CommandXboxController",
"JoystickButton",
"NetworkButton",
"POVButton",
]

224
commandsv2/src/main/python/commands2/button/commandgenerichid.py Normal file
View File

@@ -0,0 +1,224 @@
# validated: 2024-01-20 DS 92149efa11fa button/CommandGenericHID.java
from typing import Optional
from wpilib.event import EventLoop
from wpilib.interfaces import GenericHID
from ..commandscheduler import CommandScheduler
from .trigger import Trigger
class CommandGenericHID:
"""
A version of :class:`wpilib.interfaces.GenericHID` with :class:`.Trigger` factories for command-based.
"""
def __init__(self, port: int):
"""
Construct an instance of a device.
:param port: The port on the Driver Station that the device is plugged into.
"""
self._hid = GenericHID(port)
def getHID(self) -> GenericHID:
"""
Get the underlying GenericHID object.
"""
return self._hid
def button(self, button: int, loop: Optional[EventLoop] = None) -> Trigger:
"""
Constructs an event instance around this button's digital signal.
:param button: The button index
:param loop: the event loop instance to attach the event to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: A trigger instance attached to the event loop
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getRawButton(button))
def pov(
self, angle: int, *, pov: int = 0, loop: Optional[EventLoop] = None
) -> Trigger:
"""
Constructs a Trigger instance based around this angle of a POV on the HID.
The POV angles start at 0 in the up direction, and increase clockwise (e.g. right is 90,
upper-left is 315).
:param angle: POV angle in degrees, or -1 for the center / not pressed.
:param pov: index of the POV to read (starting at 0). Defaults to 0.
:param loop: the event loop instance to attach the event to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: a Trigger instance based around this angle of a POV on the HID.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getPOV(pov) == angle)
def povUp(self) -> Trigger:
"""
Constructs a Trigger instance based around the 0 degree angle (up) of the default (index 0) POV
on the HID, attached to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: a Trigger instance based around the 0 degree angle of a POV on the HID.
"""
return self.pov(0)
def povUpRight(self) -> Trigger:
"""
Constructs a Trigger instance based around the 45 degree angle (right up) of the default (index
0) POV on the HID, attached to :func:`commands2.CommandScheduler.getDefaultButtonLoop`.
:returns: a Trigger instance based around the 45 degree angle of a POV on the HID.
"""
return self.pov(45)
def povRight(self) -> Trigger:
"""
Constructs a Trigger instance based around the 90 degree angle (right) of the default (index 0)
POV on the HID, attached to :func:`commands2.CommandScheduler.getDefaultButtonLoop`.
:returns: a Trigger instance based around the 90 degree angle of a POV on the HID.
"""
return self.pov(90)
def povDownRight(self) -> Trigger:
"""
Constructs a Trigger instance based around the 135 degree angle (right down) of the default
(index 0) POV on the HID, attached to :func:`commands2.CommandScheduler.getDefaultButtonLoop`.
:returns: a Trigger instance based around the 135 degree angle of a POV on the HID.
"""
return self.pov(135)
def povDown(self) -> Trigger:
"""
Constructs a Trigger instance based around the 180 degree angle (down) of the default (index 0)
POV on the HID, attached to :func:`commands2.CommandScheduler.getDefaultButtonLoop`.
:returns: a Trigger instance based around the 180 degree angle of a POV on the HID.
"""
return self.pov(180)
def povDownLeft(self) -> Trigger:
"""
Constructs a Trigger instance based around the 225 degree angle (down left) of the default
(index 0) POV on the HID, attached to :func:`commands2.CommandScheduler.getDefaultButtonLoop`.
:returns: a Trigger instance based around the 225 degree angle of a POV on the HID.
"""
return self.pov(225)
def povLeft(self) -> Trigger:
"""
Constructs a Trigger instance based around the 270 degree angle (left) of the default (index 0)
POV on the HID, attached to :func:`commands2.CommandScheduler.getDefaultButtonLoop`.
:returns: a Trigger instance based around the 270 degree angle of a POV on the HID.
"""
return self.pov(270)
def povUpLeft(self) -> Trigger:
"""
Constructs a Trigger instance based around the 315 degree angle (left up) of the default (index
0) POV on the HID, attached to :func:`commands2.CommandScheduler.getDefaultButtonLoop`.
:returns: a Trigger instance based around the 315 degree angle of a POV on the HID.
"""
return self.pov(315)
def povCenter(self) -> Trigger:
"""
Constructs a Trigger instance based around the center (not pressed) position of the default
(index 0) POV on the HID, attached to :func:`commands2.CommandScheduler.getDefaultButtonLoop`.
:returns: a Trigger instance based around the center position of a POV on the HID.
"""
return self.pov(-1)
def axisLessThan(
self, axis: int, threshold: float, loop: Optional[EventLoop] = None
) -> Trigger:
"""
Constructs a Trigger instance that is true when the axis value is less than ``threshold``,
attached to the given loop.
:param axis: The axis to read, starting at 0
:param threshold: The value below which this trigger should return true.
:param loop: the event loop instance to attach the trigger to
:returns: a Trigger instance that is true when the axis value is less than the provided
threshold.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getRawAxis(axis) < threshold)
def axisGreaterThan(
self, axis: int, threshold: float, loop: Optional[EventLoop] = None
) -> Trigger:
"""
Constructs a Trigger instance that is true when the axis value is greater than
``threshold``, attached to the given loop.
:param axis: The axis to read, starting at 0
:param threshold: The value above which this trigger should return true.
:param loop: the event loop instance to attach the trigger to.
:returns: a Trigger instance that is true when the axis value is greater than the provided
threshold.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getRawAxis(axis) > threshold)
def axisMagnitudeGreaterThan(
self, axis: int, threshold: float, loop: Optional[EventLoop] = None
) -> Trigger:
"""
Constructs a Trigger instance that is true when the axis magnitude is greater than
``threshold``, attached to the given loop.
:param axis: The axis to read, starting at 0
:param threshold: The value above which this trigger should return true.
:param loop: the event loop instance to attach the trigger to.
:returns: a Trigger instance that is true when the axis magnitude is greater than the provided
threshold.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: abs(self._hid.getRawAxis(axis)) > threshold)
def getRawAxis(self, axis: int) -> float:
"""
Get the value of the axis.
:param axis: The axis to read, starting at 0.
:returns: The value of the axis.
"""
return self._hid.getRawAxis(axis)
def setRumble(self, type: GenericHID.RumbleType, value: float):
"""
Set the rumble output for the HID.
The DS currently supports 2 rumble values, left rumble and right rumble.
:param type: Which rumble value to set.
:param value: The normalized value (0 to 1) to set the rumble to.
"""
self._hid.setRumble(type, value)
def isConnected(self):
"""
Get if the HID is connected.
:returns: True if the HID is connected.
"""
return self._hid.isConnected()

226
commandsv2/src/main/python/commands2/button/commandjoystick.py Normal file
View File

@@ -0,0 +1,226 @@
# validated: 2024-01-20 DS 92aecab2ef05 button/CommandJoystick.java
from typing import Optional
from wpilib import Joystick
from wpilib.event import EventLoop
from ..commandscheduler import CommandScheduler
from .commandgenerichid import CommandGenericHID
from .trigger import Trigger
class CommandJoystick(CommandGenericHID):
"""
A version of :class:`wpilib.Joystick` with :class:`.Trigger` factories for command-based.
"""
_hid: Joystick
def __init__(self, port: int):
"""
Construct an instance of a controller.
:param port: The port index on the Driver Station that the controller is plugged into.
"""
super().__init__(port)
self._hid = Joystick(port)
def getHID(self) -> Joystick:
"""
Get the underlying GenericHID object.
:returns: the wrapped GenericHID object
"""
return self._hid
def trigger(self, loop: Optional[EventLoop] = None) -> Trigger:
"""
Constructs an event instance around the trigger button's digital signal.
:param loop: the event loop instance to attach the event to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: an event instance representing the trigger button's digital signal attached to the
given loop.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getTrigger())
def top(self, loop: Optional[EventLoop] = None) -> Trigger:
"""
Constructs an event instance around the top button's digital signal.
:param loop: the event loop instance to attach the event to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: an event instance representing the top button's digital signal attached to the given
loop.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getTop())
def setXChannel(self, channel: int):
"""
Set the channel associated with the X axis.
:param channel: The channel to set the axis to.
"""
self._hid.setXChannel(channel)
def setYChannel(self, channel: int):
"""
Set the channel associated with the Y axis.
:param channel: The channel to set the axis to.
"""
self._hid.setYChannel(channel)
def setZChannel(self, channel: int):
"""
Set the channel associated with the Z axis.
:param channel: The channel to set the axis to.
"""
self._hid.setZChannel(channel)
def setThrottleChannel(self, channel: int):
"""
Set the channel associated with the throttle axis.
:param channel: The channel to set the axis to.
"""
self._hid.setThrottleChannel(channel)
def setTwistChannel(self, channel: int):
"""
Set the channel associated with the twist axis.
:param channel: The channel to set the axis to.
"""
self._hid.setTwistChannel(channel)
def getXChannel(self) -> int:
"""
Get the channel currently associated with the X axis.
:returns: The channel for the axis.
"""
return self._hid.getXChannel()
def getYChannel(self) -> int:
"""
Get the channel currently associated with the Y axis.
:returns: The channel for the axis.
"""
return self._hid.getYChannel()
def getZChannel(self) -> int:
"""
Get the channel currently associated with the Z axis.
:returns: The channel for the axis.
"""
return self._hid.getZChannel()
def getTwistChannel(self) -> int:
"""
Get the channel currently associated with the twist axis.
:returns: The channel for the axis.
"""
return self._hid.getTwistChannel()
def getThrottleChannel(self) -> int:
"""
Get the channel currently associated with the throttle axis.
:returns: The channel for the axis.
"""
return self._hid.getThrottleChannel()
def getX(self) -> float:
"""
Get the x position of the HID.
This depends on the mapping of the joystick connected to the current port. On most
joysticks, positive is to the right.
:returns: the x position
"""
return self._hid.getX()
def getY(self) -> float:
"""
Get the y position of the HID.
This depends on the mapping of the joystick connected to the current port. On most
joysticks, positive is to the back.
:returns: the y position
"""
return self._hid.getY()
def getZ(self) -> float:
"""
Get the z position of the HID.
:returns: the z position
"""
return self._hid.getZ()
def getTwist(self) -> float:
"""
Get the twist value of the current joystick. This depends on the mapping of the joystick
connected to the current port.
:returns: The Twist value of the joystick.
"""
return self._hid.getTwist()
def getThrottle(self) -> float:
"""
Get the throttle value of the current joystick. This depends on the mapping of the joystick
connected to the current port.
:returns: The Throttle value of the joystick.
"""
return self._hid.getThrottle()
def getMagnitude(self) -> float:
"""
Get the magnitude of the vector formed by the joystick's current position relative to its
origin.
:returns: The magnitude of the direction vector
"""
return self._hid.getMagnitude()
def getDirectionRadians(self) -> float:
"""
Get the direction of the vector formed by the joystick and its origin in radians. 0 is forward
and clockwise is positive. (Straight right is π/2.)
:returns: The direction of the vector in radians
"""
# https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html#joystick-and-controller-coordinate-system
# A positive rotation around the X axis moves the joystick right, and a
# positive rotation around the Y axis moves the joystick backward. When
# treating them as translations, 0 radians is measured from the right
# direction, and angle increases clockwise.
#
# It's rotated 90 degrees CCW (y is negated and the arguments are reversed)
# so that 0 radians is forward.
return self._hid.getDirectionRadians()
def getDirectionDegrees(self) -> float:
"""
Get the direction of the vector formed by the joystick and its origin in degrees. 0 is forward
and clockwise is positive. (Straight right is 90.)
:returns: The direction of the vector in degrees
"""
return self._hid.getDirectionDegrees()

280
commandsv2/src/main/python/commands2/button/commandps4controller.py Normal file
View File

@@ -0,0 +1,280 @@
# validated: 2024-01-20 DS d426873ed15b button/CommandPS4Controller.java
from typing import Optional
from wpilib import PS4Controller
from wpilib.event import EventLoop
from ..commandscheduler import CommandScheduler
from .commandgenerichid import CommandGenericHID
from .trigger import Trigger
class CommandPS4Controller(CommandGenericHID):
"""
A version of PS4Controller with Trigger factories for command-based.
"""
_hid: PS4Controller
def __init__(self, port: int):
"""
Construct an instance of a device.
:param port: The port index on the Driver Station that the device is plugged into.
"""
super().__init__(port)
self._hid = PS4Controller(port)
def getHID(self) -> PS4Controller:
"""
Get the underlying GenericHID object.
:returns: the wrapped GenericHID object
"""
return self._hid
def L2(self, loop: Optional[EventLoop] = None) -> Trigger:
"""
Constructs an event instance around the L2 button's digital signal.
:param loop: the event loop instance to attach the event to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: an event instance representing the L2 button's digital signal attached to the given
loop.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getL2Button())
def R2(self, loop: Optional[EventLoop] = None) -> Trigger:
"""
Constructs an event instance around the R2 button's digital signal.
:param loop: the event loop instance to attach the event to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: an event instance representing the R2 button's digital signal attached to the given
loop.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getR2Button())
def L1(self, loop: Optional[EventLoop] = None) -> Trigger:
"""
Constructs an event instance around the L1 button's digital signal.
:param loop: the event loop instance to attach the event to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: an event instance representing the L1 button's digital signal attached to the given
loop.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getL1Button())
def R1(self, loop: Optional[EventLoop] = None) -> Trigger:
"""
Constructs an event instance around the R1 button's digital signal.
:param loop: the event loop instance to attach the event to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: an event instance representing the R1 button's digital signal attached to the given
loop.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getR1Button())
def L3(self, loop: Optional[EventLoop] = None) -> Trigger:
"""
Constructs an event instance around the L3 button's digital signal.
:param loop: the event loop instance to attach the event to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: an event instance representing the L3 button's digital signal attached to the given
loop.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getL3Button())
def R3(self, loop: Optional[EventLoop] = None) -> Trigger:
"""
Constructs an event instance around the R3 button's digital signal.
:param loop: the event loop instance to attach the event to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: an event instance representing the R3 button's digital signal attached to the given
loop.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getR3Button())
def square(self, loop: Optional[EventLoop] = None) -> Trigger:
"""
Constructs an event instance around the square button's digital signal.
:param loop: the event loop instance to attach the event to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: an event instance representing the square button's digital signal attached to the given
loop.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getSquareButton())
def cross(self, loop: Optional[EventLoop] = None) -> Trigger:
"""
Constructs an event instance around the cross button's digital signal.
:param loop: the event loop instance to attach the event to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: an event instance representing the cross button's digital signal attached to the given
loop.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getCrossButton())
def triangle(self, loop: Optional[EventLoop] = None) -> Trigger:
"""
Constructs an event instance around the triangle button's digital signal.
:param loop: the event loop instance to attach the event to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: an event instance representing the triangle button's digital signal attached to the
given loop.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getTriangleButton())
def circle(self, loop: Optional[EventLoop] = None) -> Trigger:
"""
Constructs an event instance around the circle button's digital signal.
:param loop: the event loop instance to attach the event to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: an event instance representing the circle button's digital signal attached to the given
loop.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getCircleButton())
def share(self, loop: Optional[EventLoop] = None) -> Trigger:
"""
Constructs an event instance around the share button's digital signal.
:param loop: the event loop instance to attach the event to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: an event instance representing the share button's digital signal attached to the given
loop.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getShareButton())
def PS(self, loop: Optional[EventLoop] = None) -> Trigger:
"""
Constructs an event instance around the PS button's digital signal.
:param loop: the event loop instance to attach the event to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: an event instance representing the PS button's digital signal attached to the given
loop.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getPSButton())
def options(self, loop: Optional[EventLoop] = None) -> Trigger:
"""
Constructs an event instance around the options button's digital signal.
:param loop: the event loop instance to attach the event to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: an event instance representing the options button's digital signal attached to the
given loop.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getOptionsButton())
def touchpad(self, loop: Optional[EventLoop] = None) -> Trigger:
"""
Constructs an event instance around the touchpad's digital signal.
:param loop: the event loop instance to attach the event to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: an event instance representing the touchpad's digital signal attached to the given
loop.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getTouchpad())
def getLeftX(self) -> float:
"""
Get the X axis value of left side of the controller. Right is positive.
:returns: the axis value.
"""
return self._hid.getLeftX()
def getRightX(self) -> float:
"""
Get the X axis value of right side of the controller. Right is positive.
:returns: the axis value.
"""
return self._hid.getRightX()
def getLeftY(self) -> float:
"""
Get the Y axis value of left side of the controller. Back is positive.
:returns: the axis value.
"""
return self._hid.getLeftY()
def getRightY(self) -> float:
"""
Get the Y axis value of right side of the controller. Back is positive.
:returns: the axis value.
"""
return self._hid.getRightY()
def getL2Axis(self) -> float:
"""
Get the L2 axis value of the controller. Note that this axis is bound to the range of [0, 1] as
opposed to the usual [-1, 1].
:returns: the axis value.
"""
return self._hid.getL2Axis()
def getR2Axis(self) -> float:
"""
Get the R2 axis value of the controller. Note that this axis is bound to the range of [0, 1] as
opposed to the usual [-1, 1].
:returns: the axis value.
"""
return self._hid.getR2Axis()

262
commandsv2/src/main/python/commands2/button/commandxboxcontroller.py Normal file
View File

@@ -0,0 +1,262 @@
# validated: 2024-01-20 DS 3ba501f9478a button/CommandXboxController.java
from typing import Optional
from wpilib import XboxController
from wpilib.event import EventLoop
from ..commandscheduler import CommandScheduler
from .commandgenerichid import CommandGenericHID
from .trigger import Trigger
class CommandXboxController(CommandGenericHID):
"""
A version of XboxController with Trigger factories for command-based.
"""
_hid: XboxController
def __init__(self, port: int):
"""
Construct an instance of a controller.
:param port: The port index on the Driver Station that the controller is plugged into.
"""
super().__init__(port)
self._hid = XboxController(port)
def getHID(self) -> XboxController:
"""
Get the underlying GenericHID object.
:returns: the wrapped GenericHID object
"""
return self._hid
def leftBumper(self, loop: Optional[EventLoop] = None) -> Trigger:
"""
Constructs an event instance around the left bumper's digital signal.
:param loop: the event loop instance to attach the event to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: an event instance representing the right bumper's digital signal attached to the given
loop.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getLeftBumper())
def rightBumper(self, loop: Optional[EventLoop] = None) -> Trigger:
"""
Constructs an event instance around the right bumper's digital signal.
:param loop: the event loop instance to attach the event to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: an event instance representing the left bumper's digital signal attached to the given
loop.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getRightBumper())
def leftStick(self, loop: Optional[EventLoop] = None) -> Trigger:
"""
Constructs an event instance around the left stick button's digital signal.
:param loop: the event loop instance to attach the event to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: an event instance representing the left stick button's digital signal attached to the
given loop.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getLeftStickButton())
def rightStick(self, loop: Optional[EventLoop] = None) -> Trigger:
"""
Constructs an event instance around the right stick button's digital signal.
:param loop: the event loop instance to attach the event to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: an event instance representing the right stick button's digital signal attached to the
given loop.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getRightStickButton())
def a(self, loop: Optional[EventLoop] = None) -> Trigger:
"""
Constructs an event instance around the A button's digital signal.
:param loop: the event loop instance to attach the event to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: an event instance representing the A button's digital signal attached to the given
loop.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getAButton())
def b(self, loop: Optional[EventLoop] = None) -> Trigger:
"""
Constructs an event instance around the B button's digital signal.
:param loop: the event loop instance to attach the event to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: an event instance representing the B button's digital signal attached to the given
loop.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getBButton())
def x(self, loop: Optional[EventLoop] = None) -> Trigger:
"""
Constructs an event instance around the X button's digital signal.
:param loop: the event loop instance to attach the event to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: an event instance representing the X button's digital signal attached to the given
loop.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getXButton())
def y(self, loop: Optional[EventLoop] = None) -> Trigger:
"""
Constructs an event instance around the Y button's digital signal.
:param loop: the event loop instance to attach the event to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: an event instance representing the Y button's digital signal attached to the given
loop.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getYButton())
def start(self, loop: Optional[EventLoop] = None) -> Trigger:
"""
Constructs an event instance around the start button's digital signal.
:param loop: the event loop instance to attach the event to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: an event instance representing the start button's digital signal attached to the given
loop.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getStartButton())
def back(self, loop: Optional[EventLoop] = None) -> Trigger:
"""
Constructs an event instance around the back button's digital signal.
:param loop: the event loop instance to attach the event to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: an event instance representing the back button's digital signal attached to the given
loop.
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getBackButton())
def leftTrigger(
self, threshold: float = 0.5, loop: Optional[EventLoop] = None
) -> Trigger:
"""
Constructs a Trigger instance around the axis value of the left trigger. The returned trigger
will be true when the axis value is greater than {@code threshold}.
:param threshold: the minimum axis value for the returned Trigger to be true. This value
should be in the range [0, 1] where 0 is the unpressed state of the axis.
:param loop: the event loop instance to attach the Trigger to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: a Trigger instance that is true when the left trigger's axis exceeds the provided
threshold, attached to the given event loop
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getLeftTriggerAxis() > threshold)
def rightTrigger(
self, threshold: float = 0.5, loop: Optional[EventLoop] = None
) -> Trigger:
"""
Constructs a Trigger instance around the axis value of the right trigger. The returned trigger
will be true when the axis value is greater than {@code threshold}.
:param threshold: the minimum axis value for the returned Trigger to be true. This value
should be in the range [0, 1] where 0 is the unpressed state of the axis.
:param loop: the event loop instance to attach the Trigger to, defaults
to :func:`commands2.CommandScheduler.getDefaultButtonLoop`
:returns: a Trigger instance that is true when the right trigger's axis exceeds the provided
threshold, attached to the given event loop
"""
if loop is None:
loop = CommandScheduler.getInstance().getDefaultButtonLoop()
return Trigger(loop, lambda: self._hid.getRightTriggerAxis() > threshold)
def getLeftX(self) -> float:
"""
Get the X axis value of left side of the controller. Right is positive.
:returns: The axis value.
"""
return self._hid.getLeftX()
def getRightX(self) -> float:
"""
Get the X axis value of right side of the controller. Right is positive.
:returns: The axis value.
"""
return self._hid.getRightX()
def getLeftY(self) -> float:
"""
Get the Y axis value of left side of the controller. Back is positive.
:returns: The axis value.
"""
return self._hid.getLeftY()
def getRightY(self) -> float:
"""
Get the Y axis value of right side of the controller. Back is positive.
:returns: The axis value.
"""
return self._hid.getRightY()
def getLeftTriggerAxis(self) -> float:
"""
Get the left trigger (LT) axis value of the controller. Note that this axis is bound to the
range of [0, 1] as opposed to the usual [-1, 1].
:returns: The axis value.
"""
return self._hid.getLeftTriggerAxis()
def getRightTriggerAxis(self) -> float:
"""
Get the right trigger (RT) axis value of the controller. Note that this axis is bound to the
range of [0, 1] as opposed to the usual [-1, 1].
:returns: The axis value.
"""
return self._hid.getRightTriggerAxis()

19
commandsv2/src/main/python/commands2/button/joystickbutton.py Normal file
View File

@@ -0,0 +1,19 @@
# validated: 2024-01-20 DS 7a099cb02a33 button/JoystickButton.java
from wpilib.interfaces import GenericHID
from .trigger import Trigger
class JoystickButton(Trigger):
"""
A Button that gets its state from a :class:`wpilib.interfaces.GenericHID`.
"""
def __init__(self, joystick: GenericHID, buttonNumber: int):
"""
Creates a joystick button for triggering commands.
:param joystick: The GenericHID object that has the button (e.g. Joystick, KinectStick, etc)
:param buttonNumber: The button number (see :func:`wpilib.interfaces.GenericHID.getRawButton`
"""
super().__init__(lambda: joystick.getRawButton(buttonNumber))

126
commandsv2/src/main/python/commands2/button/networkbutton.py Normal file
View File

@@ -0,0 +1,126 @@
# validated: 2024-01-20 DS 7a099cb02a33 button/NetworkButton.java
from typing import overload
from ntcore import BooleanSubscriber, BooleanTopic, NetworkTable, NetworkTableInstance
from ..util import format_args_kwargs
from .trigger import Trigger
class NetworkButton(Trigger):
"""
A Button that uses a :class:`ntcore.NetworkTable` boolean field.
"""
@overload
def __init__(self, topic: BooleanTopic) -> None:
"""
Creates a NetworkButton that commands can be bound to.
:param topic: The boolean topic that contains the value.
"""
pass
@overload
def __init__(self, sub: BooleanSubscriber) -> None:
"""
Creates a NetworkButton that commands can be bound to.
:param sub: The boolean subscriber that provides the value.
"""
pass
@overload
def __init__(self, table: NetworkTable, field: str) -> None:
"""
Creates a NetworkButton that commands can be bound to.
:param table: The table where the networktable value is located.
:param field: The field that is the value.
"""
pass
@overload
def __init__(self, table: str, field: str) -> None:
"""
Creates a NetworkButton that commands can be bound to.
:param table: The table where the networktable value is located.
:param field: The field that is the value.
"""
pass
@overload
def __init__(self, inst: NetworkTableInstance, table: str, field: str) -> None:
"""
Creates a NetworkButton that commands can be bound to.
:param inst: The NetworkTable instance to use
:param table: The table where the networktable value is located.
:param field: the field that is the value.
"""
pass
def __init__(self, *args, **kwargs) -> None:
def init_sub(sub: BooleanSubscriber):
return super(NetworkButton, self).__init__(
lambda: sub.getTopic().getInstance().isConnected() and sub.get()
)
def init_topic(topic: BooleanTopic):
init_sub(topic.subscribe(False))
def init_table_field(table: NetworkTable, field: str):
init_topic(table.getBooleanTopic(field))
def init_inst_table_field(inst: NetworkTableInstance, table: str, field: str):
init_table_field(inst.getTable(table), field)
def init_str_table_field(table: str, field: str):
init_inst_table_field(NetworkTableInstance.getDefault(), table, field)
num_args = len(args) + len(kwargs)
if num_args == 1:
if "topic" in kwargs:
return init_topic(kwargs["topic"])
if "sub" in kwargs:
return init_sub(kwargs["sub"])
if isinstance(args[0], BooleanTopic):
return init_topic(args[0])
if isinstance(args[0], BooleanSubscriber):
return init_sub(args[0])
elif num_args == 2:
table, field, *_ = args + (None, None)
if "table" in kwargs:
table = kwargs["table"]
if "field" in kwargs:
field = kwargs["field"]
if table is not None and field is not None:
if isinstance(table, NetworkTable):
return init_table_field(table, field)
if isinstance(table, str):
return init_str_table_field(table, field)
elif num_args == 3:
inst, table, field, *_ = args + (None, None, None)
if "inst" in kwargs:
inst = kwargs["inst"]
if "table" in kwargs:
table = kwargs["table"]
if "field" in kwargs:
field = kwargs["field"]
if inst is not None and table is not None and field is not None:
return init_inst_table_field(inst, table, field)
raise TypeError(
f"""
TypeError: NetworkButton(): incompatible function arguments. The following argument types are supported:
1. (self: NetworkButton, topic: BooleanTopic)
2. (self: NetworkButton, sub: BooleanSubscriber)
3. (self: NetworkButton, table: NetworkTable, field: str)
4. (self: NetworkButton, table: str, field: str)
5. (self: NetworkButton, inst: NetworkTableInstance, table: str, field: str)
Invoked with: {format_args_kwargs(self, *args, **kwargs)}
"""
)

20
commandsv2/src/main/python/commands2/button/povbutton.py Normal file
View File

@@ -0,0 +1,20 @@
# validated: 2024-01-20 DS 7a099cb02a33 button/POVButton.java
from wpilib.interfaces import GenericHID
from .trigger import Trigger
class POVButton(Trigger):
"""
A Button that gets its state from a POV on a :class:`wpilib.interfaces.GenericHID`.
"""
def __init__(self, joystick: GenericHID, angle: int, povNumber: int = 0):
"""
Creates a POV button for triggering commands.
:param joystick: The GenericHID object that has the POV
:param angle: The desired angle in degrees (e.g. 90, 270)
:param povNumber: The POV number (see :func:`wpilib.interfaces.GenericHID.getPOV`)
"""
super().__init__(lambda: joystick.getPOV(povNumber) == angle)

304
commandsv2/src/main/python/commands2/button/trigger.py Normal file
View File

@@ -0,0 +1,304 @@
# validated: 2024-04-02 DS 0b1345946950 button/Trigger.java
from types import SimpleNamespace
from typing import Callable, overload
from typing_extensions import Self
from wpilib.event import EventLoop
from wpimath.filter import Debouncer
from ..command import Command
from ..commandscheduler import CommandScheduler
from ..util import format_args_kwargs
class Trigger:
"""
This class provides an easy way to link commands to conditions.
It is very easy to link a button to a command. For instance, you could link the trigger button
of a joystick to a "score" command.
"""
_loop: EventLoop
_condition: Callable[[], bool]
@overload
def __init__(self, condition: Callable[[], bool] = lambda: False):
"""
Creates a new trigger based on the given condition.
Polled by the default scheduler button loop.
:param condition: the condition represented by this trigger
"""
...
@overload
def __init__(self, loop: EventLoop, condition: Callable[[], bool]):
"""
Creates a new trigger based on the given condition.
:param loop: The loop instance that polls this trigger.
:param condition: the condition represented by this trigger
"""
...
def __init__(self, *args, **kwargs):
def init_loop_condition(loop: EventLoop, condition: Callable[[], bool]):
assert callable(condition)
self._loop = loop
self._condition = condition
def init_condition(condition: Callable[[], bool]):
init_loop_condition(
CommandScheduler.getInstance().getDefaultButtonLoop(), condition
)
num_args = len(args) + len(kwargs)
if num_args == 0:
return init_condition(lambda: False)
elif num_args == 1 and len(kwargs) == 1:
if "condition" in kwargs:
return init_condition(kwargs["condition"])
elif num_args == 1 and len(args) == 1:
if callable(args[0]):
return init_condition(args[0])
elif num_args == 2:
loop, condition, *_ = args + (None, None)
if "loop" in kwargs:
loop = kwargs["loop"]
if "condition" in kwargs:
condition = kwargs["condition"]
if loop is not None and condition is not None:
return init_loop_condition(loop, condition)
raise TypeError(
f"""
TypeError: Trigger(): incompatible function arguments. The following argument types are supported:
1. (self: Trigger)
2. (self: Trigger, condition: () -> bool)
3. (self: Trigger, loop: EventLoop, condition: () -> bool)
Invoked with: {format_args_kwargs(self, *args, **kwargs)}
"""
)
def onTrue(self, command: Command) -> Self:
"""
Starts the given command whenever the condition changes from `False` to `True`.
:param command: the command to start
:returns: this trigger, so calls can be chained
"""
state = SimpleNamespace(pressed_last=self._condition())
@self._loop.bind
def _():
pressed = self._condition()
if not state.pressed_last and pressed:
command.schedule()
state.pressed_last = pressed
return self
def onFalse(self, command: Command) -> Self:
"""
Starts the given command whenever the condition changes from `True` to `False`.
:param command: the command to start
:returns: this trigger, so calls can be chained
"""
state = SimpleNamespace(pressed_last=self._condition())
@self._loop.bind
def _():
pressed = self._condition()
if state.pressed_last and not pressed:
command.schedule()
state.pressed_last = pressed
return self
def onChange(self, command: Command) -> Self:
"""
Starts the command when the condition changes.
:param command: the command t start
:returns: this trigger, so calls can be chained
"""
state = SimpleNamespace(pressed_last=self._condition())
@self._loop.bind
def _():
pressed = self._condition()
if state.pressed_last != pressed:
command.schedule()
state.pressed_last = pressed
return self
def whileTrue(self, command: Command) -> Self:
"""
Starts the given command when the condition changes to `True` and cancels it when the condition
changes to `False`.
Doesn't re-start the command if it ends while the condition is still `True`. If the command
should restart, see :class:`commands2.RepeatCommand`.
:param command: the command to start
:returns: this trigger, so calls can be chained
"""
state = SimpleNamespace(pressed_last=self._condition())
@self._loop.bind
def _():
pressed = self._condition()
if not state.pressed_last and pressed:
command.schedule()
elif state.pressed_last and not pressed:
command.cancel()
state.pressed_last = pressed
return self
def whileFalse(self, command: Command) -> Self:
"""
Starts the given command when the condition changes to `False` and cancels it when the
condition changes to `True`.
Doesn't re-start the command if it ends while the condition is still `False`. If the command
should restart, see :class:`commands2.RepeatCommand`.
:param command: the command to start
:returns: this trigger, so calls can be chained
"""
state = SimpleNamespace(pressed_last=self._condition())
@self._loop.bind
def _():
pressed = self._condition()
if state.pressed_last and not pressed:
command.schedule()
elif not state.pressed_last and pressed:
command.cancel()
state.pressed_last = pressed
return self
def toggleOnTrue(self, command: Command) -> Self:
"""
Toggles a command when the condition changes from `False` to `True`.
:param command: the command to toggle
:returns: this trigger, so calls can be chained
"""
state = SimpleNamespace(pressed_last=self._condition())
@self._loop.bind
def _():
pressed = self._condition()
if not state.pressed_last and pressed:
if command.isScheduled():
command.cancel()
else:
command.schedule()
state.pressed_last = pressed
return self
def toggleOnFalse(self, command: Command) -> Self:
"""
Toggles a command when the condition changes from `True` to `False`.
:param command: the command to toggle
:returns: this trigger, so calls can be chained
"""
state = SimpleNamespace(pressed_last=self._condition())
@self._loop.bind
def _():
pressed = self._condition()
if state.pressed_last and not pressed:
if command.isScheduled():
command.cancel()
else:
command.schedule()
state.pressed_last = pressed
return self
def __call__(self) -> bool:
return self._condition()
def getAsBoolean(self) -> bool:
return self._condition()
def __bool__(self) -> bool:
return self._condition()
def __and__(self, other: Callable[[], bool]) -> "Trigger":
assert callable(other)
return Trigger(self._loop, lambda: self() and other())
def and_(self, other: Callable[[], bool]) -> "Trigger":
"""
Composes two triggers with logical AND.
:param trigger: the condition to compose with
:returns: A trigger which is active when both component triggers are active.
"""
return self & other
def __or__(self, other: Callable[[], bool]) -> "Trigger":
assert callable(other)
return Trigger(self._loop, lambda: self() or other())
def or_(self, other: Callable[[], bool]) -> "Trigger":
"""
Composes two triggers with logical OR.
:param trigger: the condition to compose with
:returns: A trigger which is active when either component trigger is active.
"""
return self | other
def __invert__(self) -> "Trigger":
return Trigger(self._loop, lambda: not self())
def negate(self) -> "Trigger":
"""
Creates a new trigger that is active when this trigger is inactive, i.e. that acts as the
negation of this trigger.
:returns: the negated trigger
"""
return ~self
def not_(self) -> "Trigger":
return ~self
def debounce(
self,
seconds: float,
debounce_type: Debouncer.DebounceType = Debouncer.DebounceType.kRising,
) -> "Trigger":
"""
Creates a new debounced trigger from this trigger - it will become active when this trigger has
been active for longer than the specified period.
:param seconds: The debounce period.
:param type: The debounce type.
:returns: The debounced trigger.
"""
debouncer = Debouncer(seconds, debounce_type)
return Trigger(self._loop, lambda: debouncer.calculate(self()))

239
commandsv2/src/main/python/commands2/cmd.py Normal file
View File

@@ -0,0 +1,239 @@
# validated: 2024-01-20 DS 8aeee0362672 Commands.java
from typing import Any, Callable, Dict, Hashable
from wpimath import units
from .command import Command
from .conditionalcommand import ConditionalCommand
from .functionalcommand import FunctionalCommand
from .instantcommand import InstantCommand
from .parallelcommandgroup import ParallelCommandGroup
from .paralleldeadlinegroup import ParallelDeadlineGroup
from .parallelracegroup import ParallelRaceGroup
from .printcommand import PrintCommand
from .runcommand import RunCommand
from .selectcommand import SelectCommand
from .sequentialcommandgroup import SequentialCommandGroup
from .startendcommand import StartEndCommand
from .subsystem import Subsystem
from .waitcommand import WaitCommand
from .waituntilcommand import WaitUntilCommand
# Is this whole file just to avoid the `new` keyword in Java?
def none() -> Command:
"""
Constructs a command that does nothing, finishing immediately.
:returns: the command
"""
return InstantCommand()
def idle(*requirements: Subsystem) -> Command:
"""
Constructs a command that does nothing until interrupted.
:param requirements: Subsystems to require
:return: the command
"""
return run(lambda: None, *requirements)
def runOnce(action: Callable[[], Any], *requirements: Subsystem) -> Command:
"""
Constructs a command that runs an action once and finishes.
:param action: the action to run
:param requirements: subsystems the action requires
:returns: the command
"""
return InstantCommand(action, *requirements)
def run(action: Callable[[], Any], *requirements: Subsystem) -> Command:
"""
Constructs a command that runs an action every iteration until interrupted.
:param action: the action to run
:param requirements: subsystems the action requires
:returns: the command
"""
return RunCommand(action, *requirements)
def startEnd(
run: Callable[[], Any], end: Callable[[], Any], *requirements: Subsystem
) -> Command:
"""
Constructs a command that runs an action once and another action when the command is
interrupted.
:param start: the action to run on start
:param end: the action to run on interrupt
:param requirements: subsystems the action requires
:returns: the command
"""
return StartEndCommand(run, end, *requirements)
def runEnd(
run: Callable[[], Any], end: Callable[[], Any], *requirements: Subsystem
) -> Command:
"""
Constructs a command that runs an action every iteration until interrupted, and then runs a
second action.
:param run: the action to run every iteration
:param end: the action to run on interrupt
:param requirements: subsystems the action requires
:returns: the command
"""
return FunctionalCommand(
lambda: None, run, lambda interrupted: end(), lambda: False, *requirements
)
def startRun(
start: Callable[[], Any], run: Callable[[], Any], *requirements: Subsystem
) -> Command:
"""
Constructs a command that runs an action once and another action every iteration until interrupted.
:param start: the action to run on start
:param run: the action to run every iteration
:param requirements: subsystems the action requires
:returns: the command
"""
return FunctionalCommand(
start, run, lambda interrupt: None, lambda: False, *requirements
)
def print_(message: str) -> Command:
"""
Constructs a command that prints a message and finishes.
:param message: the message to print
:returns: the command
"""
return PrintCommand(message)
def waitSeconds(seconds: units.seconds) -> Command:
"""
Constructs a command that does nothing, finishing after a specified duration.
:param seconds: after how long the command finishes
:returns: the command
"""
return WaitCommand(seconds)
def waitUntil(condition: Callable[[], bool]) -> Command:
"""
Constructs a command that does nothing, finishing once a condition becomes true.
:param condition: the condition
:returns: the command
"""
return WaitUntilCommand(condition)
def either(onTrue: Command, onFalse: Command, selector: Callable[[], bool]) -> Command:
"""
Runs one of two commands, based on the boolean selector function.
:param onTrue: the command to run if the selector function returns true
:param onFalse: the command to run if the selector function returns false
:param selector: the selector function
:returns: the command
"""
return ConditionalCommand(onTrue, onFalse, selector)
def select(
commands: Dict[Hashable, Command], selector: Callable[[], Hashable]
) -> Command:
"""
Runs one of several commands, based on the selector function.
:param selector: the selector function
:param commands: map of commands to select from
:returns: the command
"""
return SelectCommand(commands, selector)
def sequence(*commands: Command) -> Command:
"""
Runs a group of commands in series, one after the other.
:param commands: the commands to include
:returns: the command group
"""
return SequentialCommandGroup(*commands)
def repeatingSequence(*commands: Command) -> Command:
"""
Runs a group of commands in series, one after the other. Once the last command ends, the group
is restarted.
:param commands: the commands to include
:returns: the command group
"""
return sequence(*commands).repeatedly()
def parallel(*commands: Command) -> Command:
"""
Runs a group of commands at the same time. Ends once all commands in the group finish.
:param commands: the commands to include
:returns: the command
"""
return ParallelCommandGroup(*commands)
def race(*commands: Command) -> Command:
"""
Runs a group of commands at the same time. Ends once any command in the group finishes, and
cancels the others.
:param commands: the commands to include
:returns: the command group
"""
return ParallelRaceGroup(*commands)
def deadline(deadline: Command, *commands: Command) -> Command:
"""
Runs a group of commands at the same time. Ends once a specific command finishes, and cancels
the others.
:param deadline: the deadline command
:param commands: the commands to include
:returns: the command group
"""
return ParallelDeadlineGroup(deadline, *commands)
__all__ = [
"none",
"runOnce",
"run",
"startEnd",
"runEnd",
"print_",
"waitSeconds",
"waitUntil",
"either",
"select",
"sequence",
"repeatingSequence",
"parallel",
"race",
"deadline",
]

588
commandsv2/src/main/python/commands2/command.py Normal file
View File

@@ -0,0 +1,588 @@
# validated: 2024-01-20 DS f29a7d2e501b Command.java
# Don't import stuff from the package here, it'll cause a circular import
from __future__ import annotations
from enum import Enum
from typing import TYPE_CHECKING, Any, Callable, Set, Union
from typing_extensions import Self, TypeAlias
if TYPE_CHECKING:
from .instantcommand import InstantCommand
from .subsystem import Subsystem
from .parallelracegroup import ParallelRaceGroup
from .sequentialcommandgroup import SequentialCommandGroup
from .paralleldeadlinegroup import ParallelDeadlineGroup
from .parallelcommandgroup import ParallelCommandGroup
from .repeatcommand import RepeatCommand
from .proxycommand import ProxyCommand
from .conditionalcommand import ConditionalCommand
from .wrappercommand import WrapperCommand
from wpiutil import Sendable, SendableRegistry, SendableBuilder
class InterruptionBehavior(Enum):
"""
An enum describing the command's behavior when another command with a shared requirement is
scheduled.
"""
kCancelIncoming = 0
"""
This command ends, :func:`commands2.Command.end` is called, and the incoming command is
scheduled normally.
This is the default behavior.
"""
kCancelSelf = 1
""" This command continues, and the incoming command is not scheduled."""
class Command(Sendable):
"""
A state machine representing a complete action to be performed by the robot. Commands are run by
the :class:`commands2.CommandScheduler`, and can be composed into CommandGroups to allow users to build
complicated multistep actions without the need to roll the state machine logic themselves.
Commands are run synchronously from the main robot loop; no multithreading is used, unless
specified explicitly from the command implementation.
"""
InterruptionBehavior: TypeAlias = (
InterruptionBehavior # type alias for 2023 location
)
requirements: Set[Subsystem]
def __new__(cls, *args, **kwargs) -> Self:
instance = super().__new__(
cls,
)
super().__init__(instance)
SendableRegistry.add(instance, cls.__name__)
instance.requirements = set()
return instance
def __init__(self):
pass
def initialize(self):
"""The initial subroutine of a command. Called once when the command is initially scheduled."""
pass
def execute(self):
"""The main body of a command. Called repeatedly while the command is scheduled."""
pass
def end(self, interrupted: bool):
"""
The action to take when the command ends. Called when either the command finishes normally, or
when it interrupted/canceled.
Do not schedule commands here that share requirements with this command. Use :meth:`.andThen` instead.
:param interrupted: whether the command was interrupted/canceled
"""
pass
def isFinished(self) -> bool:
"""
Whether the command has finished. Once a command finishes, the scheduler will call its :meth:`commands2.Command.end`
method and un-schedule it.
:returns: whether the command has finished.
"""
return False
def getRequirements(self) -> Set[Subsystem]:
"""
Specifies the set of subsystems used by this command. Two commands cannot use the same
subsystem at the same time. If another command is scheduled that shares a requirement, :meth:`.getInterruptionBehavior` will be checked and followed. If no subsystems are required, return
an empty set.
.. note:: it is recommended that user implementations contain the requirements as a field, and
return that field here, rather than allocating a new set every time this is called.
:returns: the set of subsystems that are required
"""
return self.requirements
def addRequirements(self, *requirements: Subsystem):
"""
Adds the specified subsystems to the requirements of the command. The scheduler will prevent
two commands that require the same subsystem from being scheduled simultaneously.
.. note:: The scheduler determines the requirements of a command when it is scheduled, so
this method should normally be called from the command's constructor.
:param requirements: the requirements to add
"""
self.requirements.update(requirements)
def getName(self) -> str:
"""
Gets the name of this Command.
:returns: Name
"""
return SendableRegistry.getName(self)
def setName(self, name: str):
"""
Sets the name of this Command.
:param name: Name
"""
SendableRegistry.setName(self, name)
def getSubsystem(self) -> str:
"""
Gets the subsystem name of this Subsystem.
:returns: Subsystem name
"""
return SendableRegistry.getSubsystem(self)
def setSubsystem(self, subsystem: str):
"""
Sets the subsystem name of this Subsystem.
:param subsystem: subsystem name
"""
SendableRegistry.setSubsystem(self, subsystem)
def withTimeout(self, seconds: float) -> ParallelRaceGroup:
"""
Decorates this command with a timeout. If the specified timeout is exceeded before the command
finishes normally, the command will be interrupted and un-scheduled. Note that the timeout only
applies to the command returned by this method; the calling command is not itself changed.
.. note:: This decorator works by adding this command to a composition.
The command the decorator was called on cannot be scheduled
independently or be added to a different composition (namely,
decorators), unless it is manually cleared from the list of composed
commands with :func:`commands2.CommandScheduler.removeComposedCommand`.
The command composition returned from this method can be further
decorated without issue.
:param seconds: the timeout duration
:returns: the command with the timeout added
"""
from .waitcommand import WaitCommand
return self.raceWith(WaitCommand(seconds))
def until(self, condition: Callable[[], bool]) -> ParallelRaceGroup:
"""
Decorates this command with an interrupt condition. If the specified condition becomes true
before the command finishes normally, the command will be interrupted and un-scheduled.
.. note:: This decorator works by adding this command to a composition.
The command the decorator was called on cannot be scheduled
independently or be added to a different composition (namely,
decorators), unless it is manually cleared from the list of composed
commands with :func:`commands2.CommandScheduler.removeComposedCommand`.
The command composition returned from this method can be further
decorated without issue.
:param condition: the interrupt condition
:returns: the command with the interrupt condition added
"""
from .waituntilcommand import WaitUntilCommand
return self.raceWith(WaitUntilCommand(condition))
def onlyWhile(self, condition: Callable[[], bool]) -> ParallelRaceGroup:
"""
Decorates this command with a run condition. If the specified condition becomes false before
the command finishes normally, the command will be interrupted and un-scheduled.
.. note:: This decorator works by adding this command to a composition.
The command the decorator was called on cannot be scheduled
independently or be added to a different composition (namely,
decorators), unless it is manually cleared from the list of composed
commands with :func:`commands2.CommandScheduler.removeComposedCommand`.
The command composition returned from this method can be further
decorated without issue.
:param condition: the interrupt condition
:returns: the command with the interrupt condition added
"""
assert callable(condition)
return self.until(lambda: not condition())
def beforeStarting(
self, before: Union[Command, Callable[[], None]]
) -> SequentialCommandGroup:
"""
Decorates this command with a callable or command to run before this command starts.
.. note:: This decorator works by adding this command to a composition.
The command the decorator was called on cannot be scheduled
independently or be added to a different composition (namely,
decorators), unless it is manually cleared from the list of composed
commands with :func:`commands2.CommandScheduler.removeComposedCommand`.
The command composition returned from this method can be further
decorated without issue.
:param before: the command to run before this one
:returns: the decorated command
"""
from .instantcommand import InstantCommand
from .sequentialcommandgroup import SequentialCommandGroup
if callable(before):
before = InstantCommand(before)
return SequentialCommandGroup(before, self)
def andThen(self, *next: Command) -> SequentialCommandGroup:
"""
Decorates this command with a set of commands to run after it in sequence. Often more
convenient/less-verbose than constructing a new SequentialCommandGroup explicitly.
.. note:: This decorator works by adding this command to a composition.
The command the decorator was called on cannot be scheduled
independently or be added to a different composition (namely,
decorators), unless it is manually cleared from the list of composed
commands with :func:`commands2.CommandScheduler.removeComposedCommand`.
The command composition returned from this method can be further
decorated without issue.
:param next: the commands to run next
:returns: the decorated command
"""
from .sequentialcommandgroup import SequentialCommandGroup
return SequentialCommandGroup(self, *next)
def deadlineWith(self, *parallel: Command) -> ParallelDeadlineGroup:
"""
Decorates this command with a set of commands to run parallel to it, ending when the calling
command ends and interrupting all the others. Often more convenient/less-verbose than
constructing a new ParallelDeadlineGroup explicitly.
.. note:: This decorator works by adding this command to a composition.
The command the decorator was called on cannot be scheduled
independently or be added to a different composition (namely,
decorators), unless it is manually cleared from the list of composed
commands with :func:`commands2.CommandScheduler.removeComposedCommand`.
The command composition returned from this method can be further
decorated without issue.
:param parallel: the commands to run in parallel
:returns: the decorated command
"""
import warnings
warnings.warn(
"deadlineWith is deprecated use deadlineFor instead",
DeprecationWarning,
stacklevel=2,
)
from .paralleldeadlinegroup import ParallelDeadlineGroup
return ParallelDeadlineGroup(self, *parallel)
def deadlineFor(self, *parallel: Command) -> ParallelDeadlineGroup:
"""
Decorates this command with a set of commands to run parallel to it, ending when the calling
command ends and interrupting all the others. Often more convenient/less-verbose than
constructing a new ParallelDeadlineGroup explicitly.
.. note:: This decorator works by adding this command to a composition.
The command the decorator was called on cannot be scheduled
independently or be added to a different composition (namely,
decorators), unless it is manually cleared from the list of composed
commands with :func:`commands2.CommandScheduler.removeComposedCommand`.
The command composition returned from this method can be further
decorated without issue.
:param parallel: the commands to run in parallel
:returns: the decorated command
"""
from .paralleldeadlinegroup import ParallelDeadlineGroup
return ParallelDeadlineGroup(self, *parallel)
def alongWith(self, *parallel: Command) -> ParallelCommandGroup:
"""
Decorates this command with a set of commands to run parallel to it, ending when the last
command ends. Often more convenient/less-verbose than constructing a new {@link
ParallelCommandGroup} explicitly.
.. note:: This decorator works by adding this command to a composition.
The command the decorator was called on cannot be scheduled
independently or be added to a different composition (namely,
decorators), unless it is manually cleared from the list of composed
commands with :func:`commands2.CommandScheduler.removeComposedCommand`.
The command composition returned from this method can be further
decorated without issue.
:param parallel: the commands to run in parallel
:returns: the decorated command
"""
from .parallelcommandgroup import ParallelCommandGroup
return ParallelCommandGroup(self, *parallel)
def raceWith(self, *parallel: Command) -> ParallelRaceGroup:
"""
Decorates this command with a set of commands to run parallel to it, ending when the first
command ends. Often more convenient/less-verbose than constructing a new {@link
ParallelRaceGroup} explicitly.
.. note:: This decorator works by adding this command to a composition.
The command the decorator was called on cannot be scheduled
independently or be added to a different composition (namely,
decorators), unless it is manually cleared from the list of composed
commands with :func:`commands2.CommandScheduler.removeComposedCommand`.
The command composition returned from this method can be further
decorated without issue.
:param parallel: the commands to run in parallel
:returns: the decorated command
"""
from .parallelracegroup import ParallelRaceGroup
return ParallelRaceGroup(self, *parallel)
def repeatedly(self) -> RepeatCommand:
"""
Decorates this command to run repeatedly, restarting it when it ends, until this command is
interrupted. The decorated command can still be canceled.
.. note:: This decorator works by adding this command to a composition.
The command the decorator was called on cannot be scheduled
independently or be added to a different composition (namely,
decorators), unless it is manually cleared from the list of composed
commands with :func:`commands2.CommandScheduler.removeComposedCommand`.
The command composition returned from this method can be further
decorated without issue.
:returns: the decorated command
"""
from .repeatcommand import RepeatCommand
return RepeatCommand(self)
def asProxy(self) -> ProxyCommand:
"""
Decorates this command to run "by proxy" by wrapping it in a ProxyCommand. Use this for
"forking off" from command compositions when the user does not wish to extend the command's
requirements to the entire command composition. ProxyCommand has unique implications and
semantics, see the `WPILib docs <https://docs.wpilib.org/en/stable/docs/software/commandbased/command-compositions.html#scheduling-other-commands>`_ for a full explanation.
:returns: the decorated command
"""
from .proxycommand import ProxyCommand
return ProxyCommand(self)
def unless(self, condition: Callable[[], bool]) -> ConditionalCommand:
"""
Decorates this command to only run if this condition is not met. If the command is already
running and the condition changes to true, the command will not stop running. The requirements
of this command will be kept for the new conditional command.
.. note:: This decorator works by adding this command to a composition.
The command the decorator was called on cannot be scheduled
independently or be added to a different composition (namely,
decorators), unless it is manually cleared from the list of composed
commands with :func:`commands2.CommandScheduler.removeComposedCommand`.
The command composition returned from this method can be further
decorated without issue.
:param condition: the condition that will prevent the command from running
:returns: the decorated command
"""
from .conditionalcommand import ConditionalCommand
from .instantcommand import InstantCommand
return ConditionalCommand(InstantCommand(), self, condition)
def onlyIf(self, condition: Callable[[], bool]) -> ConditionalCommand:
"""
Decorates this command to only run if this condition is met. If the command is already running
and the condition changes to false, the command will not stop running. The requirements of this
command will be kept for the new conditional command.
.. note:: This decorator works by adding this command to a composition.
The command the decorator was called on cannot be scheduled
independently or be added to a different composition (namely,
decorators), unless it is manually cleared from the list of composed
commands with :func:`commands2.CommandScheduler.removeComposedCommand`.
The command composition returned from this method can be further
decorated without issue.
:param condition: the condition that will allow the command to run
:returns: the decorated command
"""
return self.unless(lambda: not condition())
def ignoringDisable(self, doesRunWhenDisabled: bool) -> WrapperCommand:
"""
Decorates this command to run or stop when disabled.
:param doesRunWhenDisabled: true to run when disabled.
:returns: the decorated command
"""
from .wrappercommand import WrapperCommand
class W(WrapperCommand):
def runsWhenDisabled(self) -> bool:
return doesRunWhenDisabled
return W(self)
def withInterruptBehavior(self, behavior: InterruptionBehavior) -> WrapperCommand:
"""
Decorates this command to have a different InterruptionBehavior interruption behavior.
:param interruptBehavior: the desired interrupt behavior
:returns: the decorated command
"""
from .wrappercommand import WrapperCommand
class W(WrapperCommand):
def getInterruptionBehavior(self) -> InterruptionBehavior:
return behavior
return W(self)
def finallyDo(self, end: Callable[[bool], Any]) -> WrapperCommand:
"""
Decorates this command with a lambda to call on interrupt or end, following the command's
inherent :func:`commands2.Command.end` method.
:param end: a lambda accepting a boolean parameter specifying whether the command was
interrupted.
:return: the decorated command
"""
from .wrappercommand import WrapperCommand
class W(WrapperCommand):
def end(self, interrupted: bool) -> None:
self._command.end(interrupted)
end(interrupted)
return W(self)
def handleInterrupt(self, handler: Callable[[], Any]) -> WrapperCommand:
"""
Decorates this command with a lambda to call on interrupt, following the command's inherent
:func:`commands2.Command.end` method.
:param handler: a lambda to run when the command is interrupted
:returns: the decorated command
"""
return self.finallyDo(lambda interrupted: handler() if interrupted else None)
def schedule(self) -> None:
"""Schedules this command."""
from .commandscheduler import CommandScheduler
CommandScheduler.getInstance().schedule(self)
def cancel(self) -> None:
"""
Cancels this command. Will call ``end(true)``. Commands will be canceled
regardless of InterruptionBehavior interruption behavior.
"""
from .commandscheduler import CommandScheduler
CommandScheduler.getInstance().cancel(self)
def isScheduled(self) -> bool:
"""
Whether the command is currently scheduled. Note that this does not detect whether the command
is in a composition, only whether it is directly being run by the scheduler.
:returns: Whether the command is scheduled.
"""
from .commandscheduler import CommandScheduler
return CommandScheduler.getInstance().isScheduled(self)
def hasRequirement(self, requirement: Subsystem) -> bool:
"""
Whether the command requires a given subsystem.
:param requirement: the subsystem to inquire about
:returns: whether the subsystem is required
"""
return requirement in self.requirements
def getInterruptionBehavior(self) -> InterruptionBehavior:
"""
How the command behaves when another command with a shared requirement is scheduled.
:returns: a variant of InterruptionBehavior, defaulting to {@link InterruptionBehavior#kCancelSelf kCancelSelf}.
"""
return InterruptionBehavior.kCancelSelf
def runsWhenDisabled(self) -> bool:
"""
Whether the given command should run when the robot is disabled. Override to return true if the
command should run when disabled.
:returns: whether the command should run when the robot is disabled
"""
return False
def withName(self, name: str) -> WrapperCommand:
"""
Decorates this command with a name.
:param name: the name of the command
:returns: the decorated command
"""
from .wrappercommand import WrapperCommand
wrapper = WrapperCommand(self)
wrapper.setName(name)
return wrapper
def initSendable(self, builder: SendableBuilder) -> None:
from .commandscheduler import CommandScheduler
builder.setSmartDashboardType("Command")
builder.addStringProperty(
".name",
self.getName,
lambda _: None,
)
def on_set(value: bool) -> None:
if value:
if not self.isScheduled():
self.schedule()
else:
if self.isScheduled():
self.cancel()
builder.addBooleanProperty(
"running",
self.isScheduled,
on_set,
)
builder.addBooleanProperty(
".isParented",
lambda: CommandScheduler.getInstance().isComposed(self),
lambda _: None,
)
builder.addStringProperty(
"interruptBehavior",
lambda: self.getInterruptionBehavior().name,
lambda _: None,
)
builder.addBooleanProperty(
"runsWhenDisabled",
self.runsWhenDisabled,
lambda _: None,
)

643
commandsv2/src/main/python/commands2/commandscheduler.py Normal file
View File

@@ -0,0 +1,643 @@
# validated: 2024-01-23 DS 8aeee0362672 CommandScheduler.java
from __future__ import annotations
import inspect
import os.path
import traceback
from typing import Any, Callable, Dict, Iterable, List, Optional, Set, Union
import hal
from typing_extensions import Self
from wpilib import (
RobotBase,
RobotState,
TimedRobot,
Watchdog,
reportWarning,
)
from wpilib.event import EventLoop
from wpiutil import Sendable, SendableBuilder, SendableRegistry
from .command import Command, InterruptionBehavior
from .exceptions import IllegalCommandUse
from .subsystem import Subsystem
_cmd_path = os.path.dirname(__file__)
class CommandScheduler(Sendable):
"""
The scheduler responsible for running Commands. A Command-based robot should call
:meth:`.run` on the singleton instance in its periodic block in order to run commands
synchronously from the main loop. Subsystems should be registered with the scheduler using :meth:`.registerSubsystem` in order for their :meth:`commands2.Subsystem.periodic`
methods to be called and for their default commands to be scheduled.
"""
_instance: Optional[CommandScheduler] = None
def __new__(cls) -> CommandScheduler:
if cls._instance is None:
return super().__new__(cls)
return cls._instance
@staticmethod
def getInstance() -> CommandScheduler:
"""
Returns the Scheduler instance.
:returns: the instance
"""
return CommandScheduler()
@staticmethod
def resetInstance() -> None:
"""
Resets the scheduler instance, which is useful for testing purposes. This should not be
called by user code.
"""
inst = CommandScheduler._instance
if inst:
inst._defaultButtonLoop.clear()
SendableRegistry.remove(inst)
CommandScheduler._instance = None
def __init__(self) -> None:
"""
Gets the scheduler instance.
"""
super().__init__()
if CommandScheduler._instance is not None:
return
CommandScheduler._instance = self
self._composedCommands: Dict[Command, str] = {}
# A set of the currently-running commands.
self._scheduledCommands: Dict[Command, None] = {}
# A map from required subsystems to their requiring commands. Also used as a set
# of the currently-required subsystems.
self._requirements: Dict[Subsystem, Command] = {}
# A map from subsystems registered with the scheduler to their default commands.
# Also used as a list of currently-registered subsystems.
self._subsystems: Dict[Subsystem, Optional[Command]] = {}
self._defaultButtonLoop = EventLoop()
# The set of currently-registered buttons that will be polled every iteration.
self._activeButtonLoop = self._defaultButtonLoop
self._disabled = False
# Lists of user-supplied actions to be executed on scheduling events for every
# command.
self._initActions: List[Callable[[Command], None]] = []
self._executeActions: List[Callable[[Command], None]] = []
self._interruptActions: List[Callable[[Command, Optional[Command]], None]] = []
self._finishActions: List[Callable[[Command], None]] = []
self._inRunLoop = False
self._toSchedule: Dict[Command, None] = {}
# python: toCancelInterruptors stored in _toCancel dict
self._toCancel: Dict[Command, Optional[Command]] = {}
# self._toCancelInterruptors: List[Optional[Command]] = []
self._endingCommands: Set[Command] = set()
self._watchdog = Watchdog(TimedRobot.kDefaultPeriod, lambda: None)
hal.reportUsage("CommandScheduler", "")
def setPeriod(self, period: float) -> None:
"""
Changes the period of the loop overrun watchdog. This should be kept in sync with the
TimedRobot period.
:param period: Period in seconds.
"""
self._watchdog.setTimeout(period)
def getDefaultButtonLoop(self) -> EventLoop:
"""
Get the default button poll.
:returns: a reference to the default EventLoop object polling buttons.
"""
return self._defaultButtonLoop
def getActiveButtonLoop(self) -> EventLoop:
"""
Get the active button poll.
:returns: a reference to the current EventLoop object polling buttons.
"""
return self._activeButtonLoop
def setActiveButtonLoop(self, loop: EventLoop) -> None:
"""
Replace the button poll with another one.
:param loop: the new button polling loop object.
"""
self._activeButtonLoop = loop
def _initCommand(self, command: Command, *requirements: Subsystem) -> None:
"""
Initializes a given command, adds its requirements to the list, and performs the init actions.
:param command: The command to initialize
:param requirements: The command requirements
"""
self._scheduledCommands[command] = None
for requirement in requirements:
self._requirements[requirement] = command
command.initialize()
for action in self._initActions:
action(command)
self._watchdog.addEpoch(f"{command.getName()}.initialize()")
def schedule(self, *commands: Command) -> None:
"""
Schedules a command for execution. Does nothing if the command is already scheduled. If a
command's requirements are not available, it will only be started if all the commands currently
using those requirements have been scheduled as interruptible. If this is the case, they will
be interrupted and the command will be scheduled.
WARNING: using this function directly can often lead to unexpected behavior and should be
avoided. Instead Triggers should be used to schedule Commands.
:param commands: the commands to schedule.
"""
for command in commands:
self._schedule(command)
def _schedule(self, command: Optional[Command]) -> None:
if command is None:
reportWarning("Tried to schedule a null command!", True)
return
if self._inRunLoop:
self._toSchedule[command] = None
return
self.requireNotComposed(command)
# Do nothing if the scheduler is disabled, the robot is disabled and the command
# doesn't run when disabled, or the command is already scheduled.
if self._disabled:
return
if self.isScheduled(command):
return
if RobotState.isDisabled() and not command.runsWhenDisabled():
return
requirements = command.getRequirements()
# Schedule the command if the requirements are not currently in-use.
if self._requirements.keys().isdisjoint(requirements):
self._initCommand(command, *requirements)
else:
# Else check if the requirements that are in use have all have interruptible
# commands, and if so, interrupt those commands and schedule the new
# command.
for requirement in requirements:
requiringCommand = self.requiring(requirement)
if (
requiringCommand is not None
and requiringCommand.getInterruptionBehavior()
== InterruptionBehavior.kCancelIncoming
):
return
for requirement in requirements:
requiringCommand = self.requiring(requirement)
if requiringCommand is not None:
self._cancel(requiringCommand, command)
self._initCommand(command, *requirements)
def run(self) -> None:
"""
Runs a single iteration of the scheduler. The execution occurs in the following order:
* Subsystem periodic methods are called.
* Button bindings are polled, and new commands are scheduled from them.
* Currently-scheduled commands are executed.
* End conditions are checked on currently-scheduled commands, and commands that are finished
have their end methods called and are removed.
* Any subsystems not being used as requirements have their default methods started.
"""
if self._disabled:
return
self._watchdog.reset()
# Run the periodic method of all registered subsystems.
for subsystem in self._subsystems:
subsystem.periodic()
if RobotBase.isSimulation():
subsystem.simulationPeriodic()
self._watchdog.addEpoch(f"{subsystem.getName()}.periodic()")
# Cache the active instance to avoid concurrency problems if setActiveLoop() is
# called from inside the button bindings.
loopCache = self._activeButtonLoop
# Poll buttons for new commands to add.
loopCache.poll()
self._watchdog.addEpoch("buttons.run()")
self._inRunLoop = True
isDisabled = RobotState.isDisabled()
# Run scheduled commands, remove finished commands.
for command in self._scheduledCommands.copy():
if isDisabled and not command.runsWhenDisabled():
self._cancel(command, None)
continue
command.execute()
for action in self._executeActions:
action(command)
self._watchdog.addEpoch(f"{command.getName()}.execute()")
if command.isFinished():
self._endingCommands.add(command)
command.end(False)
for action in self._finishActions:
action(command)
self._endingCommands.remove(command)
self._scheduledCommands.pop(command)
for requirement in command.getRequirements():
self._requirements.pop(requirement)
self._watchdog.addEpoch(f"{command.getName()}.end(False)")
self._inRunLoop = False
# Schedule/cancel commands from queues populated during loop
for command in self._toSchedule:
self._schedule(command)
for command, interruptor in self._toCancel.items():
self._cancel(command, interruptor)
self._toSchedule.clear()
self._toCancel.clear()
# Add default commands for un-required registered subsystems.
for subsystem, scommand in self._subsystems.items():
if subsystem not in self._requirements and scommand is not None:
self._schedule(scommand)
self._watchdog.disable()
if self._watchdog.isExpired():
self._watchdog.printEpochs()
def registerSubsystem(self, *subsystems: Subsystem) -> None:
"""
Registers subsystems with the scheduler. This must be called for the subsystem's periodic block
to run when the scheduler is run, and for the subsystem's default command to be scheduled. It
is recommended to call this from the constructor of your subsystem implementations.
:param subsystems: the subsystem to register
"""
for subsystem in subsystems:
if subsystem is None:
reportWarning("Tried to register a null subsystem", True)
continue
if subsystem in self._subsystems:
reportWarning("Tried to register an already-registered subsystem", True)
continue
self._subsystems[subsystem] = None
def unregisterSubsystem(self, *subsystems: Subsystem) -> None:
"""
Un-registers subsystems with the scheduler. The subsystem will no longer have its periodic
block called, and will not have its default command scheduled.
:param subsystems: the subsystem to un-register
"""
for subsystem in subsystems:
self._subsystems.pop(subsystem, None)
def unregisterAllSubsystems(self):
"""
Un-registers all registered Subsystems with the scheduler. All currently registered subsystems
will no longer have their periodic block called, and will not have their default command
scheduled.
"""
self._subsystems.clear()
def setDefaultCommand(self, subsystem: Subsystem, defaultCommand: Command) -> None:
"""
Sets the default command for a subsystem. Registers that subsystem if it is not already
registered. Default commands will run whenever there is no other command currently scheduled
that requires the subsystem. Default commands should be written to never end (i.e. their
:func:`commands2.Command.isFinished` method should return False), as they would simply be re-scheduled if they
do. Default commands must also require their subsystem.
:param subsystem: the subsystem whose default command will be set
:param defaultCommand: the default command to associate with the subsystem
"""
if subsystem is None:
reportWarning("Tried to set a default command for a null subsystem", True)
return
if defaultCommand is None:
reportWarning("Tried to set a null default command", True)
return
self.requireNotComposed(defaultCommand)
if subsystem not in defaultCommand.getRequirements():
raise IllegalCommandUse(
"Default commands must require their subsystem!",
command=defaultCommand,
subsystem=subsystem,
)
if (
defaultCommand.getInterruptionBehavior()
== InterruptionBehavior.kCancelIncoming
):
reportWarning(
"Registering a non-interruptible default command\nThis will likely prevent any other commands from requiring this subsystem.",
True,
)
# Warn, but allow -- there might be a use case for this.
self._subsystems[subsystem] = defaultCommand
def removeDefaultCommand(self, subsystem: Subsystem) -> None:
"""
Removes the default command for a subsystem. The current default command will run until another
command is scheduled that requires the subsystem, at which point the current default command
will not be re-scheduled.
:param subsystem: the subsystem whose default command will be removed
"""
if subsystem is None:
reportWarning(
"Tried to remove a default command for a null subsystem", True
)
return
self._subsystems[subsystem] = None
def getDefaultCommand(self, subsystem: Subsystem) -> Optional[Command]:
"""
Gets the default command associated with this subsystem. Null if this subsystem has no default
command associated with it.
:param subsystem: the subsystem to inquire about
:returns: the default command associated with the subsystem
"""
return self._subsystems[subsystem]
def cancel(
self,
*commands: Command,
) -> None:
"""
Cancels commands. The scheduler will only call :meth:`commands2.Command.end` method of the
canceled command with ``True``, indicating they were canceled (as opposed to finishing
normally).
Commands will be canceled regardless of InterruptionBehavior interruption behavior.
:param commands: the commands to cancel
"""
for command in commands:
self._cancel(command, None)
def _cancel(self, command: Command, interruptor: Optional[Command]):
if command is None:
reportWarning("Tried to cancel a null command", True)
return
if command in self._endingCommands:
return
if self._inRunLoop:
self._toCancel[command] = interruptor
return
if not self.isScheduled(command):
return
self._endingCommands.add(command)
command.end(True)
for action in self._interruptActions:
action(command, interruptor)
self._endingCommands.remove(command)
self._scheduledCommands.pop(command)
for requirement in command.getRequirements():
del self._requirements[requirement]
self._watchdog.addEpoch(f"{command.getName()}.end(true)")
def cancelAll(self) -> None:
"""Cancels all commands that are currently scheduled."""
self.cancel(*self._scheduledCommands)
def isScheduled(self, *commands: Command) -> bool:
"""
Whether the given commands are running. Note that this only works on commands that are directly
scheduled by the scheduler; it will not work on commands inside compositions, as the scheduler
does not see them.
:param commands: the command to query
:returns: whether the command is currently scheduled
"""
return all(command in self._scheduledCommands for command in commands)
def requiring(self, subsystem: Subsystem) -> Optional[Command]:
"""
Returns the command currently requiring a given subsystem. None if no command is currently
requiring the subsystem
:param subsystem: the subsystem to be inquired about
:returns: the command currently requiring the subsystem, or None if no command is currently
scheduled
"""
return self._requirements.get(subsystem)
def disable(self) -> None:
"""Disables the command scheduler."""
self._disabled = True
def enable(self) -> None:
"""Enables the command scheduler."""
self._disabled = False
def printWatchdogEpochs(self) -> None:
"""
Prints list of epochs added so far and their times.
"""
self._watchdog.printEpochs()
def onCommandInitialize(self, action: Callable[[Command], Any]) -> None:
"""
Adds an action to perform on the initialization of any command by the scheduler.
:param action: the action to perform
"""
assert callable(action)
self._initActions.append(action)
def onCommandExecute(self, action: Callable[[Command], Any]) -> None:
"""
Adds an action to perform on the execution of any command by the scheduler.
:param action: the action to perform
"""
assert callable(action)
self._executeActions.append(action)
def onCommandInterrupt(self, action: Callable[[Command], Any]) -> None:
"""
Adds an action to perform on the interruption of any command by the scheduler.
:param action: the action to perform
"""
assert callable(action)
self._interruptActions.append(lambda command, interruptor: action(command))
def onCommandInterruptWithCause(
self, action: Callable[[Command, Optional[Command]], Any]
) -> None:
"""
Adds an action to perform on the interruption of any command by the scheduler. The action receives the interrupted command and the command that interrupted it
:param action: the action to perform
"""
assert callable(action)
self._interruptActions.append(action)
def onCommandFinish(self, action: Callable[[Command], Any]) -> None:
"""
Adds an action to perform on the finishing of any command by the scheduler.
:param action: the action to perform
"""
assert callable(action)
self._finishActions.append(action)
def registerComposedCommands(self, commands: Iterable[Command]) -> None:
"""
Register commands as composed. An exception will be thrown if these commands are scheduled
directly or added to a composition.
:param commands: the commands to register
:raises IllegalCommandUse: if the given commands have already been composed.
"""
cmds = tuple(commands)
if len(set(cmds)) != len(cmds):
raise IllegalCommandUse(
"Cannot compose a command twice in the same composition!"
)
self.requireNotComposedOrScheduled(*cmds)
# Find where the user called us from
# - it would be better to give a full traceback, but this is fine for now
location = "<unknown>"
for f, lineno in traceback.walk_stack(None):
info = inspect.getframeinfo(f)
if not info.filename.startswith(_cmd_path):
location = f"{info.filename}:{lineno}"
break
for cmd in cmds:
self._composedCommands[cmd] = location
def clearComposedCommands(self) -> None:
"""
Clears the list of composed commands, allowing all commands to be freely used again.
.. warning:: Using this haphazardly can result in unexpected/undesirable behavior. Do not use
this unless you fully understand what you are doing.
"""
self._composedCommands.clear()
def removeComposedCommand(self, command: Command) -> None:
"""
Removes a single command from the list of composed commands, allowing it to be freely used
again.
.. warning:: Using this haphazardly can result in unexpected/undesirable behavior. Do not use
this unless you fully understand what you are doing.
:param command: the command to remove from the list of grouped commands
"""
self._composedCommands.pop(command, None)
def requireNotComposed(self, *commands: Command) -> None:
"""
Requires that the specified command hasn't been already added to a composition.
:param commands: The commands to check
:raises IllegalCommandUse: if the given commands have already been composed.
"""
for command in commands:
location = self._composedCommands.get(command)
if location is not None:
raise IllegalCommandUse(
"Commands that have been composed may not be added to another"
f"composition or scheduled individually (originally composed at {location})",
command=command,
)
def requireNotComposedOrScheduled(self, *commands: Command) -> None:
"""
Requires that the specified command hasn't already been added to a composition, and is not
currently scheduled.
:param command: The command to check
:raises IllegalCommandUse: if the given command has already been composed or scheduled.
"""
for command in commands:
if self.isScheduled(command):
raise IllegalCommandUse(
"Commands that have been scheduled individually may not be added to a composition!",
command=command,
)
self.requireNotComposed(command)
def isComposed(self, command: Command) -> bool:
"""
Check if the given command has been composed.
:param command: The command to check
:returns: true if composed
"""
return command in self._composedCommands
def initSendable(self, builder: SendableBuilder):
builder.setSmartDashboardType("Scheduler")
builder.addStringArrayProperty(
"Names",
lambda: [command.getName() for command in self._scheduledCommands],
lambda _: None,
)
builder.addIntegerArrayProperty(
"Ids",
lambda: [id(command) for command in self._scheduledCommands],
lambda _: None,
)
def cancel_commands(to_cancel: List[int]):
ids = {id(command): command for command in self._scheduledCommands}
for hash_value in to_cancel:
cancelCmd = ids.get(hash_value)
if cancelCmd is not None:
self.cancel(cancelCmd)
builder.addIntegerArrayProperty(
"Cancel", lambda: [], lambda to_cancel: cancel_commands(to_cancel) # type: ignore
)

93
commandsv2/src/main/python/commands2/conditionalcommand.py Normal file
View File

@@ -0,0 +1,93 @@
# validated: 2024-01-19 DS aaea85ff1656 ConditionalCommand.java
from __future__ import annotations
from typing import Callable, Optional
from wpiutil import SendableBuilder
from .command import Command, InterruptionBehavior
from .commandscheduler import CommandScheduler
class ConditionalCommand(Command):
"""
A command composition that runs one of two commands, depending on the value of the given
condition when this command is initialized.
The rules for command compositions apply: command instances that are passed to it cannot be
added to any other composition or scheduled individually, and the composition requires all
subsystems its components require.
"""
selectedCommand: Optional[Command]
def __init__(
self, onTrue: Command, onFalse: Command, condition: Callable[[], bool]
):
"""
Creates a new ConditionalCommand.
:param onTrue: the command to run if the condition is true
:param onFalse: the command to run if the condition is false
:param condition: the condition to determine which command to run
"""
super().__init__()
CommandScheduler.getInstance().registerComposedCommands([onTrue, onFalse])
self.onTrue = onTrue
self.onFalse = onFalse
self.condition = condition
self.addRequirements(*onTrue.getRequirements())
self.addRequirements(*onFalse.getRequirements())
def initialize(self):
if self.condition():
self.selectedCommand = self.onTrue
else:
self.selectedCommand = self.onFalse
self.selectedCommand.initialize()
def execute(self):
assert self.selectedCommand is not None
self.selectedCommand.execute()
def end(self, interrupted: bool):
assert self.selectedCommand is not None
self.selectedCommand.end(interrupted)
def isFinished(self) -> bool:
assert self.selectedCommand is not None
return self.selectedCommand.isFinished()
def runsWhenDisabled(self) -> bool:
return self.onTrue.runsWhenDisabled() and self.onFalse.runsWhenDisabled()
def getInterruptionBehavior(self) -> InterruptionBehavior:
if (
self.onTrue.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf
or self.onFalse.getInterruptionBehavior()
== InterruptionBehavior.kCancelSelf
):
return InterruptionBehavior.kCancelSelf
else:
return InterruptionBehavior.kCancelIncoming
def initSendable(self, builder: SendableBuilder):
super().initSendable(builder)
builder.addStringProperty("onTrue", self.onTrue.getName, lambda _: None)
builder.addStringProperty("onFalse", self.onFalse.getName, lambda _: None)
def _selected():
if self.selectedCommand is None:
return "null"
else:
return self.selectedCommand.getName()
builder.addStringProperty(
"selected",
_selected,
lambda _: None,
)

71
commandsv2/src/main/python/commands2/deferredcommand.py Normal file
View File

@@ -0,0 +1,71 @@
# validated: 2024-01-24 DS 192a28af4731 DeferredCommand.java
from typing import Callable
from wpiutil import SendableBuilder
from .command import Command
from .commandscheduler import CommandScheduler
from .printcommand import PrintCommand
from .subsystem import Subsystem
class DeferredCommand(Command):
"""
Defers Command construction to runtime. Runs the command returned by a supplier when this command
is initialized, and ends when it ends. Useful for performing runtime tasks before creating a new
command. If this command is interrupted, it will cancel the command.
Note that the supplier *must* create a new Command each call. For selecting one of a
preallocated set of commands, use :class:`commands2.SelectCommand`.
"""
def __init__(self, supplier: Callable[[], Command], *requirements: Subsystem):
"""
Creates a new DeferredCommand that directly runs the supplied command when initialized, and
ends when it ends. Useful for lazily creating commands when the DeferredCommand is initialized,
such as if the supplied command depends on runtime state. The Supplier will be called
each time this command is initialized. The Supplier *must* create a new Command each call.
:param supplier: The command supplier
:param requirements: The command requirements.
"""
super().__init__()
assert callable(supplier)
self._null_command = PrintCommand(
f"[DeferredCommand] Supplied command (from {supplier!r} was None!"
)
self._supplier = supplier
self._command = self._null_command
self.addRequirements(*requirements)
def initialize(self):
cmd = self._supplier()
if cmd is not None:
self._command = cmd
CommandScheduler.getInstance().registerComposedCommands([self._command])
self._command.initialize()
def execute(self):
self._command.execute()
def isFinished(self):
return self._command.isFinished()
def end(self, interrupted):
self._command.end(interrupted)
self._command = self._null_command
def initSendable(self, builder: SendableBuilder):
super().initSendable(builder)
builder.addStringProperty(
"deferred",
lambda: (
"null"
if self._command is self._null_command
else self._command.getName()
),
lambda _: None,
)

19
commandsv2/src/main/python/commands2/exceptions.py Normal file
View File

@@ -0,0 +1,19 @@
# notrack
import typing
class IllegalCommandUse(Exception):
"""
This exception is raised when a command is used in a way that it shouldn't be.
You shouldn't try to catch this exception, if it occurs it means your code is
doing something it probably shouldn't be doing
"""
def __init__(self, msg: str, **kwargs: typing.Any) -> None:
if kwargs:
args_repr = ", ".join(f"{k}={v!r}" for k, v in kwargs.items())
msg = f"{msg} ({args_repr})"
super().__init__(msg)

59
commandsv2/src/main/python/commands2/functionalcommand.py Normal file
View File

@@ -0,0 +1,59 @@
# validated: 2024-01-19 DS 6e58db398d63 FunctionalCommand.java
from __future__ import annotations
from typing import Any, Callable
from .command import Command
from .subsystem import Subsystem
class FunctionalCommand(Command):
"""
A command that allows the user to pass in functions for each of the basic command methods through
the constructor. Useful for inline definitions of complex commands - note, however, that if a
command is beyond a certain complexity it is usually better practice to write a proper class for
it than to inline it.
"""
def __init__(
self,
onInit: Callable[[], Any],
onExecute: Callable[[], Any],
onEnd: Callable[[bool], Any],
isFinished: Callable[[], bool],
*requirements: Subsystem,
):
"""
Creates a new FunctionalCommand.
:param onInit: the function to run on command initialization
:param onExecute: the function to run on command execution
:param onEnd: the function to run on command end
:param isFinished: the function that determines whether the command has finished
:param requirements: the subsystems required by this command
"""
super().__init__()
assert callable(onInit)
assert callable(onExecute)
assert callable(onEnd)
assert callable(isFinished)
self._onInit = onInit
self._onExecute = onExecute
self._onEnd = onEnd
self._isFinished = isFinished
self.addRequirements(*requirements)
def initialize(self):
self._onInit()
def execute(self):
self._onExecute()
def end(self, interrupted: bool):
self._onEnd(interrupted)
def isFinished(self) -> bool:
return self._isFinished()

33
commandsv2/src/main/python/commands2/instantcommand.py Normal file
View File

@@ -0,0 +1,33 @@
# validated: 2024-01-19 DS 5cf961edb973 InstantCommand.java
from __future__ import annotations
from typing import Callable, Optional
from .functionalcommand import FunctionalCommand
from .subsystem import Subsystem
class InstantCommand(FunctionalCommand):
"""
A Command that runs instantly; it will initialize, execute once, and end on the same iteration of
the scheduler. Users can either pass in a Callable and a set of requirements, or else subclass
this command if desired.
"""
def __init__(
self, toRun: Optional[Callable[[], None]] = None, *requirements: Subsystem
):
"""
Creates a new InstantCommand that runs the given Callable with the given requirements.
:param toRun: the Runnable to run
:param requirements: the subsystems required by this command
"""
assert toRun is None or callable(toRun)
super().__init__(
toRun or (lambda: None),
lambda: None,
lambda _: None,
lambda: True,
*requirements,
)

46
commandsv2/src/main/python/commands2/notifiercommand.py Normal file
View File

@@ -0,0 +1,46 @@
# validated: 2024-01-19 DS 6e58db398d63 NotifierCommand.java
from __future__ import annotations
from typing import Any, Callable
from wpilib import Notifier
from wpimath import units
from .command import Command
from .subsystem import Subsystem
class NotifierCommand(Command):
"""
A command that starts a notifier to run the given Callable periodically in a separate thread. Has
no end condition as-is; either subclass it or use :func:`commands2.Command.withTimeout` or :func:`commands2.Command.until` to give it one.
.. warning:: Do not use this class unless you are confident in your ability
to make the executed code thread-safe. If you do not know what
"thread-safe" means, that is a good sign that you should not use
this class.
"""
def __init__(
self, toRun: Callable[[], Any], period: units.seconds, *requirements: Subsystem
):
"""
Creates a new NotifierCommand.
:param toRun: the Callable for the notifier to run
:param period: the period at which the notifier should run, in seconds
:param requirements: the subsystems required by this command
"""
super().__init__()
assert callable(toRun)
self._notifier = Notifier(toRun)
self._period = period
self.addRequirements(*requirements)
def initialize(self):
self._notifier.startPeriodic(self._period)
def end(self, interrupted: bool):
self._notifier.stop()

95
commandsv2/src/main/python/commands2/parallelcommandgroup.py Normal file
View File

@@ -0,0 +1,95 @@
# validated: 2024-01-19 DS aaea85ff1656 ParallelCommandGroup.java
from __future__ import annotations
from typing import Dict
from .command import Command, InterruptionBehavior
from .commandscheduler import CommandScheduler
from .exceptions import IllegalCommandUse
from .util import flatten_args_commands
class ParallelCommandGroup(Command):
"""
A command composition that runs a set of commands in parallel, ending when the last command ends.
The rules for command compositions apply: command instances that are passed to it cannot be
added to any other composition or scheduled individually, and the composition requires all
subsystems its components require.
"""
def __init__(self, *commands: Command):
"""
Creates a new ParallelCommandGroup. The given commands will be executed simultaneously. The
command composition will finish when the last command finishes. If the composition is
interrupted, only the commands that are still running will be interrupted.
:param commands: the commands to include in this composition.
"""
super().__init__()
self._commands: Dict[Command, bool] = {}
self._runsWhenDisabled = True
self._interruptBehavior = InterruptionBehavior.kCancelIncoming
self.addCommands(*commands)
def addCommands(self, *commands: Command):
"""
Adds the given commands to the group.
:param commands: Commands to add to the group
"""
commands = flatten_args_commands(commands)
if True in self._commands.values():
raise IllegalCommandUse(
"Commands cannot be added to a composition while it is running"
)
CommandScheduler.getInstance().registerComposedCommands(commands)
for command in commands:
in_common = command.getRequirements().intersection(self.requirements)
if in_common:
raise IllegalCommandUse(
"Multiple commands in a parallel composition cannot require the same subsystems.",
common=in_common,
)
self._commands[command] = False
self.requirements.update(command.getRequirements())
self._runsWhenDisabled = (
self._runsWhenDisabled and command.runsWhenDisabled()
)
if command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf:
self._interruptBehavior = InterruptionBehavior.kCancelSelf
def initialize(self):
for command in self._commands:
command.initialize()
self._commands[command] = True
def execute(self):
for command, isRunning in self._commands.items():
if not isRunning:
continue
command.execute()
if command.isFinished():
command.end(False)
self._commands[command] = False
def end(self, interrupted: bool):
if interrupted:
for command, isRunning in self._commands.items():
if not isRunning:
continue
command.end(True)
self._commands[command] = False
def isFinished(self) -> bool:
return True not in self._commands.values()
def runsWhenDisabled(self) -> bool:
return self._runsWhenDisabled
def getInterruptionBehavior(self) -> InterruptionBehavior:
return self._interruptBehavior

140
commandsv2/src/main/python/commands2/paralleldeadlinegroup.py Normal file
View File

@@ -0,0 +1,140 @@
# validated: 2024-01-19 DS e07de37e64f2 ParallelDeadlineGroup.java
from __future__ import annotations
from typing import Dict
from wpiutil import SendableBuilder
from .command import Command, InterruptionBehavior
from .commandscheduler import CommandScheduler
from .exceptions import IllegalCommandUse
from .util import flatten_args_commands
class ParallelDeadlineGroup(Command):
"""
A command composition that runs a set of commands in parallel, ending only when a specific
command (the "deadline") ends, interrupting all other commands that are still running at that
point.
The rules for command compositions apply: command instances that are passed to it cannot be
added to any other composition or scheduled individually, and the composition requires all
subsystems its components require.
"""
def __init__(self, deadline: Command, *commands: Command):
"""
Creates a new ParallelDeadlineGroup. The given commands (including the
deadline) will be executed simultaneously. The composition will finish when
the deadline finishes, interrupting all other still-running commands. If
the composition is interrupted, only the commands still running will be
interrupted.
:param deadline: the command that determines when the composition ends
:param commands: the commands to be executed
:raises IllegalCommandUse: if the deadline command is also in the otherCommands argument
"""
super().__init__()
self._commands: Dict[Command, bool] = {}
self._runsWhenDisabled = True
self._finished = True
self._interruptBehavior = InterruptionBehavior.kCancelIncoming
self.addCommands(*commands)
self.setDeadline(deadline)
def setDeadline(self, deadline: Command):
"""
Sets the deadline to the given command. The deadline is added to the group if it is not already
contained.
:param deadline: the command that determines when the group ends
:raises IllegalCommandUse: if the deadline command is already in the composition
"""
# use getattr here because deadline not set in constructor
isAlreadyDeadline = deadline == getattr(self, "_deadline", None)
if isAlreadyDeadline:
return
if deadline in self._commands:
raise IllegalCommandUse(
f"The deadline command cannot also be in the other commands!",
deadline=deadline,
)
self.addCommands(deadline)
self._deadline = deadline
def addCommands(self, *commands: Command):
"""
Adds the given commands to the group.
:param commands: Commands to add to the group.
:raises IllegalCommandUse: if the deadline command is already in the composition
"""
commands = flatten_args_commands(commands)
if not self._finished:
raise IllegalCommandUse(
"Commands cannot be added to a composition while it is running"
)
CommandScheduler.getInstance().registerComposedCommands(commands)
for command in commands:
in_common = command.getRequirements().intersection(self.requirements)
if in_common:
raise IllegalCommandUse(
"Multiple commands in a parallel composition cannot require the same subsystems.",
common=in_common,
)
self._commands[command] = False
self.requirements.update(command.getRequirements())
self._runsWhenDisabled = (
self._runsWhenDisabled and command.runsWhenDisabled()
)
if command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf:
self._interruptBehavior = InterruptionBehavior.kCancelSelf
def initialize(self):
for command in self._commands:
command.initialize()
self._commands[command] = True
self._finished = False
def execute(self):
for command, isRunning in self._commands.items():
if not isRunning:
continue
command.execute()
if command.isFinished():
command.end(False)
self._commands[command] = False
if command == self._deadline:
self._finished = True
def end(self, interrupted: bool):
for command, isRunning in self._commands.items():
if not isRunning:
continue
command.end(True)
self._commands[command] = False
def isFinished(self) -> bool:
return self._finished
def runsWhenDisabled(self) -> bool:
return self._runsWhenDisabled
def getInterruptionBehavior(self) -> InterruptionBehavior:
return self._interruptBehavior
def initSendable(self, builder: SendableBuilder):
super().initSendable(builder)
builder.addStringProperty(
"deadline", lambda: self._deadline.getName(), lambda _: None
)

90
commandsv2/src/main/python/commands2/parallelracegroup.py Normal file
View File

@@ -0,0 +1,90 @@
# validated: 2024-01-19 DS aaea85ff1656 ParallelRaceGroup.java
from __future__ import annotations
from typing import Set
from .command import Command, InterruptionBehavior
from .commandscheduler import CommandScheduler
from .exceptions import IllegalCommandUse
from .util import flatten_args_commands
class ParallelRaceGroup(Command):
"""
A composition that runs a set of commands in parallel, ending when any one of the commands ends
and interrupting all the others.
The rules for command compositions apply: command instances that are passed to it cannot be
added to any other composition or scheduled individually, and the composition requires all
subsystems its components require.
"""
def __init__(self, *commands: Command):
"""
Creates a new ParallelCommandRace. The given commands will be executed simultaneously, and will
"race to the finish" - the first command to finish ends the entire command, with all other
commands being interrupted.
:param commands: the commands to include in this composition.
"""
super().__init__()
self._commands: Set[Command] = set()
self._runsWhenDisabled = True
self._finished = True
self._interruptBehavior = InterruptionBehavior.kCancelIncoming
self.addCommands(*commands)
def addCommands(self, *commands: Command):
"""
Adds the given commands to the group.
:param commands: Commands to add to the group.
"""
commands = flatten_args_commands(commands)
if not self._finished:
raise IllegalCommandUse(
"Commands cannot be added to a composition while it is running"
)
CommandScheduler.getInstance().registerComposedCommands(commands)
for command in commands:
in_common = command.getRequirements().intersection(self.requirements)
if in_common:
raise IllegalCommandUse(
"Multiple commands in a parallel composition cannot require the same subsystems.",
common=in_common,
)
self._commands.add(command)
self.requirements.update(command.getRequirements())
self._runsWhenDisabled = (
self._runsWhenDisabled and command.runsWhenDisabled()
)
if command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf:
self._interruptBehavior = InterruptionBehavior.kCancelSelf
def initialize(self):
self._finished = False
for command in self._commands:
command.initialize()
def execute(self):
for command in self._commands:
command.execute()
if command.isFinished():
self._finished = True
def end(self, interrupted: bool):
for command in self._commands:
command.end(not command.isFinished())
def isFinished(self) -> bool:
return self._finished
def runsWhenDisabled(self) -> bool:
return self._runsWhenDisabled
def getInterruptionBehavior(self) -> InterruptionBehavior:
return self._interruptBehavior

78
commandsv2/src/main/python/commands2/pidcommand.py Normal file
View File

@@ -0,0 +1,78 @@
# validated: 2024-01-19 DS f29a7d2e501b PIDCommand.java
# Copyright (c) FIRST and other WPILib contributors.
# Open Source Software; you can modify and/or share it under the terms of
# the WPILib BSD license file in the root directory of this project.
from __future__ import annotations
from typing import Any, Callable, Union
from .command import Command
from .subsystem import Subsystem
from wpimath.controller import PIDController
class PIDCommand(Command):
"""
A command that controls an output with a :class:`wpimath.controller.PIDController`. Runs forever by default - to add
exit conditions and/or other behavior, subclass this class. The controller calculation and output
are performed synchronously in the command's execute() method.
"""
def __init__(
self,
controller: PIDController,
measurementSource: Callable[[], float],
setpoint: Union[Callable[[], float], float, int],
useOutput: Callable[[float], Any],
*requirements: Subsystem,
):
"""
Creates a new PIDCommand, which controls the given output with a :class:`wpimath.controller.PIDController`.
:param controller: the controller that controls the output.
:param measurementSource: the measurement of the process variable
:param setpoint: the controller's setpoint (either a function that returns a)
number or a number
:param useOutput: the controller's output
:param requirements: the subsystems required by this command
"""
super().__init__()
assert callable(measurementSource)
assert callable(useOutput)
self._controller = controller
self._useOutput = useOutput
self._measurement = measurementSource
if isinstance(setpoint, (float, int)):
setpoint = float(setpoint)
self._setpoint = lambda: setpoint
elif callable(setpoint):
self._setpoint = setpoint
else:
raise ValueError(
f"invalid setpoint (must be callable or number; got {type(setpoint)})"
)
self.addRequirements(*requirements)
def initialize(self):
self._controller.reset()
def execute(self):
self._useOutput(
self._controller.calculate(self._measurement(), self._setpoint())
)
def end(self, interrupted):
self._useOutput(0)
def getController(self):
"""
Returns the PIDController used by the command.
:return: The PIDController
"""
return self._controller

100
commandsv2/src/main/python/commands2/pidsubsystem.py Normal file
View File

@@ -0,0 +1,100 @@
# validated: 2024-01-19 DS f29a7d2e501b PIDSubsystem.java
# Copyright (c) FIRST and other WPILib contributors.
# Open Source Software; you can modify and/or share it under the terms of
# the WPILib BSD license file in the root directory of this project.
from __future__ import annotations
from wpimath.controller import PIDController
from .subsystem import Subsystem
class PIDSubsystem(Subsystem):
"""
A subsystem that uses a :class:`wpimath.controller.PIDController` to control an output. The
controller is run synchronously from the subsystem's periodic() method.
"""
def __init__(self, controller: PIDController, initial_position: float = 0.0):
"""
Creates a new PIDSubsystem.
:param controller: The PIDController to use.
:param initial_position: The initial setpoint of the subsystem.
"""
super().__init__()
self._controller = controller
self.setSetpoint(initial_position)
self.addChild("PID Controller", self._controller)
self._enabled = False
def periodic(self):
"""
Executes the PID control logic during each periodic update.
This method is called synchronously from the subsystem's periodic() method.
"""
if self._enabled:
self.useOutput(
self._controller.calculate(self.getMeasurement()), self.getSetpoint()
)
def getController(self) -> PIDController:
"""
Returns the PIDController used by the subsystem.
:return: The PIDController.
"""
return self._controller
def setSetpoint(self, setpoint: float):
"""
Sets the setpoint for the subsystem.
:param setpoint: The setpoint for the subsystem.
"""
self._controller.setSetpoint(setpoint)
def getSetpoint(self) -> float:
"""
Returns the current setpoint of the subsystem.
:return: The current setpoint.
"""
return self._controller.getSetpoint()
def useOutput(self, output: float, setpoint: float):
"""
Uses the output from the PIDController.
:param output: The output of the PIDController.
:param setpoint: The setpoint of the PIDController (for feedforward).
"""
raise NotImplementedError(f"{self.__class__} must implement useOutput")
def getMeasurement(self) -> float:
"""
Returns the measurement of the process variable used by the PIDController.
:return: The measurement of the process variable.
"""
raise NotImplementedError(f"{self.__class__} must implement getMeasurement")
def enable(self):
"""Enables the PID control. Resets the controller."""
self._enabled = True
self._controller.reset()
def disable(self):
"""Disables the PID control. Sets output to zero."""
self._enabled = False
self.useOutput(0, 0)
def isEnabled(self) -> bool:
"""
Returns whether the controller is enabled.
:return: Whether the controller is enabled.
"""
return self._enabled

21
commandsv2/src/main/python/commands2/printcommand.py Normal file
View File

@@ -0,0 +1,21 @@
# validated: 2024-01-19 DS 8ac45f20bb47 PrintCommand.java
from __future__ import annotations
from .instantcommand import InstantCommand
class PrintCommand(InstantCommand):
"""
A command that prints a string when initialized.
"""
def __init__(self, message: str):
"""
Creates a new a PrintCommand.
:param message: the message to print
"""
super().__init__(lambda: print(message))
def runsWhenDisabled(self) -> bool:
return True

81
commandsv2/src/main/python/commands2/profiledpidcommand.py Normal file
View File

@@ -0,0 +1,81 @@
# validated: 2024-01-24 DS f29a7d2e501b ProfiledPIDCommand.java
#
# Copyright (c) FIRST and other WPILib contributors.
# Open Source Software; you can modify and/or share it under the terms of
# the WPILib BSD license file in the root directory of this project.
#
from typing import Any, Generic
from wpimath.controller import ProfiledPIDController, ProfiledPIDControllerRadians
from wpimath.trajectory import TrapezoidProfile, TrapezoidProfileRadians
from .command import Command
from .subsystem import Subsystem
from .typing import (
FloatOrFloatSupplier,
FloatSupplier,
TProfiledPIDController,
UseOutputFunction,
)
class ProfiledPIDCommand(Command, Generic[TProfiledPIDController]):
"""A command that controls an output with a :class:`.ProfiledPIDController`. Runs forever by default -
to add exit conditions and/or other behavior, subclass this class. The controller calculation and
output are performed synchronously in the command's execute() method.
"""
_stateCls: Any
def __init__(
self,
controller: TProfiledPIDController,
measurementSource: FloatSupplier,
goalSource: FloatOrFloatSupplier,
useOutput: UseOutputFunction,
*requirements: Subsystem,
):
"""Creates a new ProfiledPIDCommand, which controls the given output with a ProfiledPIDController. Goal
velocity is specified.
:param controller: the controller that controls the output.
:param measurementSource: the measurement of the process variable
:param goalSource: the controller's goal
:param useOutput: the controller's output
:param requirements: the subsystems required by this command
"""
super().__init__()
if isinstance(controller, ProfiledPIDController):
self._stateCls = TrapezoidProfile.State
elif isinstance(controller, ProfiledPIDControllerRadians):
self._stateCls = TrapezoidProfileRadians.State
else:
raise ValueError(f"unknown controller type {controller!r}")
self._controller: TProfiledPIDController = controller
self._useOutput = useOutput
self._measurement = measurementSource
if isinstance(goalSource, (float, int)):
self._goal = lambda: float(goalSource)
else:
self._goal = goalSource
self.addRequirements(*requirements)
def initialize(self):
self._controller.reset(self._measurement())
def execute(self):
self._useOutput.accept(
self._controller.calculate(self._measurement(), self._goal()),
self._controller.getSetpoint(),
)
def end(self, interrupted: bool):
self._useOutput(0.0, self._stateCls())
def getController(self):
"""Gets the controller used by the command"""
return self._controller

81
commandsv2/src/main/python/commands2/profiledpidsubsystem.py Normal file
View File

@@ -0,0 +1,81 @@
# Copyright (c) FIRST and other WPILib contributors.
# Open Source Software; you can modify and/or share it under the terms of
# the WPILib BSD license file in the root directory of this project.
from typing import Generic
from wpimath.trajectory import TrapezoidProfile
from .subsystem import Subsystem
from .typing import TProfiledPIDController, TTrapezoidProfileState
class ProfiledPIDSubsystem(
Subsystem, Generic[TProfiledPIDController, TTrapezoidProfileState]
):
"""
A subsystem that uses a :class:`wpimath.controller.ProfiledPIDController`
or :class:`wpimath.controller.ProfiledPIDControllerRadians` to
control an output. The controller is run synchronously from the subsystem's
:meth:`.periodic` method.
"""
def __init__(
self,
controller: TProfiledPIDController,
initial_position: float = 0,
):
"""
Creates a new Profiled PID Subsystem using the provided PID Controller
:param controller: the controller that controls the output
:param initial_position: the initial value of the process variable
"""
super().__init__()
self._controller: TProfiledPIDController = controller
self._enabled = False
self.setGoal(initial_position)
def periodic(self):
"""Updates the output of the controller."""
if self._enabled:
self.useOutput(
self._controller.calculate(self.getMeasurement()),
self._controller.getSetpoint(),
)
def getController(
self,
) -> TProfiledPIDController:
"""Returns the controller"""
return self._controller
def setGoal(self, goal):
"""Sets the goal state for the subsystem."""
self._controller.setGoal(goal)
def useOutput(self, output: float, setpoint: TTrapezoidProfileState):
"""Uses the output from the controller object."""
raise NotImplementedError(f"{self.__class__} must implement useOutput")
def getMeasurement(self) -> float:
"""
Returns the measurement of the process variable used by the
controller object.
"""
raise NotImplementedError(f"{self.__class__} must implement getMeasurement")
def enable(self):
"""Enables the PID control. Resets the controller."""
self._enabled = True
self._controller.reset(self.getMeasurement())
def disable(self):
"""Disables the PID control. Sets output to zero."""
self._enabled = False
self.useOutput(0, TrapezoidProfile.State())
def isEnabled(self) -> bool:
"""Returns whether the controller is enabled."""
return self._enabled

126
commandsv2/src/main/python/commands2/proxycommand.py Normal file
View File

@@ -0,0 +1,126 @@
# validated: 2024-01-19 DS 192a28af4731 ProxyCommand.java
from __future__ import annotations
from typing import Callable, overload
from wpiutil import SendableBuilder
from .command import Command
from .util import format_args_kwargs
import warnings
class ProxyCommand(Command):
"""
Schedules a given command when this command is initialized and ends when it ends, but does not
directly run it. Use this for including a command in a composition without adding its
requirements, but only if you know what you are doing. If you are unsure, see
`the WPILib docs <https://docs.wpilib.org/en/stable/docs/software/commandbased/command-compositions.html#scheduling-other-commands>`_
for a complete explanation of proxy semantics. Do not proxy a command from a subsystem already
required by the composition, or else the composition will cancel itself when the proxy is reached.
If this command is interrupted, it will cancel the command.
"""
_supplier: Callable[[], Command]
@overload
def __init__(self, supplier: Callable[[], Command]):
"""
Creates a new ProxyCommand that schedules the supplied command when initialized, and ends when
it is no longer scheduled. Use this for lazily creating **proxied** commands at
runtime. Proxying should only be done to escape from composition requirement semantics, so if
only initialization time command construction is needed, use DeferredCommand instead.
:param supplier: the command supplier
This constructor's similarity to DeferredCommand is confusing and opens
potential footguns for users who do not fully understand the semantics and implications of
proxying, but who simply want runtime construction. Users who do know what they are doing
and need a supplier-constructed proxied command should instead proxy a DeferredCommand
using the ``asProxy`` decorator.
"""
...
@overload
def __init__(self, command: Command):
"""
Creates a new ProxyCommand that schedules the given command when initialized, and ends when it
is no longer scheduled.
:param command: the command to run by proxy
"""
...
def __init__(self, *args, **kwargs):
super().__init__()
def init_supplier(supplier: Callable[[], Command]):
assert callable(supplier)
self._supplier = supplier
warnings.warn(
"The ProxyCommand supplier constructor has been deprecated",
DeprecationWarning,
stacklevel=3,
)
def init_command(command: Command):
self.setName(f"Proxy({command.getName()})")
self._supplier = lambda: command
num_args = len(args) + len(kwargs)
if num_args == 1 and len(kwargs) == 1:
if "supplier" in kwargs:
return init_supplier(kwargs["supplier"])
elif "command" in kwargs:
return init_command(kwargs["command"])
elif num_args == 1 and len(args) == 1:
if isinstance(args[0], Command):
return init_command(args[0])
elif callable(args[0]):
return init_supplier(args[0])
raise TypeError(
f"""
TypeError: ProxyCommand(): incompatible function arguments. The following argument types are supported:
1. (self: ProxyCommand, supplier: () -> Command)
2. (self: ProxyCommand, command: Command)
Invoked with: {format_args_kwargs(self, *args, **kwargs)}
"""
)
def initialize(self):
self._command = self._supplier()
self._command.schedule()
def end(self, interrupted: bool):
assert self._command is not None
if interrupted:
self._command.cancel()
self._command = None
def execute(self):
pass
def isFinished(self) -> bool:
# because we're between `initialize` and `end`, `self._command` is necessarily not None
# but if called otherwise and m_command is None,
# it's UB, so we can do whatever we want -- like return true.
return self._command is None or not self._command.isScheduled()
def runsWhenDisabled(self) -> bool:
"""
Whether the given command should run when the robot is disabled. Override to return true if the
command should run when disabled.
:returns: true. Otherwise, this proxy would cancel commands that do run when disabled.
"""
return True
def initSendable(self, builder: SendableBuilder):
super().initSendable(builder)
builder.addStringProperty(
"proxied",
lambda: "null" if self._command is None else self._command.getName(),
lambda _: None,
)

0
commandsv2/src/main/python/commands2/py.typed Normal file
View File

68
commandsv2/src/main/python/commands2/repeatcommand.py Normal file
View File

@@ -0,0 +1,68 @@
# validated: 2024-01-19 DS 6e58db398d63 RepeatCommand.java
from __future__ import annotations
from wpiutil import SendableBuilder
from .command import Command, InterruptionBehavior
from .commandscheduler import CommandScheduler
class RepeatCommand(Command):
"""
A command that runs another command repeatedly, restarting it when it ends, until this command is
interrupted. Command instances that are passed to it cannot be added to any other groups, or
scheduled individually.
The rules for command compositions apply: command instances that are passed to it cannot be
added to any other composition or scheduled individually,and the composition requires all
subsystems its components require.
"""
def __init__(self, command: Command):
"""
Creates a new RepeatCommand. Will run another command repeatedly, restarting it whenever it
ends, until this command is interrupted.
:param command: the command to run repeatedly
"""
super().__init__()
self._command = command
CommandScheduler.getInstance().registerComposedCommands([command])
self.requirements.update(command.getRequirements())
self.setName(f"Repeat({command.getName()})")
def initialize(self):
self._ended = False
self._command.initialize()
def execute(self):
if self._ended:
self._ended = False
self._command.initialize()
self._command.execute()
if self._command.isFinished():
# restart command
self._command.end(False)
self._ended = True
def isFinished(self) -> bool:
return False
def end(self, interrupted: bool):
# Make sure we didn't already call end() (which would happen if the command finished in the
# last call to our execute())
if not self._ended:
self._command.end(interrupted)
self._ended = True
def runsWhenDisabled(self) -> bool:
return self._command.runsWhenDisabled()
def getInterruptionBehavior(self) -> InterruptionBehavior:
return self._command.getInterruptionBehavior()
def initSendable(self, builder: SendableBuilder) -> None:
super().initSendable(builder)
builder.addStringProperty("command", self._command.getName, lambda _: None)

28
commandsv2/src/main/python/commands2/runcommand.py Normal file
View File

@@ -0,0 +1,28 @@
# validated: 2024-01-19 DS 5cf961edb973 RunCommand.java
from __future__ import annotations
from typing import Any, Callable
from .functionalcommand import FunctionalCommand
from .subsystem import Subsystem
class RunCommand(FunctionalCommand):
"""
A command that runs a Callable continuously. Has no end condition as-is; either subclass it or
use :func:`commands2.Command.withTimeout` or :func:`commands2.Command.until` to give it one.
If you only wish to execute a Callable once, use :class:`commands2.InstantCommand`.
"""
def __init__(self, toRun: Callable[[], Any], *requirements: Subsystem):
"""
Creates a new RunCommand. The Callable will be run continuously until the command ends. Does
not run when disabled.
:param toRun: the Callable to run
:param requirements: the subsystems to require
"""
assert callable(toRun)
super().__init__(
lambda: None, toRun, lambda interrupted: None, lambda: False, *requirements
)

31
commandsv2/src/main/python/commands2/schedulecommand.py Normal file
View File

@@ -0,0 +1,31 @@
# validated: 2024-01-19 DS aaea85ff1656 ScheduleCommand.java
from __future__ import annotations
from .command import Command
class ScheduleCommand(Command):
"""
Schedules the given commands when this command is initialized. Useful for forking off from
CommandGroups. Note that if run from a composition, the composition will not know about the
status of the scheduled commands, and will treat this command as finishing instantly.
"""
def __init__(self, *commands: Command):
"""
Creates a new ScheduleCommand that schedules the given commands when initialized.
:param toSchedule: the commands to schedule
"""
super().__init__()
self._toSchedule = set(commands)
def initialize(self):
for command in self._toSchedule:
command.schedule()
def isFinished(self) -> bool:
return True
def runsWhenDisabled(self) -> bool:
return True

86
commandsv2/src/main/python/commands2/selectcommand.py Normal file
View File

@@ -0,0 +1,86 @@
# validated: 2024-01-19 DS a4a8ad9c753e SelectCommand.java
from __future__ import annotations
from typing import Callable, Dict, Hashable
from wpiutil import SendableBuilder
from .command import Command, InterruptionBehavior
from .commandscheduler import CommandScheduler
from .printcommand import PrintCommand
class SelectCommand(Command):
"""
A command composition that runs one of a selection of commands, either using a selector and a key
to command mapping, or a supplier that returns the command directly at runtime.
The rules for command compositions apply: command instances that are passed to it cannot be
added to any other composition or scheduled individually, and the composition requires all
subsystems its components require.
"""
def __init__(
self,
commands: Dict[Hashable, Command],
selector: Callable[[], Hashable],
):
"""
Creates a new SelectCommand.
:param commands: the map of commands to choose from
:param selector: the selector to determine which command to run
"""
super().__init__()
assert callable(selector)
self._defaultCommand = PrintCommand(
"SelectCommand selector value does not correspond to any command!"
)
self._commands = commands
self._selector = selector
# This is slightly different than Java but avoids UB
self._selectedCommand = self._defaultCommand
self._runsWhenDisabled = True
self._interruptBehavior = InterruptionBehavior.kCancelIncoming
scheduler = CommandScheduler.getInstance()
scheduler.registerComposedCommands([self._defaultCommand])
scheduler.registerComposedCommands(commands.values())
for command in commands.values():
self.addRequirements(*command.getRequirements())
self._runsWhenDisabled = (
self._runsWhenDisabled and command.runsWhenDisabled()
)
if command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf:
self._interruptBehavior = InterruptionBehavior.kCancelSelf
def initialize(self):
self._selectedCommand = self._commands.get(
self._selector(), self._defaultCommand
)
self._selectedCommand.initialize()
def execute(self):
self._selectedCommand.execute()
def end(self, interrupted: bool):
self._selectedCommand.end(interrupted)
def isFinished(self) -> bool:
return self._selectedCommand.isFinished()
def runsWhenDisabled(self) -> bool:
return self._runsWhenDisabled
def getInterruptionBehavior(self) -> InterruptionBehavior:
return self._interruptBehavior
def initSendable(self, builder: SendableBuilder) -> None:
super().initSendable(builder)
builder.addStringProperty(
"selected", lambda: self._defaultCommand.getName(), lambda _: None
)

101
commandsv2/src/main/python/commands2/sequentialcommandgroup.py Normal file
View File

@@ -0,0 +1,101 @@
# validated: 2024-01-19 DS aaea85ff1656 SequentialCommandGroup.java
from __future__ import annotations
from typing import List
from wpiutil import SendableBuilder
from .command import Command, InterruptionBehavior
from .commandscheduler import CommandScheduler
from .exceptions import IllegalCommandUse
from .util import flatten_args_commands
class SequentialCommandGroup(Command):
"""
A command composition that runs a list of commands in sequence.
The rules for command compositions apply: command instances that are passed to it cannot be
added to any other composition or scheduled individually, and the composition requires all
subsystems its components require.
"""
def __init__(self, *commands: Command):
"""
Creates a new SequentialCommandGroup. The given commands will be run sequentially, with the
composition finishing when the last command finishes.
:param commands: the commands to include in this composition.
"""
super().__init__()
self._commands: List[Command] = []
self._currentCommandIndex = -1
self._runsWhenDisabled = True
self._interruptBehavior = InterruptionBehavior.kCancelIncoming
self.addCommands(*commands)
def addCommands(self, *commands: Command):
"""
Adds the given commands to the group.
:param commands: Commands to add to the group.
"""
commands = flatten_args_commands(commands)
if self._currentCommandIndex != -1:
raise IllegalCommandUse(
"Commands cannot be added to a composition while it is running"
)
CommandScheduler.getInstance().registerComposedCommands(commands)
for command in commands:
self._commands.append(command)
self.requirements.update(command.getRequirements())
self._runsWhenDisabled = (
self._runsWhenDisabled and command.runsWhenDisabled()
)
if command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf:
self._interruptBehavior = InterruptionBehavior.kCancelSelf
def initialize(self):
self._currentCommandIndex = 0
if self._commands:
self._commands[0].initialize()
def execute(self):
if not self._commands:
return
currentCommand = self._commands[self._currentCommandIndex]
currentCommand.execute()
if currentCommand.isFinished():
currentCommand.end(False)
self._currentCommandIndex += 1
if self._currentCommandIndex < len(self._commands):
self._commands[self._currentCommandIndex].initialize()
def end(self, interrupted: bool):
if (
interrupted
and self._commands
and -1 < self._currentCommandIndex < len(self._commands)
):
self._commands[self._currentCommandIndex].end(True)
self._currentCommandIndex = -1
def isFinished(self) -> bool:
return self._currentCommandIndex == len(self._commands)
def runsWhenDisabled(self) -> bool:
return self._runsWhenDisabled
def getInterruptionBehavior(self) -> InterruptionBehavior:
return self._interruptBehavior
def initSendable(self, builder: SendableBuilder) -> None:
super().initSendable(builder)
builder.addIntegerProperty(
"index", lambda: self._currentCommandIndex, lambda _: None
)

36
commandsv2/src/main/python/commands2/startendcommand.py Normal file
View File

@@ -0,0 +1,36 @@
# validated: 2024-01-19 DS b390cad09505 StartEndCommand.java
from __future__ import annotations
from typing import Any, Callable
from .functionalcommand import FunctionalCommand
from .subsystem import Subsystem
class StartEndCommand(FunctionalCommand):
"""
A command that runs a given Callable when it is initialized, and another Callable when it ends.
Useful for running and then stopping a motor, or extending and then retracting a solenoid. Has no
end condition as-is; either subclass it or use :func:`commands2.Command.withTimeout`
or :func:`commands2.Command.until` to give it one.
"""
def __init__(
self,
onInit: Callable[[], Any],
onEnd: Callable[[], Any],
*requirements: Subsystem,
):
"""
Creates a new StartEndCommand. Will run the given Callables when the command starts and when it
ends.
:param onInit: the Callable to run on command init
:param onEnd: the Callable to run on command end
:param requirements: the subsystems required by this command
"""
assert callable(onInit)
assert callable(onEnd)
super().__init__(
onInit, lambda: None, lambda _: onEnd(), lambda: False, *requirements
)

248
commandsv2/src/main/python/commands2/subsystem.py Normal file
View File

@@ -0,0 +1,248 @@
# validated: 2024-01-20 DS 1144115da01f Subsystem.java
# Don't import stuff from the package here, it'll cause a circular import
from __future__ import annotations
from typing import TYPE_CHECKING, Callable, Optional
from typing_extensions import Self
if TYPE_CHECKING:
from .command import Command
from .commandscheduler import CommandScheduler
from wpiutil import Sendable, SendableBuilder, SendableRegistry
class Subsystem(Sendable):
"""
A robot subsystem. Subsystems are the basic unit of robot organization in the Command-based
framework; they encapsulate low-level hardware objects (motor controllers, sensors, etc.) and
provide methods through which they can be used by Commands. Subsystems are used by the
CommandScheduler's resource management system to ensure multiple robot actions are not
"fighting" over the same hardware; Commands that use a subsystem should include that subsystem in
their :func:`commands2.Command.getRequirements` method, and resources used within a subsystem should
generally remain encapsulated and not be shared by other parts of the robot.
Subsystems must be registered with the scheduler with the :func:`commands2.CommandScheduler.registerSubsystem`
method in order for the :func:`.periodic` method to be called. It is recommended that this method be called from the
constructor of users' Subsystem implementations.
"""
def __new__(cls, *arg, **kwargs) -> Self:
instance = super().__new__(cls)
super().__init__(instance)
SendableRegistry.add(instance, cls.__name__, cls.__name__)
# add to the scheduler
from .commandscheduler import CommandScheduler
CommandScheduler.getInstance().registerSubsystem(instance)
return instance
def __init__(self) -> None:
pass
def periodic(self) -> None:
"""
This method is called periodically by the CommandScheduler. Useful for updating
subsystem-specific state that you don't want to offload to a Command. Teams should try
to be consistent within their own codebases about which responsibilities will be handled by
Commands, and which will be handled here.
"""
pass
def simulationPeriodic(self) -> None:
"""
This method is called periodically by the CommandScheduler. Useful for updating
subsystem-specific state that needs to be maintained for simulations, such as for updating simulation classes and setting simulated sensor readings.
"""
pass
def setDefaultCommand(self, command: Command) -> None:
"""
Sets the default Command of the subsystem. The default command will be automatically
scheduled when no other commands are scheduled that require the subsystem. Default commands
should generally not end on their own, i.e. their :func:`commands2.Command.isFinished` method should
always return false. Will automatically register this subsystem with the CommandScheduler.
:param defaultCommand: the default command to associate with this subsystem
"""
from .commandscheduler import CommandScheduler
CommandScheduler.getInstance().setDefaultCommand(self, command)
def removeDefaultCommand(self) -> None:
"""
Removes the default command for the subsystem. This will not cancel the default command if it
is currently running.
"""
CommandScheduler.getInstance().removeDefaultCommand(self)
def getDefaultCommand(self) -> Optional[Command]:
"""
Gets the default command for this subsystem. Returns None if no default command is currently
associated with the subsystem.
:returns: the default command associated with this subsystem
"""
from .commandscheduler import CommandScheduler
return CommandScheduler.getInstance().getDefaultCommand(self)
def getCurrentCommand(self) -> Optional[Command]:
"""
Returns the command currently running on this subsystem. Returns None if no command is
currently scheduled that requires this subsystem.
:returns: the scheduled command currently requiring this subsystem
"""
from .commandscheduler import CommandScheduler
return CommandScheduler.getInstance().requiring(self)
def register(self):
"""
Registers this subsystem with the :class:`.CommandScheduler`, allowing its
:func:`.periodic` method to be called when the scheduler runs.
"""
from .commandscheduler import CommandScheduler
return CommandScheduler.getInstance().registerSubsystem(self)
def runOnce(self, action: Callable[[], None]) -> Command:
"""
Constructs a command that runs an action once and finishes. Requires this subsystem.
:param action: the action to run
:return: the command
"""
from .cmd import runOnce
return runOnce(action, self)
def run(self, action: Callable[[], None]) -> Command:
"""
Constructs a command that runs an action every iteration until interrupted. Requires this
subsystem.
:param action: the action to run
:returns: the command"""
from .cmd import run
return run(action, self)
def startEnd(self, start: Callable[[], None], end: Callable[[], None]) -> Command:
"""
Constructs a command that runs an action once and another action when the command is
interrupted. Requires this subsystem.
:param start: the action to run on start
:param end: the action to run on interrupt
:returns: the command
"""
from .cmd import startEnd
return startEnd(start, end, self)
def runEnd(self, run: Callable[[], None], end: Callable[[], None]) -> Command:
"""
Constructs a command that runs an action every iteration until interrupted, and then runs a
second action. Requires this subsystem.
:param run: the action to run every iteration
:param end: the action to run on interrupt
:returns: the command
"""
from .cmd import runEnd
return runEnd(run, end, self)
def startRun(self, start: Callable[[], None], run: Callable[[], None]) -> Command:
"""
Constructs a command that runs an action once and another action every iteration until interrupted. Requires this subsystem.
:param start: the action to run on start
:param run: the action to run every iteration
:returns: the command
"""
from .cmd import startRun
return startRun(start, run, self)
def idle(self) -> Command:
"""
Constructs a command that does nothing until interrupted. Requires this subsystem.
:returns: the command
"""
from .cmd import idle
return idle(self)
#
# From SubsystemBase
#
def getName(self) -> str:
"""
Gets the name of this Subsystem.
:returns: Name
"""
return SendableRegistry.getName(self)
def setName(self, name: str) -> None:
"""
Set the name of this Subsystem.
"""
SendableRegistry.setName(self, name)
def getSubsystem(self) -> str:
"""
Gets the subsystem name of this Subsystem.
:returns: Subsystem name
"""
return SendableRegistry.getSubsystem(self)
def setSubsystem(self, subsystem: str):
"""
Sets the subsystem name of this Subsystem.
:param subsystem: subsystem name
"""
SendableRegistry.setSubsystem(self, subsystem)
def addChild(self, name: str, child: Sendable) -> None:
"""
Associates a :class:`wpiutil.Sendable` with this Subsystem. Also update the child's name.
:param name: name to give child
:param child: sendable
"""
SendableRegistry.add(child, self.getSubsystem(), name)
def initSendable(self, builder: SendableBuilder) -> None:
builder.setSmartDashboardType("Subsystem")
builder.addBooleanProperty(
".hasDefault", lambda: self.getDefaultCommand() is not None, lambda _: None
)
def get_default():
command = self.getDefaultCommand()
if command is not None:
return command.getName()
return "none"
builder.addStringProperty(".default", get_default, lambda _: None)
builder.addBooleanProperty(
".hasCommand", lambda: self.getCurrentCommand() is not None, lambda _: None
)
def get_current():
command = self.getCurrentCommand()
if command is not None:
return command.getName()
return "none"
builder.addStringProperty(".command", get_current, lambda _: None)

111
commandsv2/src/main/python/commands2/swervecontrollercommand.py Normal file
View File

@@ -0,0 +1,111 @@
# validated: 2024-01-20 DS 192a28af4731 SwerveControllerCommand.java
# Copyright (c) FIRST and other WPILib contributors.
# Open Source Software; you can modify and/or share it under the terms of
# the WPILib BSD license file in the root directory of this project.
from __future__ import annotations
from typing import Callable, Optional, Union, Tuple, Sequence
from wpimath.controller import (
HolonomicDriveController,
)
from wpimath.geometry import Pose2d, Rotation2d
from wpimath.kinematics import (
SwerveDrive2Kinematics,
SwerveDrive3Kinematics,
SwerveDrive4Kinematics,
SwerveDrive6Kinematics,
SwerveModuleState,
)
from wpimath.trajectory import Trajectory
from wpilib import Timer
from .command import Command
from .subsystem import Subsystem
class SwerveControllerCommand(Command):
"""
A command that uses two PID controllers (:class:`wpimath.controller.PIDController`)
and a HolonomicDriveController (:class:`wpimath.controller.HolonomicDriveController`)
to follow a trajectory (:class:`wpimath.trajectory.Trajectory`) with a swerve drive.
This command outputs the raw desired Swerve Module States (:class:`wpimath.kinematics.SwerveModuleState`) in an
array. The desired wheel and module rotation velocities should be taken from those and used in
velocity PIDs.
The robot angle controller does not follow the angle given by the trajectory but rather goes
to the angle given in the final state of the trajectory.
"""
def __init__(
self,
trajectory: Trajectory,
pose: Callable[[], Pose2d],
kinematics: Union[
SwerveDrive2Kinematics,
SwerveDrive3Kinematics,
SwerveDrive4Kinematics,
SwerveDrive6Kinematics,
],
controller: HolonomicDriveController,
outputModuleStates: Callable[[Sequence[SwerveModuleState]], None],
requirements: Tuple[Subsystem],
desiredRotation: Optional[Callable[[], Rotation2d]] = None,
) -> None:
"""
Constructs a new SwerveControllerCommand that when executed will follow the
provided trajectory. This command will not return output voltages but
rather raw module states from the position controllers which need to be put
into a velocity PID.
Note: The controllers will *not* set the outputVolts to zero upon
completion of the path- this is left to the user, since it is not
appropriate for paths with nonstationary endstates.
:param trajectory: The trajectory to follow.
:param pose: A function that supplies the robot pose - use one of the odometry classes to
provide this.
:param kinematics: The kinematics for the robot drivetrain. Can be kinematics for 2/3/4/6
SwerveKinematics.
:param controller: The HolonomicDriveController for the drivetrain.
If you have x, y, and theta controllers, pass them into
HolonomicPIDController.
:param outputModuleStates: The raw output module states from the position controllers.
:param requirements: The subsystems to require.
:param desiredRotation: (optional) The angle that the drivetrain should be
facing. This is sampled at each time step. If not specified, that rotation of
the final pose in the trajectory is used.
"""
super().__init__()
self._trajectory = trajectory
self._pose = pose
self._kinematics = kinematics
self._outputModuleStates = outputModuleStates
self._controller = controller
if desiredRotation is None:
self._desiredRotation = trajectory.states()[-1].pose.rotation
else:
self._desiredRotation = desiredRotation
self._timer = Timer()
self.addRequirements(*requirements)
def initialize(self):
self._timer.restart()
def execute(self):
curTime = self._timer.get()
desiredState = self._trajectory.sample(curTime)
targetChassisSpeeds = self._controller.calculate(
self._pose(), desiredState, self._desiredRotation()
)
targetModuleStates = self._kinematics.toSwerveModuleStates(targetChassisSpeeds)
self._outputModuleStates(targetModuleStates)
def end(self, interrupted):
self._timer.stop()
def isFinished(self):
return self._timer.hasElapsed(self._trajectory.totalTime())

4
commandsv2/src/main/python/commands2/sysid/__init__.py Normal file
View File

@@ -0,0 +1,4 @@
from .sysidroutine import SysIdRoutine
__all__ = ["SysIdRoutine"]

177
commandsv2/src/main/python/commands2/sysid/sysidroutine.py Normal file
View File

@@ -0,0 +1,177 @@
# validated: 2024-02-20 DV ee15cc172a5e sysid/SysIdRoutine.java
from dataclasses import dataclass, field
from enum import Enum
from wpilib.sysid import SysIdRoutineLog, State
from ..command import Command
from ..subsystem import Subsystem
from wpilib import Timer
from wpimath.units import seconds, volts
from typing import Callable, Optional
volts_per_second = float
class SysIdRoutine(SysIdRoutineLog):
"""A SysId characterization routine for a single mechanism. Mechanisms may have multiple motors.
A single subsystem may have multiple mechanisms, but mechanisms should not share test
routines. Each complete test of a mechanism should have its own SysIdRoutine instance, since the
log name of the recorded data is determined by the mechanism name.
The test state (e.g. "quasistatic-forward") is logged once per iteration during test
execution, and once with state "none" when a test ends. Motor frames are logged every iteration
during test execution.
Timestamps are not coordinated across data, so motor frames and test state tags may be
recorded on different log frames. Because frame alignment is not guaranteed, SysId parses the log
by using the test state flag to determine the timestamp range for each section of the test, and
then extracts the motor frames within the valid timestamp ranges. If a given test was run
multiple times in a single logfile, the user will need to select which of the tests to use for
the fit in the analysis tool.
"""
@dataclass
class Config:
"""Hardware-independent configuration for a SysId test routine.
:param rampRate: The voltage ramp rate used for quasistatic test routines. Defaults to 1 volt
per second if left null.
:param stepVoltage: The step voltage output used for dynamic test routines. Defaults to 7
volts if left null.
:param timeout: Safety timeout for the test routine commands. Defaults to 10 seconds if left
null.
:param recordState: Optional handle for recording test state in a third-party logging
solution. If provided, the test routine state will be passed to this callback instead of
logged in WPILog.
"""
rampRate: volts_per_second = 1.0
stepVoltage: volts = 7.0
timeout: seconds = 10.0
recordState: Optional[Callable[[State], None]] = None
@dataclass
class Mechanism:
"""A mechanism to be characterized by a SysId routine.
Defines callbacks needed for the SysId test routine to control
and record data from the mechanism.
:param drive: Sends the SysId-specified drive signal to the mechanism motors during test
routines.
:param log: Returns measured data of the mechanism motors during test routines. To return
data, call `motor(string motorName)` on the supplied `SysIdRoutineLog` instance, and then
call one or more of the chainable logging handles (e.g. `voltage`) on the returned
`MotorLog`. Multiple motors can be logged in a single callback by calling `motor`
multiple times.
:param subsystem: The subsystem containing the motor(s) that is (or are) being characterized.
Will be declared as a requirement for the returned test commands.
:param name: The name of the mechanism being tested. Will be appended to the log entry title
for the routine's test state, e.g. "sysid-test-state-mechanism". Defaults to the name of
the subsystem if left null.
"""
drive: Callable[[volts], None]
log: Callable[[SysIdRoutineLog], None]
subsystem: Subsystem
name: str = None # type: ignore[assignment]
def __post_init__(self):
if self.name is None:
self.name = self.subsystem.getName()
class Direction(Enum):
"""Motor direction for a SysId test."""
kForward = 1
kReverse = -1
def __init__(self, config: Config, mechanism: Mechanism):
"""Create a new SysId characterization routine.
:param config: Hardware-independent parameters for the SysId routine.
:param mechanism: Hardware interface for the SysId routine.
"""
super().__init__(mechanism.name)
self.config = config
self.mechanism = mechanism
self.outputVolts = 0.0
self.logState = config.recordState or self.recordState
def quasistatic(self, direction: Direction) -> Command:
"""Returns a command to run a quasistatic test in the specified direction.
The command will call the `drive` and `log` callbacks supplied at routine construction once
per iteration. Upon command end or interruption, the `drive` callback is called with a value of
0 volts.
:param direction: The direction in which to run the test.
:returns: A command to run the test.
"""
timer = Timer()
if direction == self.Direction.kForward:
state = State.kQuasistaticForward
else:
state = State.kQuasistaticReverse
def execute():
self.outputVolts = direction.value * timer.get() * self.config.rampRate
self.mechanism.drive(self.outputVolts)
self.mechanism.log(self)
self.logState(state)
def end(interrupted: bool):
self.mechanism.drive(0.0)
self.logState(State.kNone)
timer.stop()
return (
self.mechanism.subsystem.runOnce(timer.restart)
.andThen(self.mechanism.subsystem.run(execute))
.finallyDo(end)
.withName(f"sysid-{state}-{self.mechanism.name}")
.withTimeout(self.config.timeout)
)
def dynamic(self, direction: Direction) -> Command:
"""Returns a command to run a dynamic test in the specified direction.
The command will call the `drive` and `log` callbacks supplied at routine construction once
per iteration. Upon command end or interruption, the `drive` callback is called with a value of
0 volts.
:param direction: The direction in which to run the test.
:returns: A command to run the test.
"""
if direction == self.Direction.kForward:
state = State.kDynamicForward
else:
state = State.kDynamicReverse
def command():
self.outputVolts = direction.value * self.config.stepVoltage
def execute():
self.mechanism.drive(self.outputVolts)
self.mechanism.log(self)
self.logState(state)
def end(interrupted: bool):
self.mechanism.drive(0.0)
self.logState(State.kNone)
return (
self.mechanism.subsystem.runOnce(command)
.andThen(self.mechanism.subsystem.run(execute))
.finallyDo(end)
.withName(f"sysid-{state}-{self.mechanism.name}")
.withTimeout(self.config.timeout)
)

16
commandsv2/src/main/python/commands2/timedcommandrobot.py Normal file
View File

@@ -0,0 +1,16 @@
# notrack
from wpilib import TimedRobot
from .commandscheduler import CommandScheduler
seconds = float
class TimedCommandRobot(TimedRobot):
kSchedulerOffset = 0.005
def __init__(self, period: seconds = TimedRobot.kDefaultPeriod / 1000) -> None:
super().__init__(period)
self.addPeriodic(
CommandScheduler.getInstance().run, period, self.kSchedulerOffset
)

64
commandsv2/src/main/python/commands2/trapezoidprofilecommand.py Normal file
View File

@@ -0,0 +1,64 @@
# validated: 2024-01-24 DS 192a28af4731 TrapezoidProfileCommand.java
#
# Copyright (c) FIRST and other WPILib contributors.
# Open Source Software; you can modify and/or share it under the terms of
# the WPILib BSD license file in the root directory of this project.
#
from __future__ import annotations
from typing import Callable, Any
from wpilib import Timer
from .command import Command
from .subsystem import Subsystem
class TrapezoidProfileCommand(Command):
"""
A command that runs a :class:`wpimath.trajectory.TrapezoidProfile`. Useful for smoothly controlling mechanism motion.
"""
def __init__(
self,
profile,
output: Callable[[Any], Any],
goal: Callable[[], Any],
currentState: Callable[[], Any],
*requirements: Subsystem,
):
"""Creates a new TrapezoidProfileCommand that will execute the given :class:`wpimath.trajectory.TrapezoidProfile`.
Output will be piped to the provided consumer function.
:param profile: The motion profile to execute.
:param output: The consumer for the profile output.
:param goal: The supplier for the desired state
:param currentState: The supplier for the current state
:param requirements: The subsystems required by this command.
"""
super().__init__()
self._profile = profile
self._output = output
self._goal = goal
self._currentState = currentState
self._timer = Timer()
self.addRequirements(*requirements)
def initialize(self) -> None:
self._timer.restart()
def execute(self) -> None:
self._output(
self._profile.calculate(
self._timer.get(),
self._currentState(),
self._goal(),
)
)
def end(self, interrupted) -> None:
self._timer.stop()
def isFinished(self) -> bool:
return self._timer.hasElapsed(self._profile.totalTime())

94
commandsv2/src/main/python/commands2/trapezoidprofilesubsystem.py Normal file
View File

@@ -0,0 +1,94 @@
# validated: 2024-02-20 DV 6cc7e52de74a TrapezoidProfileSubsystem.java
#
# Copyright (c) FIRST and other WPILib contributors.
# Open Source Software; you can modify and/or share it under the terms of
# the WPILib BSD license file in the root directory of this project.
#
from __future__ import annotations
from typing import Any, Union
from wpimath import units
from wpimath.trajectory import TrapezoidProfile, TrapezoidProfileRadians
from .subsystem import Subsystem
class TrapezoidProfileSubsystem(Subsystem):
"""
A subsystem that generates and runs trapezoidal motion profiles automatically. The user specifies
how to use the current state of the motion profile by overriding the `useState` method.
"""
_profile: Any
_stateCls: Any
def __init__(
self,
constraints: Union[
TrapezoidProfile.Constraints, TrapezoidProfileRadians.Constraints
],
initial_position: float = 0.0,
period: units.seconds = 0.02,
):
"""
Creates a new TrapezoidProfileSubsystem.
:param constraints: The constraints (maximum velocity and acceleration) for the profiles.
:param initial_position: The initial position of the controlled mechanism when the subsystem is constructed.
:param period: The period of the main robot loop, in seconds.
"""
if isinstance(constraints, TrapezoidProfile.Constraints):
self._profile = TrapezoidProfile(constraints)
self._stateCls = TrapezoidProfile.State
elif isinstance(constraints, TrapezoidProfileRadians.Constraints):
self._profile = TrapezoidProfileRadians(constraints)
self._stateCls = TrapezoidProfileRadians.State
else:
raise ValueError(f"Invalid constraints {constraints}")
self._state = self._stateCls(initial_position, 0)
self.setGoal(initial_position)
self._period = period
self._enabled = True
def periodic(self):
"""
Executes the TrapezoidProfileSubsystem logic during each periodic update.
This method is called synchronously from the subsystem's periodic() method.
"""
self._state = self._profile.calculate(self._period, self._state, self._goal)
if self._enabled:
self.useState(self._state)
def setGoal(self, goal):
"""
Sets the goal state for the subsystem. Goal velocity assumed to be zero.
:param goal: The goal position for the subsystem's motion profile. The goal
can either be a `TrapezoidProfile.State` or `float`. If float is provided,
the assumed velocity for the goal will be 0.
"""
# If we got a float, instantiate the state
if isinstance(goal, (float, int)):
goal = self._stateCls(goal, 0)
self._goal = goal
def enable(self):
"""Enable the TrapezoidProfileSubsystem's output."""
self._enabled = True
def disable(self):
"""Disable the TrapezoidProfileSubsystem's output."""
self._enabled = False
def useState(self, state):
"""
Users should override this to consume the current state of the motion profile.
:param state: The current state of the motion profile.
"""
raise NotImplementedError(f"{self.__class__} must implement useState")

30
commandsv2/src/main/python/commands2/typing.py Normal file
View File

@@ -0,0 +1,30 @@
from typing import Callable, Protocol, TypeVar, Union
from typing_extensions import TypeAlias
from wpimath.controller import ProfiledPIDController, ProfiledPIDControllerRadians
from wpimath.trajectory import TrapezoidProfile, TrapezoidProfileRadians
# Generic Types
TProfiledPIDController = TypeVar(
"TProfiledPIDController", ProfiledPIDControllerRadians, ProfiledPIDController
)
TTrapezoidProfileState = TypeVar(
"TTrapezoidProfileState",
TrapezoidProfileRadians.State,
TrapezoidProfile.State,
)
# Protocols - Structural Typing
class UseOutputFunction(Protocol):
def __init__(self, *args, **kwargs) -> None: ...
def __call__(self, t: float, u: TTrapezoidProfileState) -> None: ...
def accept(self, t: float, u: TTrapezoidProfileState) -> None: ...
# Type Aliases
FloatSupplier: TypeAlias = Callable[[], float]
FloatOrFloatSupplier: TypeAlias = Union[float, Callable[[], float]]

25
commandsv2/src/main/python/commands2/util.py Normal file
View File

@@ -0,0 +1,25 @@
# notrack
from __future__ import annotations
from typing import Iterable, List, Tuple, Union
from .command import Command
def flatten_args_commands(
*commands: Union[Command, Iterable[Command]]
) -> Tuple[Command, ...]:
flattened_commands: List[Command] = []
for command in commands:
if isinstance(command, Command):
flattened_commands.append(command)
elif isinstance(command, Iterable):
flattened_commands.extend(flatten_args_commands(*command))
return tuple(flattened_commands)
def format_args_kwargs(*args, **kwargs) -> str:
return ", ".join(
[repr(arg) for arg in args]
+ [f"{key}={repr(value)}" for key, value in kwargs.items()]
)

41
commandsv2/src/main/python/commands2/waitcommand.py Normal file
View File

@@ -0,0 +1,41 @@
# validated: 2024-01-20 DS f29a7d2e501b WaitCommand.java
from __future__ import annotations
from wpilib import Timer
from wpimath import units
from wpiutil import SendableBuilder
from .command import Command
class WaitCommand(Command):
"""
A command that does nothing but takes a specified amount of time to finish.
"""
def __init__(self, seconds: units.seconds):
"""
Creates a new WaitCommand. This command will do nothing, and end after the specified duration.
:param seconds: the time to wait, in seconds
"""
super().__init__()
self._duration = seconds
self._timer = Timer()
self.setName(f"{self.getName()}: {seconds}")
def initialize(self):
self._timer.restart()
def end(self, interrupted: bool):
self._timer.stop()
def isFinished(self) -> bool:
return self._timer.hasElapsed(self._duration)
def runsWhenDisabled(self) -> bool:
return True
def initSendable(self, builder: SendableBuilder) -> None:
super().initSendable(builder)
builder.addDoubleProperty("duration", lambda: self._duration, lambda _: None)

80
commandsv2/src/main/python/commands2/waituntilcommand.py Normal file
View File

@@ -0,0 +1,80 @@
# validated: 2024-01-20 DS aaea85ff1656 WaitUntilCommand.java
from __future__ import annotations
from typing import Callable, overload
from wpilib import Timer
from wpimath import units
from .command import Command
from .util import format_args_kwargs
class WaitUntilCommand(Command):
"""
A command that does nothing but ends after a specified match time or condition. Useful for
CommandGroups.
"""
_condition: Callable[[], bool]
@overload
def __init__(self, condition: Callable[[], bool]):
"""
Creates a new WaitUntilCommand that ends after a given condition becomes true.
:param condition: the condition to determine when to end
"""
...
@overload
def __init__(self, time: units.seconds):
"""
Creates a new WaitUntilCommand that ends after a given match time.
NOTE: The match timer used for this command is UNOFFICIAL. Using this command does NOT
guarantee that the time at which the action is performed will be judged to be legal by the
referees. When in doubt, add a safety factor or time the action manually.
:param time: the match time at which to end, in seconds
"""
...
def __init__(self, *args, **kwargs):
super().__init__()
def init_condition(condition: Callable[[], bool]) -> None:
assert callable(condition)
self._condition = condition
def init_time(time: float) -> None:
self._condition = lambda: Timer.getMatchTime() - time > 0
num_args = len(args) + len(kwargs)
if num_args == 1 and len(kwargs) == 1:
if "condition" in kwargs:
return init_condition(kwargs["condition"])
elif "time" in kwargs:
return init_time(kwargs["time"])
elif num_args == 1 and len(args) == 1:
if isinstance(args[0], float):
return init_time(args[0])
elif callable(args[0]):
return init_condition(args[0])
raise TypeError(
f"""
TypeError: WaitUntilCommand(): incompatible function arguments. The following argument types are supported:
1. (self: WaitUntilCommand, condition: () -> bool)
2. (self: WaitUntilCommand, time: wpimath.units.seconds)
Invoked with: {format_args_kwargs(self, *args, **kwargs)}
"""
)
def isFinished(self) -> bool:
return self._condition()
def runsWhenDisabled(self) -> bool:
return True

90
commandsv2/src/main/python/commands2/wrappercommand.py Normal file
View File

@@ -0,0 +1,90 @@
# validated: 2024-01-20 DS ad0859a8c9ec WrapperCommand.java
from __future__ import annotations
from typing import Set
from .command import Command, InterruptionBehavior
from .commandscheduler import CommandScheduler
class WrapperCommand(Command):
"""
A class used internally to wrap commands while overriding a specific method; all other methods
will call through to the wrapped command.
The rules for command compositions apply: command instances that are passed to it cannot be
added to any other composition or scheduled individually, and the composition requires all
subsystems its components require.
"""
def __init__(self, command: Command):
"""
Wrap a command.
:param command: the command being wrapped. Trying to directly schedule this
command or add it to a composition will throw an exception.
"""
super().__init__()
CommandScheduler.getInstance().registerComposedCommands([command])
self._command = command
self.setName(self._command.getName())
def initialize(self):
"""
The initial subroutine of a command. Called once when the command is initially scheduled.
"""
self._command.initialize()
def execute(self):
"""
The main body of a command. Called repeatedly while the command is scheduled.
"""
self._command.execute()
def end(self, interrupted: bool):
"""
The action to take when the command ends. Called when either the command finishes normally, or
when it interrupted/canceled.
Do not schedule commands here that share requirements with this command. Use
:func:`commands2.Command.andThen` instead.
:param interrupted: whether the command was interrupted/canceled
"""
self._command.end(interrupted)
def isFinished(self) -> bool:
"""
Whether the command has finished. Once a command finishes, the scheduler will call its end()
method and un-schedule it.
:returns: whether the command has finished.
"""
return self._command.isFinished()
def getRequirements(self) -> Set:
"""
Specifies the set of subsystems used by this command. Two commands cannot use the same
subsystem at the same time. If the command is scheduled as interruptible and another command is
scheduled that shares a requirement, the command will be interrupted. Else, the command will
not be scheduled. If no subsystems are required, return an empty set.
Note: it is recommended that user implementations contain the requirements as a field, and
return that field here, rather than allocating a new set every time this is called.
:returns: the set of subsystems that are required
"""
return self._command.getRequirements()
def runsWhenDisabled(self) -> bool:
"""
Whether the given command should run when the robot is disabled. Override to return true if the
command should run when disabled.
:returns: whether the command should run when the robot is disabled
"""
return self._command.runsWhenDisabled()
def getInterruptionBehavior(self) -> InterruptionBehavior:
return self._command.getInterruptionBehavior()