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

Project import generated by Copybara.

GitOrigin-RevId: 715c8e8372d936f447f2937aab6b1a22dc619126

.styleguide

@@ -68,6 +68,8 @@ generatedFileExclude {
 
   apriltag/src/main/python/
   apriltag/src/test/python/
+  commandsv2/src/main/python/
+  commandsv2/src/test/python/
   datalog/src/main/python/
   datalog/src/test/python/
   hal/src/main/python/

commandsv2/src/main/python/commands2/__init__.py

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

commandsv2/src/main/python/commands2/button/__init__.py

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

commandsv2/src/main/python/commands2/button/commandgenerichid.py

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

commandsv2/src/main/python/commands2/button/commandjoystick.py

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

commandsv2/src/main/python/commands2/button/commandps4controller.py

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

commandsv2/src/main/python/commands2/button/commandxboxcontroller.py

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

commandsv2/src/main/python/commands2/button/joystickbutton.py

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

commandsv2/src/main/python/commands2/button/networkbutton.py

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

commandsv2/src/main/python/commands2/button/povbutton.py

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

commandsv2/src/main/python/commands2/button/trigger.py

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

commandsv2/src/main/python/commands2/cmd.py

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

commandsv2/src/main/python/commands2/command.py

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

commandsv2/src/main/python/commands2/commandscheduler.py

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

commandsv2/src/main/python/commands2/conditionalcommand.py

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

commandsv2/src/main/python/commands2/deferredcommand.py

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

commandsv2/src/main/python/commands2/exceptions.py

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

commandsv2/src/main/python/commands2/functionalcommand.py

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

commandsv2/src/main/python/commands2/instantcommand.py

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

commandsv2/src/main/python/commands2/notifiercommand.py

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

commandsv2/src/main/python/commands2/parallelcommandgroup.py

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

commandsv2/src/main/python/commands2/paralleldeadlinegroup.py

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

commandsv2/src/main/python/commands2/parallelracegroup.py

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

commandsv2/src/main/python/commands2/pidcommand.py

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

commandsv2/src/main/python/commands2/pidsubsystem.py

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

commandsv2/src/main/python/commands2/printcommand.py

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

commandsv2/src/main/python/commands2/profiledpidcommand.py

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

commandsv2/src/main/python/commands2/profiledpidsubsystem.py

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

commandsv2/src/main/python/commands2/proxycommand.py

@@ -0,0 +1,126 @@
+# validated: 2024-01-19 DS 192a28af4731 ProxyCommand.java
+from __future__ import annotations
+
+from typing import Callable, overload
+
+from wpiutil import SendableBuilder
+
+from .command import Command
+from .util import format_args_kwargs
+import warnings
+
+
+class ProxyCommand(Command):
+    """
+    Schedules a given command when this command is initialized and ends when it ends, but does not
+    directly run it. Use this for including a command in a composition without adding its
+    requirements, but only if you know what you are doing. If you are unsure, see
+    `the WPILib docs <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,
+        )

commandsv2/src/main/python/commands2/repeatcommand.py

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

commandsv2/src/main/python/commands2/runcommand.py

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

commandsv2/src/main/python/commands2/schedulecommand.py

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

commandsv2/src/main/python/commands2/selectcommand.py

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

commandsv2/src/main/python/commands2/sequentialcommandgroup.py

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

commandsv2/src/main/python/commands2/startendcommand.py

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

commandsv2/src/main/python/commands2/subsystem.py

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

commandsv2/src/main/python/commands2/swervecontrollercommand.py

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

commandsv2/src/main/python/commands2/sysid/__init__.py

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

commandsv2/src/main/python/commands2/sysid/sysidroutine.py

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

commandsv2/src/main/python/commands2/timedcommandrobot.py

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

commandsv2/src/main/python/commands2/trapezoidprofilecommand.py

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

commandsv2/src/main/python/commands2/trapezoidprofilesubsystem.py

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

commandsv2/src/main/python/commands2/typing.py

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

commandsv2/src/main/python/commands2/util.py

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

commandsv2/src/main/python/commands2/waitcommand.py

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

commandsv2/src/main/python/commands2/waituntilcommand.py

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

commandsv2/src/main/python/commands2/wrappercommand.py

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

commandsv2/src/test/python/compositiontestbase.py

@@ -0,0 +1,166 @@
+from typing import Generic, TypeVar
+
+import commands2
+import pytest
+
+# T = TypeVar("T", bound=commands2.Command)
+# T = commands2.Command
+
+from util import *
+
+
+class SingleCompositionTestBase:
+    def composeSingle(self, member: commands2.Command):
+        raise NotImplementedError
+
+    @pytest.mark.parametrize(
+        "interruptionBehavior",
+        [
+            commands2.InterruptionBehavior.kCancelSelf,
+            commands2.InterruptionBehavior.kCancelIncoming,
+        ],
+    )
+    def test_interruptible(self, interruptionBehavior: commands2.InterruptionBehavior):
+        command = self.composeSingle(
+            commands2.WaitUntilCommand(lambda: False).withInterruptBehavior(
+                interruptionBehavior
+            )
+        )
+        assert command.getInterruptionBehavior() == interruptionBehavior
+
+    @pytest.mark.parametrize("runsWhenDisabled", [True, False])
+    def test_runWhenDisabled(self, runsWhenDisabled: bool):
+        command = self.composeSingle(
+            commands2.WaitUntilCommand(lambda: False).ignoringDisable(runsWhenDisabled)
+        )
+        assert command.runsWhenDisabled() == runsWhenDisabled
+
+    def test_command_in_other_composition(self):
+        command = commands2.InstantCommand()
+        wrapped = commands2.WrapperCommand(command)
+        with pytest.raises(commands2.IllegalCommandUse):
+            self.composeSingle(command)
+
+    def test_command_in_multiple_compositions(self):
+        command = commands2.InstantCommand()
+        self.composeSingle(command)
+        with pytest.raises(commands2.IllegalCommandUse):
+            self.composeSingle(command)
+
+    def test_compose_then_schedule(self, scheduler: commands2.CommandScheduler):
+        command = commands2.InstantCommand()
+        self.composeSingle(command)
+        with pytest.raises(commands2.IllegalCommandUse):
+            scheduler.schedule(command)
+
+    def test_schedule_then_compose(self, scheduler: commands2.CommandScheduler):
+        command = commands2.RunCommand(lambda: None)
+        scheduler.schedule(command)
+        with pytest.raises(commands2.IllegalCommandUse):
+            self.composeSingle(command)
+
+
+class MultiCompositionTestBase(SingleCompositionTestBase):
+    def compose(self, *members: commands2.Command):
+        raise NotImplementedError
+
+    def composeSingle(self, member: commands2.Command):
+        return self.compose(member)
+
+    @pytest.mark.parametrize(
+        "expected,command1,command2,command3",
+        [
+            pytest.param(
+                commands2.InterruptionBehavior.kCancelSelf,
+                commands2.WaitUntilCommand(lambda: False).withInterruptBehavior(
+                    commands2.InterruptionBehavior.kCancelSelf
+                ),
+                commands2.WaitUntilCommand(lambda: False).withInterruptBehavior(
+                    commands2.InterruptionBehavior.kCancelSelf
+                ),
+                commands2.WaitUntilCommand(lambda: False).withInterruptBehavior(
+                    commands2.InterruptionBehavior.kCancelSelf
+                ),
+                id="AllCancelSelf",
+            ),
+            pytest.param(
+                commands2.InterruptionBehavior.kCancelIncoming,
+                commands2.WaitUntilCommand(lambda: False).withInterruptBehavior(
+                    commands2.InterruptionBehavior.kCancelIncoming
+                ),
+                commands2.WaitUntilCommand(lambda: False).withInterruptBehavior(
+                    commands2.InterruptionBehavior.kCancelIncoming
+                ),
+                commands2.WaitUntilCommand(lambda: False).withInterruptBehavior(
+                    commands2.InterruptionBehavior.kCancelIncoming
+                ),
+                id="AllCancelIncoming",
+            ),
+            pytest.param(
+                commands2.InterruptionBehavior.kCancelSelf,
+                commands2.WaitUntilCommand(lambda: False).withInterruptBehavior(
+                    commands2.InterruptionBehavior.kCancelSelf
+                ),
+                commands2.WaitUntilCommand(lambda: False).withInterruptBehavior(
+                    commands2.InterruptionBehavior.kCancelSelf
+                ),
+                commands2.WaitUntilCommand(lambda: False).withInterruptBehavior(
+                    commands2.InterruptionBehavior.kCancelIncoming
+                ),
+                id="TwoCancelSelfOneIncoming",
+            ),
+            pytest.param(
+                commands2.InterruptionBehavior.kCancelSelf,
+                commands2.WaitUntilCommand(lambda: False).withInterruptBehavior(
+                    commands2.InterruptionBehavior.kCancelIncoming
+                ),
+                commands2.WaitUntilCommand(lambda: False).withInterruptBehavior(
+                    commands2.InterruptionBehavior.kCancelIncoming
+                ),
+                commands2.WaitUntilCommand(lambda: False).withInterruptBehavior(
+                    commands2.InterruptionBehavior.kCancelSelf
+                ),
+                id="TwoCancelIncomingOneSelf",
+            ),
+        ],
+    )
+    def test_interruptible(self, expected, command1, command2, command3):
+        command = self.compose(command1, command2, command3)
+        assert command.getInterruptionBehavior() == expected
+
+    @pytest.mark.parametrize(
+        "expected,command1,command2,command3",
+        [
+            pytest.param(
+                False,
+                commands2.WaitUntilCommand(lambda: False).ignoringDisable(False),
+                commands2.WaitUntilCommand(lambda: False).ignoringDisable(False),
+                commands2.WaitUntilCommand(lambda: False).ignoringDisable(False),
+                id="AllFalse",
+            ),
+            pytest.param(
+                True,
+                commands2.WaitUntilCommand(lambda: False).ignoringDisable(True),
+                commands2.WaitUntilCommand(lambda: False).ignoringDisable(True),
+                commands2.WaitUntilCommand(lambda: False).ignoringDisable(True),
+                id="AllTrue",
+            ),
+            pytest.param(
+                False,
+                commands2.WaitUntilCommand(lambda: False).ignoringDisable(True),
+                commands2.WaitUntilCommand(lambda: False).ignoringDisable(True),
+                commands2.WaitUntilCommand(lambda: False).ignoringDisable(False),
+                id="TwoTrueOneFalse",
+            ),
+            pytest.param(
+                False,
+                commands2.WaitUntilCommand(lambda: False).ignoringDisable(False),
+                commands2.WaitUntilCommand(lambda: False).ignoringDisable(False),
+                commands2.WaitUntilCommand(lambda: False).ignoringDisable(True),
+                id="TwoFalseOneTrue",
+            ),
+        ],
+    )
+    def test_runWhenDisabled(self, expected, command1, command2, command3):
+        command = self.compose(command1, command2, command3)
+        assert command.runsWhenDisabled() == expected

commandsv2/src/test/python/conftest.py

@@ -0,0 +1,20 @@
+import commands2
+import pytest
+from ntcore import NetworkTableInstance
+from wpilib.simulation import DriverStationSim
+
+
+@pytest.fixture(autouse=True)
+def scheduler():
+    commands2.CommandScheduler.resetInstance()
+    DriverStationSim.setEnabled(True)
+    DriverStationSim.notifyNewData()
+    return commands2.CommandScheduler.getInstance()
+
+
+@pytest.fixture()
+def nt_instance():
+    inst = NetworkTableInstance.create()
+    inst.startLocal()
+    yield inst
+    inst.stopLocal()

commandsv2/src/test/python/test_command_decorators.py

@@ -0,0 +1,222 @@
+from typing import TYPE_CHECKING
+
+from util import *  # type: ignore
+
+if TYPE_CHECKING:
+    from .util import *
+
+import commands2
+import pytest
+
+
+def test_timeout(scheduler: commands2.CommandScheduler):
+    with ManualSimTime() as sim:
+        command1 = commands2.WaitCommand(1)
+        timeout = command1.withTimeout(2)
+        scheduler.schedule(timeout)
+        scheduler.run()
+        assert not command1.isScheduled()
+        assert timeout.isScheduled()
+        sim.step(3)
+        scheduler.run()
+        assert not timeout.isScheduled()
+
+
+def test_until(scheduler: commands2.CommandScheduler):
+    condition = OOBoolean(False)
+    command = commands2.WaitCommand(10).until(condition)
+    scheduler.schedule(command)
+    scheduler.run()
+    assert command.isScheduled()
+    condition.set(True)
+    scheduler.run()
+    assert not command.isScheduled()
+
+
+def test_only_while(scheduler: commands2.CommandScheduler):
+    condition = OOBoolean(True)
+    command = commands2.WaitCommand(10).onlyWhile(condition)
+    scheduler.schedule(command)
+    scheduler.run()
+    assert command.isScheduled()
+    condition.set(False)
+    scheduler.run()
+    assert not command.isScheduled()
+
+
+def test_ignoringDisable(scheduler: commands2.CommandScheduler):
+    command = commands2.RunCommand(lambda: None).ignoringDisable(True)
+    DriverStationSim.setEnabled(False)
+    DriverStationSim.notifyNewData()
+    scheduler.schedule(command)
+    scheduler.run()
+    assert command.isScheduled()
+
+
+def test_beforeStarting(scheduler: commands2.CommandScheduler):
+    condition = OOBoolean(False)
+    condition.set(False)
+    command = commands2.InstantCommand()
+    scheduler.schedule(
+        command.beforeStarting(commands2.InstantCommand(lambda: condition.set(True)))
+    )
+    assert condition == True
+
+
+@pytest.mark.skip
+def test_andThenLambda(scheduler: commands2.CommandScheduler): ...
+
+
+def test_andThen(scheduler: commands2.CommandScheduler):
+    condition = OOBoolean(False)
+    condition.set(False)
+    command1 = commands2.InstantCommand()
+    command2 = commands2.InstantCommand(lambda: condition.set(True))
+    scheduler.schedule(command1.andThen(command2))
+    assert condition == False
+    scheduler.run()
+    assert condition == True
+
+
+def test_deadlineWith(scheduler: commands2.CommandScheduler):
+    condition = OOBoolean(False)
+    condition.set(False)
+
+    dictator = commands2.WaitUntilCommand(condition)
+    endsBefore = commands2.InstantCommand()
+    endsAfter = commands2.WaitUntilCommand(lambda: False)
+
+    group = dictator.deadlineWith(endsBefore, endsAfter)
+
+    scheduler.schedule(group)
+    scheduler.run()
+    assert group.isScheduled()
+    condition.set(True)
+    scheduler.run()
+    assert not group.isScheduled()
+
+
+def test_deadlineFor(scheduler: commands2.CommandScheduler):
+    condition = OOBoolean(False)
+    condition.set(False)
+
+    dictator = commands2.WaitUntilCommand(condition)
+    endsBefore = commands2.InstantCommand()
+    endsAfter = commands2.WaitUntilCommand(lambda: False)
+
+    group = dictator.deadlineFor(endsBefore, endsAfter)
+
+    scheduler.schedule(group)
+    scheduler.run()
+    assert group.isScheduled()
+    condition.set(True)
+    scheduler.run()
+    assert not group.isScheduled()
+
+
+def test_alongWith(scheduler: commands2.CommandScheduler):
+    condition = OOBoolean()
+    condition.set(False)
+
+    command1 = commands2.WaitUntilCommand(condition)
+    command2 = commands2.InstantCommand()
+
+    group = command1.alongWith(command2)
+
+    scheduler.schedule(group)
+    scheduler.run()
+    assert group.isScheduled()
+    condition.set(True)
+    scheduler.run()
+    assert not group.isScheduled()
+
+
+def test_raceWith(scheduler: commands2.CommandScheduler):
+    command1 = commands2.WaitUntilCommand(lambda: False)
+    command2 = commands2.InstantCommand()
+
+    group = command1.raceWith(command2)
+
+    scheduler.schedule(group)
+    scheduler.run()
+    assert not group.isScheduled()
+
+
+def test_unless(scheduler: commands2.CommandScheduler):
+    unlessCondition = OOBoolean(True)
+    hasRunCondition = OOBoolean(False)
+
+    command = commands2.InstantCommand(lambda: hasRunCondition.set(True)).unless(
+        unlessCondition
+    )
+
+    scheduler.schedule(command)
+    scheduler.run()
+    assert hasRunCondition == False
+    unlessCondition.set(False)
+    scheduler.schedule(command)
+    scheduler.run()
+    assert hasRunCondition == True
+
+
+def test_onlyIf(scheduler: commands2.CommandScheduler):
+    onlyIfCondition = OOBoolean(False)
+    hasRunCondition = OOBoolean(False)
+
+    command = commands2.InstantCommand(lambda: hasRunCondition.set(True)).onlyIf(
+        onlyIfCondition
+    )
+
+    scheduler.schedule(command)
+    scheduler.run()
+    assert hasRunCondition == False
+    onlyIfCondition.set(True)
+    scheduler.schedule(command)
+    scheduler.run()
+    assert hasRunCondition == True
+
+
+def test_finallyDo(scheduler: commands2.CommandScheduler):
+    first = OOInteger(0)
+    second = OOInteger(0)
+
+    command = commands2.FunctionalCommand(
+        lambda: None,
+        lambda: None,
+        lambda interrupted: first.incrementAndGet() if not interrupted else None,
+        lambda: True,
+    ).finallyDo(lambda interrupted: second.addAndGet(1 + first()))
+
+    scheduler.schedule(command)
+    assert first == 0
+    assert second == 0
+    scheduler.run()
+    assert first == 1
+    assert second == 2
+
+
+def test_handleInterrupt(scheduler: commands2.CommandScheduler):
+    first = OOInteger(0)
+    second = OOInteger(0)
+
+    command = commands2.FunctionalCommand(
+        lambda: None,
+        lambda: None,
+        lambda interrupted: first.incrementAndGet() if interrupted else None,
+        lambda: False,
+    ).handleInterrupt(lambda: second.addAndGet(1 + first()))
+
+    scheduler.schedule(command)
+    scheduler.run()
+    assert first == 0
+    assert second == 0
+    scheduler.cancel(command)
+    assert first == 1
+    assert second == 2
+
+
+def test_withName(scheduler: commands2.CommandScheduler):
+    command = commands2.InstantCommand()
+    name = "Named"
+    named = command.withName(name)
+    assert named.getName() == name

commandsv2/src/test/python/test_command_requirements.py

@@ -0,0 +1,58 @@
+from typing import TYPE_CHECKING
+
+import commands2
+from util import *  # type: ignore
+
+if TYPE_CHECKING:
+    from .util import *
+
+import pytest
+
+
+def test_requirementInterrupt(scheduler: commands2.CommandScheduler):
+    requirement = commands2.Subsystem()
+    interrupted = commands2.Command()
+    interrupted.addRequirements(requirement)
+    interrupter = commands2.Command()
+    interrupter.addRequirements(requirement)
+    start_spying_on(interrupted)
+    start_spying_on(interrupter)
+
+    scheduler.schedule(interrupted)
+    scheduler.run()
+    scheduler.schedule(interrupter)
+    scheduler.run()
+
+    verify(interrupted).initialize()
+    verify(interrupted).execute()
+    verify(interrupted).end(True)
+
+    verify(interrupter).initialize()
+    verify(interrupter).execute()
+
+    assert not interrupted.isScheduled()
+    assert interrupter.isScheduled()
+
+
+def test_requirementUninterruptible(scheduler: commands2.CommandScheduler):
+    requirement = commands2.Subsystem()
+    notInterrupted = commands2.RunCommand(
+        lambda: None, requirement
+    ).withInterruptBehavior(commands2.InterruptionBehavior.kCancelIncoming)
+    interrupter = commands2.Command()
+    interrupter.addRequirements(requirement)
+    start_spying_on(notInterrupted)
+
+    scheduler.schedule(notInterrupted)
+    scheduler.schedule(interrupter)
+
+    assert scheduler.isScheduled(notInterrupted)
+    assert not scheduler.isScheduled(interrupter)
+
+
+def test_defaultCommandRequirementError(scheduler: commands2.CommandScheduler):
+    system = commands2.Subsystem()
+    missingRequirement = commands2.WaitUntilCommand(lambda: False)
+
+    with pytest.raises(commands2.IllegalCommandUse):
+        scheduler.setDefaultCommand(system, missingRequirement)

commandsv2/src/test/python/test_command_schedule.py

@@ -0,0 +1,90 @@
+from typing import TYPE_CHECKING
+
+import commands2
+from util import *  # type: ignore
+
+if TYPE_CHECKING:
+    from .util import *
+
+import pytest
+
+
+def test_instantSchedule(scheduler: commands2.CommandScheduler):
+    command = commands2.Command()
+    command.isFinished = lambda: True
+    start_spying_on(command)
+
+    scheduler.schedule(command)
+    assert scheduler.isScheduled(command)
+    verify(command).initialize()
+
+    scheduler.run()
+
+    verify(command).execute()
+    verify(command).end(False)
+    assert not scheduler.isScheduled(command)
+
+
+def test_singleIterationSchedule(scheduler: commands2.CommandScheduler):
+    command = commands2.Command()
+    start_spying_on(command)
+
+    scheduler.schedule(command)
+    assert scheduler.isScheduled(command)
+
+    scheduler.run()
+    command.isFinished = lambda: True
+    scheduler.run()
+
+    verify(command).initialize()
+    verify(command, times(2)).execute()
+    verify(command).end(False)
+    assert not scheduler.isScheduled(command)
+
+
+def test_multiSchedule(scheduler: commands2.CommandScheduler):
+    command1 = commands2.Command()
+    command2 = commands2.Command()
+    command3 = commands2.Command()
+
+    scheduler.schedule(command1, command2, command3)
+    assert scheduler.isScheduled(command1, command2, command3)
+    scheduler.run()
+    assert scheduler.isScheduled(command1, command2, command3)
+
+    command1.isFinished = lambda: True
+    scheduler.run()
+    assert scheduler.isScheduled(command2, command3)
+    assert not scheduler.isScheduled(command1)
+
+    command2.isFinished = lambda: True
+    scheduler.run()
+    assert scheduler.isScheduled(command3)
+    assert not scheduler.isScheduled(command1, command2)
+
+    command3.isFinished = lambda: True
+    scheduler.run()
+    assert not scheduler.isScheduled(command1, command2, command3)
+
+
+def test_schedulerCancel(scheduler: commands2.CommandScheduler):
+    command = commands2.Command()
+    start_spying_on(command)
+
+    scheduler.schedule(command)
+
+    scheduler.run()
+    scheduler.cancel(command)
+    scheduler.run()
+
+    verify(command).execute()
+    verify(command).end(True)
+    verify(command, never()).end(False)
+
+    assert not scheduler.isScheduled(command)
+
+
+def test_notScheduledCancel(scheduler: commands2.CommandScheduler):
+    command = commands2.Command()
+
+    scheduler.cancel(command)

commandsv2/src/test/python/test_commandgroup_error.py

@@ -0,0 +1,37 @@
+from typing import TYPE_CHECKING
+
+import commands2
+from util import *  # type: ignore
+
+if TYPE_CHECKING:
+    from .util import *
+
+import pytest
+
+
+def test_commandInMultipleGroups():
+    command1 = commands2.Command()
+    command2 = commands2.Command()
+
+    commands2.ParallelCommandGroup(command1, command2)
+    with pytest.raises(commands2.IllegalCommandUse):
+        commands2.ParallelCommandGroup(command1, command2)
+
+
+def test_commandInGroupExternallyScheduled(scheduler: commands2.CommandScheduler):
+    command1 = commands2.Command()
+    command2 = commands2.Command()
+
+    commands2.ParallelCommandGroup(command1, command2)
+
+    with pytest.raises(commands2.IllegalCommandUse):
+        scheduler.schedule(command1)
+
+
+def test_redecoratedCommandError(scheduler: commands2.CommandScheduler):
+    command = commands2.InstantCommand()
+    command.withTimeout(10).until(lambda: False)
+    with pytest.raises(commands2.IllegalCommandUse):
+        command.withTimeout(10)
+    scheduler.removeComposedCommand(command)
+    command.withTimeout(10)

commandsv2/src/test/python/test_conditional_command.py

@@ -0,0 +1,55 @@
+from typing import TYPE_CHECKING
+
+import commands2
+from util import *  # type: ignore
+
+if TYPE_CHECKING:
+    from .util import *
+
+import pytest
+
+
+def test_conditionalCommand(scheduler: commands2.CommandScheduler):
+    command1 = commands2.Command()
+    command1.isFinished = lambda: True
+    command2 = commands2.Command()
+
+    start_spying_on(command1)
+    start_spying_on(command2)
+
+    conditionalCommand = commands2.ConditionalCommand(command1, command2, lambda: True)
+
+    scheduler.schedule(conditionalCommand)
+    scheduler.run()
+
+    verify(command1).initialize()
+    verify(command1).execute()
+    verify(command1).end(False)
+
+    verify(command2, never()).initialize()
+    verify(command2, never()).execute()
+    verify(command2, never()).end(False)
+
+
+def test_conditionalCommandRequirement(scheduler: commands2.CommandScheduler):
+    system1 = commands2.Subsystem()
+    system2 = commands2.Subsystem()
+    system3 = commands2.Subsystem()
+
+    command1 = commands2.Command()
+    command1.addRequirements(system1, system2)
+    command2 = commands2.Command()
+    command2.addRequirements(system3)
+
+    start_spying_on(command1)
+    start_spying_on(command2)
+
+    conditionalCommand = commands2.ConditionalCommand(command1, command2, lambda: True)
+
+    scheduler.schedule(conditionalCommand)
+    scheduler.schedule(commands2.InstantCommand(lambda: None, system3))
+
+    assert not scheduler.isScheduled(conditionalCommand)
+
+    assert command1.end.called_with(True)
+    assert not command2.end.called_with(True)

commandsv2/src/test/python/test_default_command.py

@@ -0,0 +1,73 @@
+from typing import TYPE_CHECKING
+
+import commands2
+from util import *  # type: ignore
+
+if TYPE_CHECKING:
+    from .util import *
+
+import pytest
+
+
+def test_defaultCommandSchedule(scheduler: commands2.CommandScheduler):
+    hasDefaultCommand = commands2.Subsystem()
+
+    defaultCommand = commands2.Command()
+    defaultCommand.addRequirements(hasDefaultCommand)
+
+    scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand)
+    scheduler.run()
+
+    assert scheduler.isScheduled(defaultCommand)
+
+
+def test_defaultCommandInterruptResume(scheduler: commands2.CommandScheduler):
+    hasDefaultCommand = commands2.Subsystem()
+
+    defaultCommand = commands2.Command()
+    defaultCommand.addRequirements(hasDefaultCommand)
+
+    interrupter = commands2.Command()
+    interrupter.addRequirements(hasDefaultCommand)
+
+    scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand)
+    scheduler.run()
+    scheduler.schedule(interrupter)
+
+    assert not scheduler.isScheduled(defaultCommand)
+    assert scheduler.isScheduled(interrupter)
+
+    scheduler.cancel(interrupter)
+    scheduler.run()
+
+    assert scheduler.isScheduled(defaultCommand)
+    assert not scheduler.isScheduled(interrupter)
+
+
+def test_defaultCommandDisableResume(scheduler: commands2.CommandScheduler):
+    hasDefaultCommand = commands2.Subsystem()
+
+    defaultCommand = commands2.Command()
+    defaultCommand.addRequirements(hasDefaultCommand)
+    defaultCommand.runsWhenDisabled = lambda: False
+
+    start_spying_on(defaultCommand)
+
+    scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand)
+    scheduler.run()
+
+    assert scheduler.isScheduled(defaultCommand)
+
+    DriverStationSim.setEnabled(False)
+    DriverStationSim.notifyNewData()
+    scheduler.run()
+
+    assert not scheduler.isScheduled(defaultCommand)
+
+    DriverStationSim.setEnabled(True)
+    DriverStationSim.notifyNewData()
+    scheduler.run()
+
+    assert scheduler.isScheduled(defaultCommand)
+
+    assert defaultCommand.end.called_with(True)

commandsv2/src/test/python/test_deferred_command.py

@@ -0,0 +1,63 @@
+import commands2
+
+from util import *  # type: ignore
+
+
+def test_deferred_functions(scheduler: commands2.CommandScheduler):
+    inner_command = commands2.Command()
+    command = commands2.DeferredCommand(lambda: inner_command)
+
+    start_spying_on(inner_command)
+    start_spying_on(command)
+
+    command.initialize()
+    verify(inner_command).initialize()
+
+    command.execute()
+    verify(inner_command).execute()
+
+    assert not command.isFinished()
+    verify(inner_command).isFinished()
+
+    inner_command.isFinished = lambda: True
+    assert command.isFinished()
+    verify(inner_command, times=times(2)).isFinished()
+
+    command.end(False)
+    verify(inner_command).end(False)
+
+
+def test_deferred_supplier_only_called_during_init(
+    scheduler: commands2.CommandScheduler,
+):
+    supplier_called = 0
+
+    def supplier() -> commands2.Command:
+        nonlocal supplier_called
+        supplier_called += 1
+        return commands2.Command()
+
+    command = commands2.DeferredCommand(supplier)
+    assert supplier_called == 0
+
+    scheduler.schedule(command)
+    assert supplier_called == 1
+    scheduler.run()
+
+    scheduler.schedule(command)
+    assert supplier_called == 1
+
+
+def test_deferred_requirements(scheduler: commands2.CommandScheduler):
+    subsystem = commands2.Subsystem()
+    command = commands2.DeferredCommand(lambda: commands2.Command(), subsystem)
+
+    assert subsystem in command.getRequirements()
+
+
+def test_deferred_null_command(scheduler: commands2.CommandScheduler):
+    command = commands2.DeferredCommand(lambda: None)  # type: ignore
+    command.initialize()
+    command.execute()
+    command.isFinished()
+    command.end(False)

commandsv2/src/test/python/test_functional_command.py

@@ -0,0 +1,36 @@
+from typing import TYPE_CHECKING
+
+import commands2
+from util import *  # type: ignore
+
+if TYPE_CHECKING:
+    from .util import *
+
+import pytest
+
+
+def test_functionalCommandSchedule(scheduler: commands2.CommandScheduler):
+    cond1 = OOBoolean()
+    cond2 = OOBoolean()
+    cond3 = OOBoolean()
+    cond4 = OOBoolean()
+
+    command = commands2.FunctionalCommand(
+        lambda: cond1.set(True),
+        lambda: cond2.set(True),
+        lambda _: cond3.set(True),
+        lambda: cond4.get(),
+    )
+
+    scheduler.schedule(command)
+    scheduler.run()
+
+    assert scheduler.isScheduled(command)
+
+    cond4.set(True)
+    scheduler.run()
+
+    assert not scheduler.isScheduled(command)
+    assert cond1
+    assert cond2
+    assert cond3

commandsv2/src/test/python/test_instant_command.py

@@ -0,0 +1,21 @@
+from typing import TYPE_CHECKING
+
+import commands2
+from util import *  # type: ignore
+
+if TYPE_CHECKING:
+    from .util import *
+
+import pytest
+
+
+def test_instantCommandSchedule(scheduler: commands2.CommandScheduler):
+    cond = OOBoolean()
+
+    command = commands2.InstantCommand(lambda: cond.set(True))
+
+    scheduler.schedule(command)
+    scheduler.run()
+
+    assert cond
+    assert not scheduler.isScheduled(command)

commandsv2/src/test/python/test_networkbutton.py

@@ -0,0 +1,29 @@
+from typing import TYPE_CHECKING
+
+import commands2
+from ntcore import NetworkTableInstance
+from util import *  # type: ignore
+
+if TYPE_CHECKING:
+    from .util import *
+
+
+def test_networkbutton(
+    scheduler: commands2.CommandScheduler, nt_instance: NetworkTableInstance
+):
+    # command = commands2.Command()
+    command = commands2.Command()
+    start_spying_on(command)
+
+    pub = nt_instance.getTable("TestTable").getBooleanTopic("Test").publish()
+
+    button = commands2.button.NetworkButton(nt_instance, "TestTable", "Test")
+
+    pub.set(False)
+    button.onTrue(command)
+    scheduler.run()
+    assert command.schedule.times_called == 0
+    pub.set(True)
+    scheduler.run()
+    scheduler.run()
+    verify(command).schedule()

commandsv2/src/test/python/test_notifier_command.py

@@ -0,0 +1,22 @@
+from typing import TYPE_CHECKING
+
+import commands2
+from util import *  # type: ignore
+
+if TYPE_CHECKING:
+    from .util import *
+
+import pytest
+
+
+def test_notifierCommandScheduler(scheduler: commands2.CommandScheduler):
+    with ManualSimTime() as sim:
+        counter = OOInteger(0)
+        command = commands2.NotifierCommand(counter.incrementAndGet, 0.01)
+
+        scheduler.schedule(command)
+        for i in range(5):
+            sim.step(0.005)
+        scheduler.cancel(command)
+
+        assert counter == 2

commandsv2/src/test/python/test_parallelcommandgroup.py

@@ -0,0 +1,117 @@
+from typing import TYPE_CHECKING
+
+import commands2
+from compositiontestbase import MultiCompositionTestBase  # type: ignore
+from util import *  # type: ignore
+
+# from tests.compositiontestbase import T
+
+if TYPE_CHECKING:
+    from .util import *
+    from .compositiontestbase import MultiCompositionTestBase
+
+import pytest
+
+
+class TestParallelCommandGroupComposition(MultiCompositionTestBase):
+    def compose(self, *members: commands2.Command):
+        return commands2.ParallelCommandGroup(*members)
+
+
+def test_parallelGroupSchedule(scheduler: commands2.CommandScheduler):
+    command1 = commands2.Command()
+    command2 = commands2.Command()
+
+    start_spying_on(command1)
+    start_spying_on(command2)
+
+    group = commands2.ParallelCommandGroup(command1, command2)
+
+    scheduler.schedule(group)
+
+    verify(command1).initialize()
+    verify(command2).initialize()
+
+    command1.isFinished = lambda: True
+    scheduler.run()
+    command2.isFinished = lambda: True
+    scheduler.run()
+
+    verify(command1).execute()
+    verify(command1).end(False)
+    verify(command2, times(2)).execute()
+    verify(command2).end(False)
+
+    assert not scheduler.isScheduled(group)
+
+
+def test_parallelGroupInterrupt(scheduler: commands2.CommandScheduler):
+    command1 = commands2.Command()
+    command2 = commands2.Command()
+
+    start_spying_on(command1)
+    start_spying_on(command2)
+
+    group = commands2.ParallelCommandGroup(command1, command2)
+
+    scheduler.schedule(group)
+
+    command1.isFinished = lambda: True
+    scheduler.run()
+    scheduler.run()
+    scheduler.cancel(group)
+
+    verify(command1).execute()
+    verify(command1).end(False)
+    verify(command1, never()).end(True)
+
+    verify(command2, times(2)).execute()
+    verify(command2, never()).end(False)
+    verify(command2).end(True)
+
+    assert not scheduler.isScheduled(group)
+
+
+def test_notScheduledCancel(scheduler: commands2.CommandScheduler):
+    command1 = commands2.Command()
+    command2 = commands2.Command()
+
+    group = commands2.ParallelCommandGroup(command1, command2)
+
+    scheduler.cancel(group)
+
+
+def test_parallelGroupRequirement(scheduler: commands2.CommandScheduler):
+    system1 = commands2.Subsystem()
+    system2 = commands2.Subsystem()
+    system3 = commands2.Subsystem()
+    system4 = commands2.Subsystem()
+
+    command1 = commands2.Command()
+    command1.addRequirements(system1, system2)
+    command2 = commands2.Command()
+    command2.addRequirements(system3)
+    command3 = commands2.Command()
+    command3.addRequirements(system3, system4)
+
+    group = commands2.ParallelCommandGroup(command1, command2)
+
+    scheduler.schedule(group)
+    scheduler.schedule(command3)
+
+    assert not scheduler.isScheduled(group)
+    assert scheduler.isScheduled(command3)
+
+
+def test_parallelGroupRequirementError():
+    system1 = commands2.Subsystem()
+    system2 = commands2.Subsystem()
+    system3 = commands2.Subsystem()
+
+    command1 = commands2.Command()
+    command1.addRequirements(system1, system2)
+    command2 = commands2.Command()
+    command2.addRequirements(system2, system3)
+
+    with pytest.raises(commands2.IllegalCommandUse):
+        commands2.ParallelCommandGroup(command1, command2)

commandsv2/src/test/python/test_paralleldeadlinegroup.py

@@ -0,0 +1,119 @@
+from typing import TYPE_CHECKING
+
+import commands2
+from compositiontestbase import MultiCompositionTestBase  # type: ignore
+from util import *  # type: ignore
+
+# from tests.compositiontestbase import T
+
+if TYPE_CHECKING:
+    from .util import *
+    from .compositiontestbase import MultiCompositionTestBase
+
+import pytest
+
+
+class TestParallelDeadlineGroupComposition(MultiCompositionTestBase):
+    def compose(self, *members: commands2.Command):
+        return commands2.ParallelDeadlineGroup(members[0], *members[1:])
+
+
+def test_parallelDeadlineSchedule(scheduler: commands2.CommandScheduler):
+    command1 = commands2.Command()
+    command2 = commands2.Command()
+    command2.isFinished = lambda: True
+    command3 = commands2.Command()
+
+    start_spying_on(command1)
+    start_spying_on(command2)
+    start_spying_on(command3)
+
+    group = commands2.ParallelDeadlineGroup(command1, command2, command3)
+
+    scheduler.schedule(group)
+    scheduler.run()
+
+    assert scheduler.isScheduled(group)
+
+    command1.isFinished = lambda: True
+    scheduler.run()
+
+    assert not scheduler.isScheduled(group)
+
+    verify(command2).initialize()
+    verify(command2).execute()
+    verify(command2).end(False)
+    verify(command2, never()).end(True)
+
+    verify(command1).initialize()
+    verify(command1, times(2)).execute()
+    verify(command1).end(False)
+    verify(command1, never()).end(True)
+
+    verify(command3).initialize()
+    verify(command3, times(2)).execute()
+    verify(command3, never()).end(False)
+    verify(command3).end(True)
+
+
+def test_parallelDeadlineInterrupt(scheduler: commands2.CommandScheduler):
+    command1 = commands2.Command()
+    command2 = commands2.Command()
+    command2.isFinished = lambda: True
+
+    start_spying_on(command1)
+    start_spying_on(command2)
+
+    group = commands2.ParallelDeadlineGroup(command1, command2)
+
+    scheduler.schedule(group)
+
+    scheduler.run()
+    scheduler.run()
+    scheduler.cancel(group)
+
+    verify(command1, times(2)).execute()
+    verify(command1, never()).end(False)
+    verify(command1).end(True)
+
+    verify(command2).execute()
+    verify(command2).end(False)
+    verify(command2, never()).end(True)
+
+    assert not scheduler.isScheduled(group)
+
+
+def test_parallelDeadlineRequirement(scheduler: commands2.CommandScheduler):
+    system1 = commands2.Subsystem()
+    system2 = commands2.Subsystem()
+    system3 = commands2.Subsystem()
+    system4 = commands2.Subsystem()
+
+    command1 = commands2.Command()
+    command1.addRequirements(system1, system2)
+    command2 = commands2.Command()
+    command2.addRequirements(system3)
+    command3 = commands2.Command()
+    command3.addRequirements(system3, system4)
+
+    group = commands2.ParallelDeadlineGroup(command1, command2)
+
+    scheduler.schedule(group)
+    scheduler.schedule(command3)
+
+    assert not scheduler.isScheduled(group)
+    assert scheduler.isScheduled(command3)
+
+
+def test_parallelDeadlineRequirementError(scheduler: commands2.CommandScheduler):
+    system1 = commands2.Subsystem()
+    system2 = commands2.Subsystem()
+    system3 = commands2.Subsystem()
+
+    command1 = commands2.Command()
+    command1.addRequirements(system1, system2)
+    command2 = commands2.Command()
+    command2.addRequirements(system2, system3)
+
+    with pytest.raises(commands2.IllegalCommandUse):
+        commands2.ParallelDeadlineGroup(command1, command2)

commandsv2/src/test/python/test_parallelracegroup.py

@@ -0,0 +1,183 @@
+from typing import TYPE_CHECKING
+
+import commands2
+from compositiontestbase import MultiCompositionTestBase  # type: ignore
+from util import *  # type: ignore
+
+# from tests.compositiontestbase import T
+
+if TYPE_CHECKING:
+    from .util import *
+    from .compositiontestbase import MultiCompositionTestBase
+
+import pytest
+
+
+class TestParallelRaceGroupComposition(MultiCompositionTestBase):
+    def compose(self, *members: commands2.Command):
+        return commands2.ParallelRaceGroup(*members)
+
+
+def test_parallelRaceSchedule(scheduler: commands2.CommandScheduler):
+    command1 = commands2.Command()
+    command2 = commands2.Command()
+
+    start_spying_on(command1)
+    start_spying_on(command2)
+
+    group = commands2.ParallelRaceGroup(command1, command2)
+
+    scheduler.schedule(group)
+
+    verify(command1).initialize()
+    verify(command2).initialize()
+
+    command1.isFinished = lambda: True
+    scheduler.run()
+    command2.isFinished = lambda: True
+    scheduler.run()
+
+    verify(command1).execute()
+    verify(command1).end(False)
+    verify(command2).execute()
+    verify(command2).end(True)
+    verify(command2, never()).end(False)
+
+    assert not scheduler.isScheduled(group)
+
+
+def test_parallelRaceInterrupt(scheduler: commands2.CommandScheduler):
+    command1 = commands2.Command()
+    command2 = commands2.Command()
+
+    start_spying_on(command1)
+    start_spying_on(command2)
+
+    group = commands2.ParallelRaceGroup(command1, command2)
+
+    scheduler.schedule(group)
+
+    scheduler.run()
+    scheduler.run()
+    scheduler.cancel(group)
+
+    verify(command1, times(2)).execute()
+    verify(command1, never()).end(False)
+    verify(command1).end(True)
+
+    verify(command2, times(2)).execute()
+    verify(command2, never()).end(False)
+    verify(command2).end(True)
+
+    assert not scheduler.isScheduled(group)
+
+
+def test_notScheduledCancel(scheduler: commands2.CommandScheduler):
+    command1 = commands2.Command()
+    command2 = commands2.Command()
+
+    group = commands2.ParallelRaceGroup(command1, command2)
+
+    scheduler.cancel(group)
+
+
+def test_parallelRaceRequirement(scheduler: commands2.CommandScheduler):
+    system1 = commands2.Subsystem()
+    system2 = commands2.Subsystem()
+    system3 = commands2.Subsystem()
+    system4 = commands2.Subsystem()
+
+    command1 = commands2.Command()
+    command1.addRequirements(system1, system2)
+    command2 = commands2.Command()
+    command2.addRequirements(system3)
+    command3 = commands2.Command()
+    command3.addRequirements(system3, system4)
+
+    group = commands2.ParallelRaceGroup(command1, command2)
+
+    scheduler.schedule(group)
+    scheduler.schedule(command3)
+
+    assert not scheduler.isScheduled(group)
+    assert scheduler.isScheduled(command3)
+
+
+def test_parallelRaceRequirementError():
+    system1 = commands2.Subsystem()
+    system2 = commands2.Subsystem()
+    system3 = commands2.Subsystem()
+
+    command1 = commands2.Command()
+    command1.addRequirements(system1, system2)
+    command2 = commands2.Command()
+    command2.addRequirements(system2, system3)
+
+    with pytest.raises(commands2.IllegalCommandUse):
+        commands2.ParallelRaceGroup(command1, command2)
+
+
+def test_parallelRaceOnlyCallsEndOnce(scheduler: commands2.CommandScheduler):
+    system1 = commands2.Subsystem()
+    system2 = commands2.Subsystem()
+
+    command1 = commands2.Command()
+    command1.addRequirements(system1)
+    command2 = commands2.Command()
+    command2.addRequirements(system2)
+    command3 = commands2.Command()
+
+    group1 = commands2.SequentialCommandGroup(command1, command2)
+    group2 = commands2.ParallelRaceGroup(group1, command3)
+
+    scheduler.schedule(group2)
+    scheduler.run()
+    command1.isFinished = lambda: True
+    scheduler.run()
+    command2.isFinished = lambda: True
+    scheduler.run()
+    assert not scheduler.isScheduled(group2)
+
+
+def test_parallelRaceScheduleTwiceTest(scheduler: commands2.CommandScheduler):
+    command1 = commands2.Command()
+    command2 = commands2.Command()
+
+    start_spying_on(command1)
+    start_spying_on(command2)
+
+    group = commands2.ParallelRaceGroup(command1, command2)
+
+    scheduler.schedule(group)
+
+    verify(command1).initialize()
+    verify(command2).initialize()
+
+    command1.isFinished = lambda: True
+    scheduler.run()
+    command2.isFinished = lambda: True
+    scheduler.run()
+
+    verify(command1).execute()
+    verify(command1).end(False)
+    verify(command2).execute()
+    verify(command2).end(True)
+    verify(command2, never()).end(False)
+
+    assert not scheduler.isScheduled(group)
+
+    reset(command1)
+    reset(command2)
+
+    scheduler.schedule(group)
+
+    verify(command1).initialize()
+    verify(command2).initialize()
+
+    scheduler.run()
+    scheduler.run()
+    assert scheduler.isScheduled(group)
+    command2.isFinished = lambda: True
+    scheduler.run()
+
+    assert not scheduler.isScheduled(group)

commandsv2/src/test/python/test_pidcommand.py

@@ -0,0 +1,114 @@
+from typing import TYPE_CHECKING
+
+from util import *  # type: ignore
+import wpimath.controller as controller
+import commands2
+
+if TYPE_CHECKING:
+    from .util import *
+
+import pytest
+
+
+def test_pidCommandSupplier(scheduler: commands2.CommandScheduler):
+    with ManualSimTime() as sim:
+        output_float = OOFloat(0.0)
+        measurement_source = OOFloat(5.0)
+        setpoint_source = OOFloat(2.0)
+        pid_controller = controller.PIDController(0.1, 0.01, 0.001)
+        system = commands2.Subsystem()
+        pidCommand = commands2.PIDCommand(
+            pid_controller,
+            measurement_source,
+            setpoint_source,
+            output_float.set,
+            system,
+        )
+        start_spying_on(pidCommand)
+        scheduler.schedule(pidCommand)
+        scheduler.run()
+        sim.step(1)
+        scheduler.run()
+
+        assert scheduler.isScheduled(pidCommand)
+
+        assert not pidCommand._controller.atSetpoint()
+
+        # Tell the pid command we're at our setpoint through the controller
+        measurement_source.set(setpoint_source())
+
+        sim.step(2)
+
+        scheduler.run()
+
+        # Should be measuring error of 0 now
+        assert pidCommand._controller.atSetpoint()
+
+
+def test_pidCommandScalar(scheduler: commands2.CommandScheduler):
+    with ManualSimTime() as sim:
+        output_float = OOFloat(0.0)
+        measurement_source = OOFloat(5.0)
+        setpoint_source = 2.0
+        pid_controller = controller.PIDController(0.1, 0.01, 0.001)
+        system = commands2.Subsystem()
+        pidCommand = commands2.PIDCommand(
+            pid_controller,
+            measurement_source,
+            setpoint_source,
+            output_float.set,
+            system,
+        )
+        start_spying_on(pidCommand)
+        scheduler.schedule(pidCommand)
+        scheduler.run()
+        sim.step(1)
+        scheduler.run()
+
+        assert scheduler.isScheduled(pidCommand)
+
+        assert not pidCommand._controller.atSetpoint()
+
+        # Tell the pid command we're at our setpoint through the controller
+        measurement_source.set(setpoint_source)
+
+        sim.step(2)
+
+        scheduler.run()
+
+        # Should be measuring error of 0 now
+        assert pidCommand._controller.atSetpoint()
+
+
+def test_withTimeout(scheduler: commands2.CommandScheduler):
+    with ManualSimTime() as sim:
+        output_float = OOFloat(0.0)
+        measurement_source = OOFloat(5.0)
+        setpoint_source = OOFloat(2.0)
+        pid_controller = controller.PIDController(0.1, 0.01, 0.001)
+        system = commands2.Subsystem()
+        command1 = commands2.PIDCommand(
+            pid_controller,
+            measurement_source,
+            setpoint_source,
+            output_float.set,
+            system,
+        )
+        start_spying_on(command1)
+
+        timeout = command1.withTimeout(2)
+
+        scheduler.schedule(timeout)
+        scheduler.run()
+
+        verify(command1).initialize()
+        verify(command1).execute()
+        assert not scheduler.isScheduled(command1)
+        assert scheduler.isScheduled(timeout)
+
+        sim.step(3)
+        scheduler.run()
+
+        verify(command1).end(True)
+        verify(command1, never()).end(False)
+        assert not scheduler.isScheduled(timeout)

commandsv2/src/test/python/test_printcommand.py

@@ -0,0 +1,17 @@
+from typing import TYPE_CHECKING
+
+import commands2
+from util import *  # type: ignore
+
+if TYPE_CHECKING:
+    from .util import *
+
+import pytest
+
+
+def test_printCommandSchedule(capsys, scheduler: commands2.CommandScheduler):
+    command = commands2.PrintCommand("Test!")
+    scheduler.schedule(command)
+    scheduler.run()
+    assert not scheduler.isScheduled(command)
+    assert capsys.readouterr().out == "Test!\n"

commandsv2/src/test/python/test_profiledpidsubsystem.py

@@ -0,0 +1,119 @@
+from types import MethodType
+from typing import Any
+
+import pytest
+from wpimath.controller import ProfiledPIDController, ProfiledPIDControllerRadians
+from wpimath.trajectory import TrapezoidProfile, TrapezoidProfileRadians
+
+from commands2 import ProfiledPIDSubsystem
+
+MAX_VELOCITY = 30  # Radians per second
+MAX_ACCELERATION = 500  # Radians per sec squared
+PID_KP = 50
+
+
+class EvalSubsystem(ProfiledPIDSubsystem):
+    def __init__(self, controller, state_factory):
+        self._state_factory = state_factory
+        super().__init__(controller, 0)
+
+
+def simple_use_output(self, output: float, setpoint: Any):
+    """A simple useOutput method that saves the current state of the controller."""
+    self._output = output
+    self._setpoint = setpoint
+
+
+def simple_get_measurement(self) -> float:
+    """A simple getMeasurement method that returns zero (frozen or stuck plant)."""
+    return 0.0
+
+
+controller_types = [
+    (
+        ProfiledPIDControllerRadians,
+        TrapezoidProfileRadians.Constraints,
+        TrapezoidProfileRadians.State,
+    ),
+    (ProfiledPIDController, TrapezoidProfile.Constraints, TrapezoidProfile.State),
+]
+controller_ids = ["radians", "dimensionless"]
+
+
+@pytest.fixture(params=controller_types, ids=controller_ids)
+def subsystem(request):
+    """
+    Fixture that returns an EvalSubsystem object for each type of controller.
+    """
+    controller, profile_factory, state_factory = request.param
+    profile = profile_factory(MAX_VELOCITY, MAX_ACCELERATION)
+    pid = controller(PID_KP, 0, 0, profile)
+    return EvalSubsystem(pid, state_factory)
+
+
+def test_profiled_pid_subsystem_init(subsystem):
+    """
+    Verify that the ProfiledPIDSubsystem can be initialized using
+    all supported profiled PID controller / trapezoid profile types.
+    """
+    assert isinstance(subsystem, EvalSubsystem)
+
+
+def test_profiled_pid_subsystem_not_implemented_get_measurement(subsystem):
+    """
+    Verify that the ProfiledPIDSubsystem.getMeasurement method
+    raises NotImplementedError.
+    """
+    with pytest.raises(NotImplementedError):
+        subsystem.getMeasurement()
+
+
+def test_profiled_pid_subsystem_not_implemented_use_output(subsystem):
+    """
+    Verify that the ProfiledPIDSubsystem.useOutput method raises
+    NotImplementedError.
+    """
+    with pytest.raises(NotImplementedError):
+        subsystem.useOutput(0, subsystem._state_factory())
+
+
+@pytest.mark.parametrize("use_float", [True, False])
+def test_profiled_pid_subsystem_set_goal(subsystem, use_float):
+    """
+    Verify that the ProfiledPIDSubsystem.setGoal method sets the goal.
+    """
+    if use_float:
+        subsystem.setGoal(1.0)
+        assert subsystem.getController().getGoal().position == 1.0
+        assert subsystem.getController().getGoal().velocity == 0.0
+    else:
+        subsystem.setGoal(subsystem._state_factory(1.0, 2.0))
+        assert subsystem.getController().getGoal().position == 1.0
+        assert subsystem.getController().getGoal().velocity == 2.0
+
+
+def test_profiled_pid_subsystem_enable_subsystem(subsystem):
+    """
+    Verify the subsystem can be enabled.
+    """
+    # Dynamically add useOutput and getMeasurement methods so the
+    # system can be enabled
+    setattr(subsystem, "useOutput", MethodType(simple_use_output, subsystem))
+    setattr(subsystem, "getMeasurement", MethodType(simple_get_measurement, subsystem))
+    # Enable the subsystem
+    subsystem.enable()
+    assert subsystem.isEnabled()
+
+
+def test_profiled_pid_subsystem_disable_subsystem(subsystem):
+    """
+    Verify the subsystem can be disabled.
+    """
+    # Dynamically add useOutput and getMeasurement methods so the
+    # system can be enabled
+    setattr(subsystem, "useOutput", MethodType(simple_use_output, subsystem))
+    setattr(subsystem, "getMeasurement", MethodType(simple_get_measurement, subsystem))
+    # Enable and then disable the subsystem
+    subsystem.enable()
+    subsystem.disable()
+    assert not subsystem.isEnabled()

commandsv2/src/test/python/test_proxycommand.py

@@ -0,0 +1,38 @@
+from typing import TYPE_CHECKING
+
+import commands2
+from util import *  # type: ignore
+
+if TYPE_CHECKING:
+    from .util import *
+
+import pytest
+
+
+def test_proxyCommandSchedule(scheduler: commands2.CommandScheduler):
+    command1 = commands2.Command()
+    start_spying_on(command1)
+
+    scheduleCommand = commands2.ProxyCommand(command1)
+
+    scheduler.schedule(scheduleCommand)
+
+    verify(command1).schedule()
+
+
+def test_proxyCommandEnd(scheduler: commands2.CommandScheduler):
+    cond = OOBoolean()
+
+    command = commands2.WaitUntilCommand(cond)
+
+    scheduleCommand = commands2.ProxyCommand(command)
+
+    scheduler.schedule(scheduleCommand)
+    scheduler.run()
+
+    assert scheduler.isScheduled(scheduleCommand)
+
+    cond.set(True)
+    scheduler.run()
+    scheduler.run()
+    assert not scheduler.isScheduled(scheduleCommand)

commandsv2/src/test/python/test_repeatcommand.py

@@ -0,0 +1,69 @@
+from typing import TYPE_CHECKING
+
+import commands2
+from compositiontestbase import SingleCompositionTestBase  # type: ignore
+from util import *  # type: ignore
+
+if TYPE_CHECKING:
+    from .util import *
+    from .compositiontestbase import SingleCompositionTestBase
+
+import pytest
+
+
+class RepeatCommandCompositionTest(SingleCompositionTestBase):
+    def composeSingle(self, member):
+        return member.repeatedly()
+
+
+def test_callsMethodsCorrectly(scheduler: commands2.CommandScheduler):
+    command = commands2.Command()
+    repeated = command.repeatedly()
+
+    start_spying_on(command)
+
+    assert command.initialize.times_called == 0
+    assert command.execute.times_called == 0
+    assert command.isFinished.times_called == 0
+    assert command.end.times_called == 0
+
+    scheduler.schedule(repeated)
+    assert command.initialize.times_called == 1
+    assert command.execute.times_called == 0
+    assert command.isFinished.times_called == 0
+    assert command.end.times_called == 0
+
+    command.isFinished = lambda: False
+
+    scheduler.run()
+    assert command.initialize.times_called == 1
+    assert command.execute.times_called == 1
+    assert command.isFinished.times_called == 1
+    assert command.end.times_called == 0
+
+    command.isFinished = lambda: True
+    scheduler.run()
+    assert command.initialize.times_called == 1
+    assert command.execute.times_called == 2
+    assert command.isFinished.times_called == 2
+    assert command.end.times_called == 1
+
+    command.isFinished = lambda: False
+    scheduler.run()
+    assert command.initialize.times_called == 2
+    assert command.execute.times_called == 3
+    assert command.isFinished.times_called == 3
+    assert command.end.times_called == 1
+
+    command.isFinished = lambda: True
+    scheduler.run()
+    assert command.initialize.times_called == 2
+    assert command.execute.times_called == 4
+    assert command.isFinished.times_called == 4
+    assert command.end.times_called == 2
+
+    scheduler.cancel(repeated)
+    assert command.initialize.times_called == 2
+    assert command.execute.times_called == 4
+    assert command.isFinished.times_called == 4
+    assert command.end.times_called == 2

commandsv2/src/test/python/test_robotdisabledcommand.py

@@ -0,0 +1,154 @@
+from typing import TYPE_CHECKING
+
+import commands2
+from util import *  # type: ignore
+
+if TYPE_CHECKING:
+    from .util import *
+
+import pytest
+from wpilib import RobotState
+
+
+def test_robotDisabledCommandCancel(scheduler: commands2.CommandScheduler):
+    command = commands2.Command()
+    scheduler.schedule(command)
+    assert scheduler.isScheduled(command)
+    DriverStationSim.setEnabled(False)
+    DriverStationSim.notifyNewData()
+    scheduler.run()
+    assert not scheduler.isScheduled(command)
+    DriverStationSim.setEnabled(True)
+    DriverStationSim.notifyNewData()
+
+
+def test_runWhenDisabled(scheduler: commands2.CommandScheduler):
+    command = commands2.Command()
+    command.runsWhenDisabled = lambda: True
+
+    scheduler.schedule(command)
+
+    assert scheduler.isScheduled(command)
+
+    DriverStationSim.setEnabled(False)
+    DriverStationSim.notifyNewData()
+
+    scheduler.run()
+
+    assert scheduler.isScheduled(command)
+
+
+def test_sequentialGroupRunWhenDisabled(scheduler: commands2.CommandScheduler):
+    command1 = commands2.Command()
+    command1.runsWhenDisabled = lambda: True
+    command2 = commands2.Command()
+    command2.runsWhenDisabled = lambda: True
+    command3 = commands2.Command()
+    command3.runsWhenDisabled = lambda: True
+    command4 = commands2.Command()
+    command4.runsWhenDisabled = lambda: False
+
+    runWhenDisabled = commands2.SequentialCommandGroup(command1, command2)
+    dontRunWhenDisabled = commands2.SequentialCommandGroup(command3, command4)
+
+    scheduler.schedule(runWhenDisabled)
+    scheduler.schedule(dontRunWhenDisabled)
+
+    DriverStationSim.setEnabled(False)
+    DriverStationSim.notifyNewData()
+
+    scheduler.run()
+
+    assert scheduler.isScheduled(runWhenDisabled)
+    assert not scheduler.isScheduled(dontRunWhenDisabled)
+
+
+def test_parallelGroupRunWhenDisabled(scheduler: commands2.CommandScheduler):
+    command1 = commands2.Command()
+    command1.runsWhenDisabled = lambda: True
+    command2 = commands2.Command()
+    command2.runsWhenDisabled = lambda: True
+    command3 = commands2.Command()
+    command3.runsWhenDisabled = lambda: True
+    command4 = commands2.Command()
+    command4.runsWhenDisabled = lambda: False
+
+    runWhenDisabled = commands2.ParallelCommandGroup(command1, command2)
+    dontRunWhenDisabled = commands2.ParallelCommandGroup(command3, command4)
+
+    scheduler.schedule(runWhenDisabled)
+    scheduler.schedule(dontRunWhenDisabled)
+
+    DriverStationSim.setEnabled(False)
+    DriverStationSim.notifyNewData()
+
+    scheduler.run()
+
+    assert scheduler.isScheduled(runWhenDisabled)
+    assert not scheduler.isScheduled(dontRunWhenDisabled)
+
+
+def test_conditionalRunWhenDisabled(scheduler: commands2.CommandScheduler):
+    DriverStationSim.setEnabled(False)
+    DriverStationSim.notifyNewData()
+
+    command1 = commands2.Command()
+    command1.runsWhenDisabled = lambda: True
+    command2 = commands2.Command()
+    command2.runsWhenDisabled = lambda: True
+    command3 = commands2.Command()
+    command3.runsWhenDisabled = lambda: True
+    command4 = commands2.Command()
+    command4.runsWhenDisabled = lambda: False
+
+    runWhenDisabled = commands2.ConditionalCommand(command1, command2, lambda: True)
+    dontRunWhenDisabled = commands2.ConditionalCommand(command3, command4, lambda: True)
+
+    scheduler.schedule(runWhenDisabled, dontRunWhenDisabled)
+
+    assert scheduler.isScheduled(runWhenDisabled)
+    assert not scheduler.isScheduled(dontRunWhenDisabled)
+
+
+def test_selectRunWhenDisabled(scheduler: commands2.CommandScheduler):
+    DriverStationSim.setEnabled(False)
+    DriverStationSim.notifyNewData()
+
+    command1 = commands2.Command()
+    command1.runsWhenDisabled = lambda: True
+    command2 = commands2.Command()
+    command2.runsWhenDisabled = lambda: True
+    command3 = commands2.Command()
+    command3.runsWhenDisabled = lambda: True
+    command4 = commands2.Command()
+    command4.runsWhenDisabled = lambda: False
+
+    runWhenDisabled = commands2.SelectCommand({1: command1, 2: command2}, lambda: 1)
+    dontRunWhenDisabled = commands2.SelectCommand({1: command3, 2: command4}, lambda: 1)
+
+    scheduler.schedule(runWhenDisabled, dontRunWhenDisabled)
+    assert scheduler.isScheduled(runWhenDisabled)
+    assert not scheduler.isScheduled(dontRunWhenDisabled)
+
+
+def test_parallelConditionalRunWhenDisabledTest(scheduler: commands2.CommandScheduler):
+    DriverStationSim.setEnabled(False)
+    DriverStationSim.notifyNewData()
+
+    command1 = commands2.Command()
+    command1.runsWhenDisabled = lambda: True
+    command2 = commands2.Command()
+    command2.runsWhenDisabled = lambda: True
+    command3 = commands2.Command()
+    command3.runsWhenDisabled = lambda: True
+    command4 = commands2.Command()
+    command4.runsWhenDisabled = lambda: False
+
+    runWhenDisabled = commands2.ConditionalCommand(command1, command2, lambda: True)
+    dontRunWhenDisabled = commands2.ConditionalCommand(command3, command4, lambda: True)
+
+    parallel = commands2.cmd.parallel(runWhenDisabled, dontRunWhenDisabled)
+
+    scheduler.schedule(parallel)
+
+    assert not scheduler.isScheduled(runWhenDisabled)

commandsv2/src/test/python/test_runcommand.py

@@ -0,0 +1,22 @@
+from typing import TYPE_CHECKING
+
+import commands2
+from util import *  # type: ignore
+
+if TYPE_CHECKING:
+    from .util import *
+
+import pytest
+
+
+def test_runCommandSchedule(scheduler: commands2.CommandScheduler):
+    counter = OOInteger(0)
+
+    command = commands2.RunCommand(counter.incrementAndGet)
+
+    scheduler.schedule(command)
+    scheduler.run()
+    scheduler.run()
+    scheduler.run()
+
+    assert counter == 3

commandsv2/src/test/python/test_schedulecommand.py

@@ -0,0 +1,36 @@
+from typing import TYPE_CHECKING
+
+import commands2
+from util import *  # type: ignore
+
+if TYPE_CHECKING:
+    from .util import *
+
+import pytest
+
+
+def test_scheduleCommandSchedule(scheduler: commands2.CommandScheduler):
+    command1 = commands2.Command()
+    command2 = commands2.Command()
+
+    start_spying_on(command1)
+    start_spying_on(command2)
+
+    scheduleCommand = commands2.ScheduleCommand(command1, command2)
+
+    scheduler.schedule(scheduleCommand)
+
+    verify(command1).schedule()
+    verify(command2).schedule()
+
+
+def test_scheduleCommandDruingRun(scheduler: commands2.CommandScheduler):
+    toSchedule = commands2.InstantCommand()
+    scheduleCommand = commands2.ScheduleCommand(toSchedule)
+    group = commands2.SequentialCommandGroup(
+        commands2.InstantCommand(), scheduleCommand
+    )
+
+    scheduler.schedule(group)
+    scheduler.schedule(commands2.RunCommand(lambda: None))
+    scheduler.run()

commandsv2/src/test/python/test_scheduler.py

@@ -0,0 +1,134 @@
+from typing import TYPE_CHECKING
+
+import commands2
+from util import *  # type: ignore
+
+if TYPE_CHECKING:
+    from .util import *
+
+import pytest
+
+
+def test_schedulerLambdaTestNoInterrupt(scheduler: commands2.CommandScheduler):
+    counter = OOInteger()
+
+    scheduler.onCommandInitialize(lambda _: counter.incrementAndGet())
+    scheduler.onCommandExecute(lambda _: counter.incrementAndGet())
+    scheduler.onCommandFinish(lambda _: counter.incrementAndGet())
+
+    scheduler.schedule(commands2.InstantCommand())
+    scheduler.run()
+
+    assert counter == 3
+
+
+def test_schedulerInterruptLambda(scheduler: commands2.CommandScheduler):
+    counter = OOInteger()
+
+    scheduler.onCommandInterrupt(lambda _: counter.incrementAndGet())
+
+    command = commands2.WaitCommand(10)
+
+    scheduler.schedule(command)
+    scheduler.cancel(command)
+
+    assert counter == 1
+
+
+def test_scheduler_interrupt_no_cause_lambda(scheduler: commands2.CommandScheduler):
+    counter = OOInteger()
+
+    def on_interrupt(interrupted, cause):
+        assert cause is None
+        counter.incrementAndGet()
+
+    scheduler.onCommandInterruptWithCause(on_interrupt)
+
+    command = commands2.cmd.run(lambda: {})
+
+    scheduler.schedule(command)
+    scheduler.cancel(command)
+
+    assert counter.get() == 1
+
+
+def test_scheduler_interrupt_cause_lambda(scheduler: commands2.CommandScheduler):
+    counter = OOInteger()
+
+    subsystem = commands2.Subsystem()
+    command = subsystem.run(lambda: None)
+    interruptor = subsystem.runOnce(lambda: None)
+
+    def on_interrupt(interrupted, cause):
+        assert cause is interruptor
+        counter.incrementAndGet()
+
+    scheduler.onCommandInterruptWithCause(on_interrupt)
+
+    scheduler.schedule(command)
+    scheduler.schedule(interruptor)
+
+    assert counter.get() == 1
+
+
+def test_scheduler_interrupt_cause_lambda_in_run_loop(
+    scheduler: commands2.CommandScheduler,
+):
+    counter = OOInteger()
+
+    subsystem = commands2.Subsystem()
+    command = subsystem.run(lambda: None)
+    interruptor = subsystem.runOnce(lambda: None)
+    # This command will schedule interruptor in execute() inside the run loop
+    interruptor_scheduler = commands2.cmd.runOnce(
+        lambda: scheduler.schedule(interruptor)
+    )
+
+    def on_interrupt(interrupted, cause):
+        assert cause is interruptor
+        counter.incrementAndGet()
+
+    scheduler.onCommandInterruptWithCause(on_interrupt)
+
+    scheduler.schedule(command)
+    scheduler.schedule(interruptor_scheduler)
+
+    scheduler.run()
+
+    assert counter.get() == 1
+
+
+def test_unregisterSubsystem(scheduler: commands2.CommandScheduler):
+    system = commands2.Subsystem()
+    scheduler.registerSubsystem(system)
+    scheduler.unregisterSubsystem(system)
+
+
+def test_schedulerCancelAll(scheduler: commands2.CommandScheduler):
+    counter = OOInteger()
+
+    def on_interrupt(command, interruptor):
+        assert interruptor is None
+
+    scheduler.onCommandInterrupt(lambda _: counter.incrementAndGet())
+    scheduler.onCommandInterruptWithCause(on_interrupt)
+
+    command = commands2.WaitCommand(10)
+    command2 = commands2.WaitCommand(10)
+
+    scheduler.schedule(command)
+    scheduler.schedule(command2)
+    scheduler.cancelAll()
+
+    assert counter == 2
+
+
+def test_scheduleScheduledNoOp(scheduler: commands2.CommandScheduler):
+    counter = OOInteger()
+
+    command = commands2.cmd.startEnd(counter.incrementAndGet, lambda: None)
+
+    scheduler.schedule(command)
+    scheduler.schedule(command)
+
+    assert counter == 1

commandsv2/src/test/python/test_schedulingrecursion.py

@@ -0,0 +1,165 @@
+from typing import TYPE_CHECKING
+
+import commands2
+from util import *  # type: ignore
+
+if TYPE_CHECKING:
+    from .util import *
+
+import pytest
+
+
+@pytest.mark.parametrize(
+    "interruptionBehavior",
+    [
+        commands2.InterruptionBehavior.kCancelIncoming,
+        commands2.InterruptionBehavior.kCancelSelf,
+    ],
+)
+def test_cancelFromInitialize(
+    interruptionBehavior: commands2.InterruptionBehavior,
+    scheduler: commands2.CommandScheduler,
+):
+    hasOtherRun = OOBoolean()
+    requirement = commands2.Subsystem()
+
+    selfCancels = commands2.Command()
+    selfCancels.addRequirements(requirement)
+    selfCancels.getInterruptionBehavior = lambda: interruptionBehavior
+    selfCancels.initialize = lambda: scheduler.cancel(selfCancels)
+
+    other = commands2.RunCommand(lambda: hasOtherRun.set(True), requirement)
+
+    scheduler.schedule(selfCancels)
+    scheduler.run()
+    scheduler.schedule(other)
+
+    assert not scheduler.isScheduled(selfCancels)
+    assert scheduler.isScheduled(other)
+    scheduler.run()
+    assert hasOtherRun == True
+
+
+@pytest.mark.parametrize(
+    "interruptionBehavior",
+    [
+        commands2.InterruptionBehavior.kCancelIncoming,
+        commands2.InterruptionBehavior.kCancelSelf,
+    ],
+)
+def test_defaultCommandGetsRescheduledAfterSelfCanceling(
+    interruptionBehavior: commands2.InterruptionBehavior,
+    scheduler: commands2.CommandScheduler,
+):
+    hasOtherRun = OOBoolean()
+    requirement = commands2.Subsystem()
+
+    selfCancels = commands2.Command()
+    selfCancels.addRequirements(requirement)
+    selfCancels.getInterruptionBehavior = lambda: interruptionBehavior
+    selfCancels.initialize = lambda: scheduler.cancel(selfCancels)
+
+    other = commands2.RunCommand(lambda: hasOtherRun.set(True), requirement)
+    scheduler.setDefaultCommand(requirement, other)
+
+    scheduler.schedule(selfCancels)
+    scheduler.run()
+
+    scheduler.run()
+    assert not scheduler.isScheduled(selfCancels)
+    assert scheduler.isScheduled(other)
+    scheduler.run()
+    assert hasOtherRun == True
+
+
+def test_cancelFromEnd(scheduler: commands2.CommandScheduler):
+    counter = OOInteger()
+    selfCancels = commands2.Command()
+
+    @patch_via_decorator(selfCancels)
+    def end(self, interrupted):
+        counter.incrementAndGet()
+        scheduler.cancel(self)
+
+    scheduler.schedule(selfCancels)
+
+    scheduler.cancel(selfCancels)
+    assert counter == 1
+    assert not scheduler.isScheduled(selfCancels)
+
+
+def test_scheduleFromEnd(scheduler: commands2.CommandScheduler):
+    counter = OOInteger()
+    requirement = commands2.Subsystem()
+    other = commands2.InstantCommand(lambda: None, requirement)
+
+    selfCancels = commands2.Command()
+    selfCancels.addRequirements(requirement)
+
+    @patch_via_decorator(selfCancels)
+    def end(self, interrupted):
+        counter.incrementAndGet()
+        scheduler.schedule(other)
+
+    scheduler.schedule(selfCancels)
+
+    scheduler.cancel(selfCancels)
+    assert counter == 1
+    assert not scheduler.isScheduled(selfCancels)
+
+
+def test_scheduleFromEndInterrupt(scheduler: commands2.CommandScheduler):
+    counter = OOInteger()
+    requirement = commands2.Subsystem()
+    other = commands2.InstantCommand(lambda: None, requirement)
+
+    selfCancels = commands2.Command()
+    selfCancels.addRequirements(requirement)
+
+    @patch_via_decorator(selfCancels)
+    def end(self, interrupted):
+        counter.incrementAndGet()
+        scheduler.schedule(other)
+
+    scheduler.schedule(selfCancels)
+
+    scheduler.schedule(other)
+    assert counter == 1
+    assert not scheduler.isScheduled(selfCancels)
+    assert scheduler.isScheduled(other)
+
+
+@pytest.mark.parametrize(
+    "interruptionBehavior",
+    [
+        commands2.InterruptionBehavior.kCancelIncoming,
+        commands2.InterruptionBehavior.kCancelSelf,
+    ],
+)
+def test_scheduleInitializeFromDefaultCommand(
+    interruptionBehavior: commands2.InterruptionBehavior,
+    scheduler: commands2.CommandScheduler,
+):
+    counter = OOInteger()
+    requirement = commands2.Subsystem()
+    other = commands2.InstantCommand(lambda: None, requirement).withInterruptBehavior(
+        interruptionBehavior
+    )
+
+    defaultCommand = commands2.Command()
+    defaultCommand.addRequirements(requirement)
+
+    @patch_via_decorator(defaultCommand)
+    def initialize(self):
+        counter.incrementAndGet()
+        scheduler.schedule(other)
+
+    scheduler.setDefaultCommand(requirement, defaultCommand)
+
+    scheduler.run()
+    scheduler.run()
+    scheduler.run()
+
+    assert counter == 3
+    assert not scheduler.isScheduled(defaultCommand)
+    assert scheduler.isScheduled(other)

commandsv2/src/test/python/test_selectcommand.py

@@ -0,0 +1,94 @@
+from typing import TYPE_CHECKING
+
+import commands2
+from compositiontestbase import MultiCompositionTestBase  # type: ignore
+from util import *  # type: ignore
+
+if TYPE_CHECKING:
+    from .util import *
+    from .compositiontestbase import MultiCompositionTestBase
+
+import pytest
+
+
+class TestSelectCommandComposition(MultiCompositionTestBase):
+    def compose(self, *members: commands2.Command):
+        return commands2.SelectCommand(dict(enumerate(members)), lambda: 0)
+
+
+def test_selectCommand(scheduler: commands2.CommandScheduler):
+    command1 = commands2.Command()
+    command2 = commands2.Command()
+    command3 = commands2.Command()
+
+    start_spying_on(command1)
+    start_spying_on(command2)
+    start_spying_on(command3)
+
+    command1.isFinished = lambda: True
+
+    selectCommand = commands2.SelectCommand(
+        {"one": command1, "two": command2, "three": command3}, lambda: "one"
+    )
+
+    scheduler.schedule(selectCommand)
+    scheduler.run()
+
+    verify(command1).initialize()
+    verify(command1).execute()
+    verify(command1).end(False)
+
+    verify(command2, never()).initialize()
+    verify(command2, never()).execute()
+    verify(command2, never()).end(False)
+
+    verify(command3, never()).initialize()
+    verify(command3, never()).execute()
+    verify(command3, never()).end(False)
+
+
+def test_selectCommandInvalidKey(scheduler: commands2.CommandScheduler):
+    command1 = commands2.Command()
+    command2 = commands2.Command()
+    command3 = commands2.Command()
+
+    start_spying_on(command1)
+    start_spying_on(command2)
+    start_spying_on(command3)
+
+    command1.isFinished = lambda: True
+
+    selectCommand = commands2.SelectCommand(
+        {"one": command1, "two": command2, "three": command3}, lambda: "four"
+    )
+
+    scheduler.schedule(selectCommand)
+
+
+def test_selectCommandRequirement(scheduler: commands2.CommandScheduler):
+    system1 = commands2.Subsystem()
+    system2 = commands2.Subsystem()
+    system3 = commands2.Subsystem()
+    system4 = commands2.Subsystem()
+
+    command1 = commands2.Command()
+    command1.addRequirements(system1, system2)
+    command2 = commands2.Command()
+    command2.addRequirements(system3)
+    command3 = commands2.Command()
+    command3.addRequirements(system3, system4)
+
+    start_spying_on(command1)
+    start_spying_on(command2)
+    start_spying_on(command3)
+
+    selectCommand = commands2.SelectCommand(
+        {"one": command1, "two": command2, "three": command3}, lambda: "one"
+    )
+
+    scheduler.schedule(selectCommand)
+    scheduler.schedule(commands2.InstantCommand(lambda: None, system3))
+
+    verify(command1).end(interrupted=True)
+    verify(command2, never()).end(interrupted=True)
+    verify(command3, never()).end(interrupted=True)

commandsv2/src/test/python/test_sequentialcommandgroup.py

@@ -0,0 +1,114 @@
+from typing import TYPE_CHECKING
+
+import commands2
+from compositiontestbase import MultiCompositionTestBase  # type: ignore
+from util import *  # type: ignore
+
+if TYPE_CHECKING:
+    from .util import *
+    from .compositiontestbase import MultiCompositionTestBase
+
+import pytest
+
+
+class TestSequentialCommandGroupComposition(MultiCompositionTestBase):
+    def compose(self, *members: commands2.Command):
+        return commands2.SequentialCommandGroup(*members)
+
+
+def test_sequntialGroupSchedule(scheduler: commands2.CommandScheduler):
+    command1 = commands2.Command()
+    command2 = commands2.Command()
+
+    start_spying_on(command1)
+    start_spying_on(command2)
+
+    group = commands2.SequentialCommandGroup(command1, command2)
+
+    scheduler.schedule(group)
+
+    verify(command1).initialize()
+    verify(command2, never()).initialize()
+
+    command1.isFinished = lambda: True
+    scheduler.run()
+
+    verify(command1).execute()
+    verify(command1).end(False)
+    verify(command2).initialize()
+    verify(command2, never()).execute()
+    verify(command2, never()).end(False)
+
+    command2.isFinished = lambda: True
+    scheduler.run()
+
+    verify(command1).execute()
+    verify(command1).end(False)
+    verify(command2).execute()
+    verify(command2).end(False)
+
+    assert not scheduler.isScheduled(group)
+
+
+def test_sequentialGroupInterrupt(scheduler: commands2.CommandScheduler):
+    command1 = commands2.Command()
+    command2 = commands2.Command()
+    command3 = commands2.Command()
+
+    start_spying_on(command1)
+    start_spying_on(command2)
+    start_spying_on(command3)
+
+    group = commands2.SequentialCommandGroup(command1, command2, command3)
+
+    scheduler.schedule(group)
+
+    command1.isFinished = lambda: True
+    scheduler.run()
+    scheduler.cancel(group)
+    scheduler.run()
+
+    verify(command1).execute()
+    verify(command1, never()).end(True)
+    verify(command1).end(False)
+    verify(command2, never()).execute()
+    verify(command2).end(True)
+    verify(command3, never()).initialize()
+    verify(command3, never()).execute()
+
+    # assert command3.end.times_called == 0
+    verify(command3, never()).end(True)
+    verify(command3, never()).end(False)
+
+    assert not scheduler.isScheduled(group)
+
+
+def test_notScheduledCancel(scheduler: commands2.CommandScheduler):
+    command1 = commands2.Command()
+    command2 = commands2.Command()
+
+    group = commands2.SequentialCommandGroup(command1, command2)
+
+    scheduler.cancel(group)
+
+
+def test_sequentialGroupRequirement(scheduler: commands2.CommandScheduler):
+    system1 = commands2.Subsystem()
+    system2 = commands2.Subsystem()
+    system3 = commands2.Subsystem()
+    system4 = commands2.Subsystem()
+
+    command1 = commands2.InstantCommand()
+    command1.addRequirements(system1, system2)
+    command2 = commands2.InstantCommand()
+    command2.addRequirements(system3)
+    command3 = commands2.InstantCommand()
+    command3.addRequirements(system3, system4)
+
+    group = commands2.SequentialCommandGroup(command1, command2)
+
+    scheduler.schedule(group)
+    scheduler.schedule(command3)
+
+    assert not scheduler.isScheduled(group)
+    assert scheduler.isScheduled(command3)

commandsv2/src/test/python/test_startendcommand.py

@@ -0,0 +1,30 @@
+from typing import TYPE_CHECKING
+
+import commands2
+from util import *  # type: ignore
+
+if TYPE_CHECKING:
+    from .util import *
+
+import pytest
+
+
+def test_startEndCommandSchedule(scheduler: commands2.CommandScheduler):
+    cond1 = OOBoolean(False)
+    cond2 = OOBoolean(False)
+
+    command = commands2.StartEndCommand(
+        lambda: cond1.set(True),
+        lambda: cond2.set(True),
+    )
+
+    scheduler.schedule(command)
+    scheduler.run()
+
+    assert scheduler.isScheduled(command)
+
+    scheduler.cancel(command)
+
+    assert not scheduler.isScheduled(command)
+    assert cond1 == True
+    assert cond2 == True

commandsv2/src/test/python/test_swervecontrollercommand.py

@@ -0,0 +1,142 @@
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+
+from typing import TYPE_CHECKING, List, Tuple
+import math
+
+from wpilib import Timer
+
+from wpimath.geometry import Pose2d, Rotation2d, Translation2d
+from wpimath.kinematics import (
+    SwerveModuleState,
+    SwerveModulePosition,
+    SwerveDrive4Kinematics,
+    SwerveDrive4Odometry,
+)
+from wpimath.controller import (
+    ProfiledPIDControllerRadians,
+    PIDController,
+    HolonomicDriveController,
+)
+from wpimath.trajectory import (
+    TrapezoidProfileRadians,
+    Trajectory,
+    TrajectoryConfig,
+    TrajectoryGenerator,
+)
+
+
+from util import *  # type: ignore
+
+if TYPE_CHECKING:
+    from .util import *
+
+import pytest
+
+import commands2
+
+
+def test_swervecontrollercommand():
+    timer = Timer()
+    angle = Rotation2d(0)
+
+    swerve_module_states = (
+        SwerveModuleState(0, Rotation2d(0)),
+        SwerveModuleState(0, Rotation2d(0)),
+        SwerveModuleState(0, Rotation2d(0)),
+        SwerveModuleState(0, Rotation2d(0)),
+    )
+
+    swerve_module_positions = (
+        SwerveModulePosition(0, Rotation2d(0)),
+        SwerveModulePosition(0, Rotation2d(0)),
+        SwerveModulePosition(0, Rotation2d(0)),
+        SwerveModulePosition(0, Rotation2d(0)),
+    )
+
+    rot_controller = ProfiledPIDControllerRadians(
+        1,
+        0,
+        0,
+        TrapezoidProfileRadians.Constraints(3 * math.pi, math.pi),
+    )
+
+    x_tolerance = 1 / 12.0
+    y_tolerance = 1 / 12.0
+    angular_tolerance = 1 / 12.0
+
+    wheel_base = 0.5
+    track_width = 0.5
+
+    kinematics = SwerveDrive4Kinematics(
+        Translation2d(wheel_base / 2, track_width / 2),
+        Translation2d(wheel_base / 2, -track_width / 2),
+        Translation2d(-wheel_base / 2, track_width / 2),
+        Translation2d(-wheel_base / 2, -track_width / 2),
+    )
+
+    odometry = SwerveDrive4Odometry(
+        kinematics,
+        Rotation2d(0),
+        swerve_module_positions,
+        Pose2d(0, 0, Rotation2d(0)),
+    )
+
+    def set_module_states(states):
+        nonlocal swerve_module_states
+        swerve_module_states = states
+
+    def get_robot_pose() -> Pose2d:
+        odometry.update(angle, swerve_module_positions)
+        return odometry.getPose()
+
+    with ManualSimTime() as sim:
+        subsystem = commands2.Subsystem()
+        waypoints: List[Pose2d] = []
+        waypoints.append(Pose2d(0, 0, Rotation2d(0)))
+        waypoints.append(Pose2d(1, 5, Rotation2d(3)))
+        config = TrajectoryConfig(8.8, 0.1)
+        trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config)
+
+        end_state = trajectory.sample(trajectory.totalTime())
+
+        command = commands2.SwerveControllerCommand(
+            trajectory=trajectory,
+            pose=get_robot_pose,
+            kinematics=kinematics,
+            controller=HolonomicDriveController(
+                PIDController(0.6, 0, 0),
+                PIDController(0.6, 0, 0),
+                rot_controller,
+            ),
+            outputModuleStates=set_module_states,
+            requirements=(subsystem,),
+        )
+
+        timer.restart()
+
+        command.initialize()
+        while not command.isFinished():
+            command.execute()
+            angle = trajectory.sample(timer.get()).pose.rotation()
+
+            for i in range(0, len(swerve_module_positions)):
+                swerve_module_positions[i].distance += (
+                    swerve_module_states[i].speed * 0.005
+                )
+                swerve_module_positions[i].angle = swerve_module_states[i].angle
+
+            sim.step(0.005)
+
+        timer.stop()
+        command.end(True)
+
+        assert end_state.pose.X() == pytest.approx(get_robot_pose().X(), x_tolerance)
+
+        assert end_state.pose.Y() == pytest.approx(get_robot_pose().Y(), y_tolerance)
+
+        assert end_state.pose.rotation().radians() == pytest.approx(
+            get_robot_pose().rotation().radians(),
+            angular_tolerance,
+        )

commandsv2/src/test/python/test_sysidroutine.py

@@ -0,0 +1,168 @@
+import pytest
+from unittest.mock import Mock, call, ANY
+from wpilib.simulation import stepTiming, pauseTiming, resumeTiming
+from wpimath.units import volts
+from commands2 import Command, Subsystem
+from commands2.sysid import SysIdRoutine
+from wpilib.sysid import SysIdRoutineLog, State
+
+
+class Mechanism(Subsystem):
+    def recordState(self, state: State):
+        pass
+
+    def drive(self, voltage: volts):
+        pass
+
+    def log(self, log: SysIdRoutineLog):
+        pass
+
+
+@pytest.fixture
+def mechanism():
+    return Mock(spec=Mechanism)
+
+
+@pytest.fixture
+def sysid_routine(mechanism):
+    return SysIdRoutine(
+        SysIdRoutine.Config(recordState=mechanism.recordState),
+        SysIdRoutine.Mechanism(mechanism.drive, mechanism.log, Subsystem()),
+    )
+
+
+@pytest.fixture
+def quasistatic_forward(sysid_routine):
+    return sysid_routine.quasistatic(SysIdRoutine.Direction.kForward)
+
+
+@pytest.fixture
+def quasistatic_reverse(sysid_routine):
+    return sysid_routine.quasistatic(SysIdRoutine.Direction.kReverse)
+
+
+@pytest.fixture
+def dynamic_forward(sysid_routine):
+    return sysid_routine.dynamic(SysIdRoutine.Direction.kForward)
+
+
+@pytest.fixture
+def dynamic_reverse(sysid_routine):
+    return sysid_routine.dynamic(SysIdRoutine.Direction.kReverse)
+
+
+@pytest.fixture(autouse=True)
+def timing():
+    pauseTiming()
+    yield
+    resumeTiming()
+
+
+def run_command(command: Command):
+    command.initialize()
+    command.execute()
+    stepTiming(1)
+    command.execute()
+    command.end(True)
+
+
+def test_record_state_bookends_motor_logging(
+    mechanism, quasistatic_forward, dynamic_forward
+):
+    run_command(quasistatic_forward)
+
+    mechanism.assert_has_calls(
+        [
+            call.drive(ANY),
+            call.log(ANY),
+            call.recordState(State.kQuasistaticForward),
+            call.drive(ANY),
+            call.recordState(State.kNone),
+        ],
+        any_order=False,
+    )
+
+    mechanism.reset_mock()
+    run_command(dynamic_forward)
+
+    mechanism.assert_has_calls(
+        [
+            call.drive(ANY),
+            call.log(ANY),
+            call.recordState(State.kDynamicForward),
+            call.drive(ANY),
+            call.recordState(State.kNone),
+        ],
+        any_order=False,
+    )
+
+
+def test_tests_declare_correct_state(
+    mechanism,
+    quasistatic_forward,
+    quasistatic_reverse,
+    dynamic_forward,
+    dynamic_reverse,
+):
+    run_command(quasistatic_forward)
+    mechanism.recordState.assert_any_call(State.kQuasistaticForward)
+
+    run_command(quasistatic_reverse)
+    mechanism.recordState.assert_any_call(State.kQuasistaticReverse)
+
+    run_command(dynamic_forward)
+    mechanism.recordState.assert_any_call(State.kDynamicForward)
+
+    run_command(dynamic_reverse)
+    mechanism.recordState.assert_any_call(State.kDynamicReverse)
+
+
+def test_tests_output_correct_voltage(
+    mechanism,
+    quasistatic_forward,
+    quasistatic_reverse,
+    dynamic_forward,
+    dynamic_reverse,
+):
+    run_command(quasistatic_forward)
+
+    mechanism.drive.assert_has_calls(
+        [
+            call(pytest.approx(1.0)),
+            call(pytest.approx(0.0)),
+        ],
+        any_order=False,
+    )
+
+    mechanism.reset_mock()
+    run_command(quasistatic_reverse)
+
+    mechanism.drive.assert_has_calls(
+        [
+            call(pytest.approx(-1.0)),
+            call(pytest.approx(0.0)),
+        ],
+        any_order=False,
+    )
+
+    mechanism.reset_mock()
+    run_command(dynamic_forward)
+
+    mechanism.drive.assert_has_calls(
+        [
+            call(pytest.approx(7.0)),
+            call(pytest.approx(0.0)),
+        ],
+        any_order=False,
+    )
+
+    mechanism.reset_mock()
+    run_command(dynamic_reverse)
+
+    mechanism.drive.assert_has_calls(
+        [
+            call(pytest.approx(-7.0)),
+            call(pytest.approx(0.0)),
+        ],
+        any_order=False,
+    )

commandsv2/src/test/python/test_trapezoidprofilecommand.py

@@ -0,0 +1,142 @@
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+
+from typing import TYPE_CHECKING, List, Tuple
+import math
+
+import wpimath.controller as controller
+import wpimath.trajectory as trajectory
+import wpimath.geometry as geometry
+import wpimath.kinematics as kinematics
+from wpimath.trajectory import TrapezoidProfile as DimensionlessProfile
+from wpimath.trajectory import TrapezoidProfileRadians as RadiansProfile
+
+from wpilib import Timer
+
+from util import *  # type: ignore
+
+if TYPE_CHECKING:
+    from .util import *
+
+import pytest
+
+import commands2
+
+
+class TrapezoidProfileRadiansFixture:
+    def __init__(self):
+        constraints: RadiansProfile.Constraints = RadiansProfile.Constraints(
+            3 * math.pi, math.pi
+        )
+        self._profile: RadiansProfile = RadiansProfile(constraints)
+        self._goal_state = RadiansProfile.State(3, 0)
+
+        self._state = self._profile.calculate(
+            0, self._goal_state, RadiansProfile.State(0, 0)
+        )
+
+        self._timer = Timer()
+
+    def profileOutput(self, state: RadiansProfile.State) -> None:
+        self._state = state
+
+    def currentState(self) -> RadiansProfile.State:
+        return self._state
+
+    def getGoal(self) -> RadiansProfile.State:
+        return self._goal_state
+
+
+@pytest.fixture()
+def get_trapezoid_profile_radians() -> TrapezoidProfileRadiansFixture:
+    return TrapezoidProfileRadiansFixture()
+
+
+class TrapezoidProfileFixture:
+    def __init__(self):
+        constraints: DimensionlessProfile.Constraints = (
+            DimensionlessProfile.Constraints(3 * math.pi, math.pi)
+        )
+        self._profile: DimensionlessProfile = DimensionlessProfile(constraints)
+        self._goal_state = DimensionlessProfile.State(3, 0)
+
+        self._state = self._profile.calculate(
+            0, self._goal_state, DimensionlessProfile.State(0, 0)
+        )
+
+        self._timer = Timer()
+
+    def profileOutput(self, state: DimensionlessProfile.State) -> None:
+        self._state = state
+
+    def currentState(self) -> DimensionlessProfile.State:
+        return self._state
+
+    def getGoal(self) -> DimensionlessProfile.State:
+        return self._goal_state
+
+
+@pytest.fixture()
+def get_trapezoid_profile_dimensionless() -> TrapezoidProfileFixture:
+    return TrapezoidProfileFixture()
+
+
+def test_trapezoidProfileDimensionless(
+    scheduler: commands2.CommandScheduler, get_trapezoid_profile_dimensionless
+):
+    with ManualSimTime() as sim:
+        subsystem = commands2.Subsystem()
+
+        fixture_data = get_trapezoid_profile_dimensionless
+
+        command = commands2.TrapezoidProfileCommand(
+            fixture_data._profile,
+            fixture_data.profileOutput,
+            fixture_data.getGoal,
+            fixture_data.currentState,
+            subsystem,
+        )
+
+        fixture_data._timer.restart()
+
+        command.initialize()
+
+        count = 0
+        while not command.isFinished():
+            command.execute()
+            count += 1
+            sim.step(0.005)
+
+        fixture_data._timer.stop()
+        command.end(True)
+
+
+def test_trapezoidProfileRadians(
+    scheduler: commands2.CommandScheduler, get_trapezoid_profile_radians
+):
+    with ManualSimTime() as sim:
+        subsystem = commands2.Subsystem()
+
+        fixture_data = get_trapezoid_profile_radians
+
+        command = commands2.TrapezoidProfileCommand(
+            fixture_data._profile,
+            fixture_data.profileOutput,
+            fixture_data.getGoal,
+            fixture_data.currentState,
+            subsystem,
+        )
+
+        fixture_data._timer.restart()
+
+        command.initialize()
+
+        count = 0
+        while not command.isFinished():
+            command.execute()
+            count += 1
+            sim.step(0.005)
+
+        fixture_data._timer.stop()
+        command.end(True)

commandsv2/src/test/python/test_trigger.py

@@ -0,0 +1,236 @@
+from typing import TYPE_CHECKING
+
+import commands2
+from util import *  # type: ignore
+from wpilib.simulation import stepTiming
+
+if TYPE_CHECKING:
+    from .util import *
+
+
+def test_onTrue(scheduler: commands2.CommandScheduler):
+    finished = OOBoolean(False)
+    command1 = commands2.WaitUntilCommand(finished)
+
+    button = InternalButton()
+    button.setPressed(False)
+    button.onTrue(command1)
+    scheduler.run()
+    assert not command1.isScheduled()
+    button.setPressed(True)
+    scheduler.run()
+    assert command1.isScheduled()
+    finished.set(True)
+    scheduler.run()
+    assert not command1.isScheduled()
+
+
+def test_onFalse(scheduler: commands2.CommandScheduler):
+    finished = OOBoolean(False)
+    command1 = commands2.WaitUntilCommand(finished)
+
+    button = InternalButton()
+    button.setPressed(True)
+    button.onFalse(command1)
+    scheduler.run()
+    assert not command1.isScheduled()
+    button.setPressed(False)
+    scheduler.run()
+    assert command1.isScheduled()
+    finished.set(True)
+    scheduler.run()
+    assert not command1.isScheduled()
+
+
+def test_onChange(scheduler: commands2.CommandScheduler):
+    finished = OOBoolean(False)
+    command1 = commands2.WaitUntilCommand(finished)
+
+    button = InternalButton()
+    button.setPressed(True)
+    button.onChange(command1)
+    scheduler.run()
+    assert not command1.isScheduled()
+    button.setPressed(False)
+    scheduler.run()
+    assert command1.isScheduled()
+    finished.set(True)
+    scheduler.run()
+    assert not command1.isScheduled()
+
+
+def test_whileTrueRepeatedly(scheduler: commands2.CommandScheduler):
+    inits = OOInteger(0)
+    counter = OOInteger(0)
+
+    command1 = commands2.FunctionalCommand(
+        inits.incrementAndGet,
+        lambda: None,
+        lambda _: None,
+        lambda: counter.incrementAndGet() % 2 == 0,
+    ).repeatedly()
+
+    button = InternalButton()
+    button.setPressed(False)
+    button.whileTrue(command1)
+    scheduler.run()
+    assert inits == 0
+    button.setPressed(True)
+    scheduler.run()
+    assert inits == 1
+    scheduler.run()
+    assert inits == 1
+    scheduler.run()
+    assert inits == 2
+    button.setPressed(False)
+    scheduler.run()
+    assert inits == 2
+
+
+def test_whileTrueLambdaRunCommand(scheduler: commands2.CommandScheduler):
+    counter = OOInteger(0)
+
+    command1 = commands2.RunCommand(counter.incrementAndGet)
+
+    button = InternalButton()
+    button.setPressed(False)
+    button.whileTrue(command1)
+    scheduler.run()
+    assert counter == 0
+    button.setPressed(True)
+    scheduler.run()
+    assert counter == 1
+    scheduler.run()
+    assert counter == 2
+    button.setPressed(False)
+    scheduler.run()
+    assert counter == 2
+
+
+def test_whileTrueOnce(scheduler: commands2.CommandScheduler):
+    startCounter = OOInteger(0)
+    endCounter = OOInteger(0)
+
+    command1 = commands2.StartEndCommand(
+        startCounter.incrementAndGet, endCounter.incrementAndGet
+    )
+
+    button = InternalButton()
+    button.setPressed(False)
+    button.whileTrue(command1)
+    scheduler.run()
+    assert startCounter == 0
+    assert endCounter == 0
+    button.setPressed(True)
+    scheduler.run()
+    scheduler.run()
+    assert startCounter == 1
+    assert endCounter == 0
+    button.setPressed(False)
+    scheduler.run()
+    assert startCounter == 1
+    assert endCounter == 1
+
+
+def test_toggleOnTrue(scheduler: commands2.CommandScheduler):
+    startCounter = OOInteger(0)
+    endCounter = OOInteger(0)
+
+    command1 = commands2.StartEndCommand(
+        startCounter.incrementAndGet, endCounter.incrementAndGet
+    )
+
+    button = InternalButton()
+    button.setPressed(False)
+    button.toggleOnTrue(command1)
+    scheduler.run()
+    assert startCounter == 0
+    assert endCounter == 0
+    button.setPressed(True)
+    scheduler.run()
+    scheduler.run()
+    assert startCounter == 1
+    assert endCounter == 0
+    button.setPressed(False)
+    scheduler.run()
+    assert startCounter == 1
+    assert endCounter == 0
+    button.setPressed(True)
+    scheduler.run()
+    assert startCounter == 1
+    assert endCounter == 1
+
+
+def test_cancelWhenActive(scheduler: commands2.CommandScheduler):
+    startCounter = OOInteger(0)
+    endCounter = OOInteger(0)
+
+    button = InternalButton()
+    command1 = commands2.StartEndCommand(
+        startCounter.incrementAndGet, endCounter.incrementAndGet
+    ).until(button)
+
+    button.setPressed(False)
+    command1.schedule()
+    scheduler.run()
+    assert startCounter == 1
+    assert endCounter == 0
+    button.setPressed(True)
+    scheduler.run()
+    assert startCounter == 1
+    assert endCounter == 1
+    scheduler.run()
+    assert startCounter == 1
+    assert endCounter == 1
+
+
+def test_triggerComposition():
+    button1 = InternalButton()
+    button2 = InternalButton()
+
+    button1.setPressed(True)
+    button2.setPressed(False)
+
+    assert button1.and_(button2).getAsBoolean() == False
+    assert button1.or_(button2)() == True
+    assert bool(button1.negate()) == False
+    assert (button1 & ~button2)() == True
+
+
+def test_triggerCompositionSupplier():
+    button1 = InternalButton()
+    supplier = lambda: False
+
+    button1.setPressed(True)
+
+    assert button1.and_(supplier)() == False
+    assert button1.or_(supplier)() == True
+
+
+def test_debounce(scheduler: commands2.CommandScheduler):
+    command = commands2.Command()
+    start_spying_on(command)
+
+    button = InternalButton()
+    debounced = button.debounce(0.1)
+
+    debounced.onTrue(command)
+
+    button.setPressed(True)
+    scheduler.run()
+
+    verify(command, never()).schedule()
+
+    stepTiming(0.3)
+
+    button.setPressed(True)
+    scheduler.run()
+    verify(command).schedule()
+
+
+def test_booleanSupplier():
+    button = InternalButton()
+
+    assert button() == False
+    button.setPressed(True)
+    assert button() == True

commandsv2/src/test/python/test_waitcommand.py

@@ -0,0 +1,50 @@
+from typing import TYPE_CHECKING
+
+import commands2
+from util import *  # type: ignore
+
+if TYPE_CHECKING:
+    from .util import *
+
+import pytest
+
+
+def test_waitCommand(scheduler: commands2.CommandScheduler):
+    with ManualSimTime() as sim:
+        waitCommand = commands2.WaitCommand(2)
+
+        scheduler.schedule(waitCommand)
+        scheduler.run()
+        sim.step(1)
+        scheduler.run()
+
+        assert scheduler.isScheduled(waitCommand)
+
+        sim.step(2)
+
+        scheduler.run()
+
+        assert not scheduler.isScheduled(waitCommand)
+
+
+def test_withTimeout(scheduler: commands2.CommandScheduler):
+    with ManualSimTime() as sim:
+        command1 = commands2.Command()
+        start_spying_on(command1)
+
+        timeout = command1.withTimeout(2)
+
+        scheduler.schedule(timeout)
+        scheduler.run()
+
+        verify(command1).initialize()
+        verify(command1).execute()
+        assert not scheduler.isScheduled(command1)
+        assert scheduler.isScheduled(timeout)
+
+        sim.step(3)
+        scheduler.run()
+
+        verify(command1).end(True)
+        verify(command1, never()).end(False)
+        assert not scheduler.isScheduled(timeout)

commandsv2/src/test/python/test_waituntilcommand.py

@@ -0,0 +1,22 @@
+from typing import TYPE_CHECKING
+
+import commands2
+from util import *  # type: ignore
+
+if TYPE_CHECKING:
+    from .util import *
+
+import pytest
+
+
+def test_waitUntil(scheduler: commands2.CommandScheduler):
+    condition = OOBoolean()
+
+    command = commands2.WaitUntilCommand(condition)
+
+    scheduler.schedule(command)
+    scheduler.run()
+    assert scheduler.isScheduled(command)
+    condition.set(True)
+    scheduler.run()
+    assert not scheduler.isScheduled(command)

commandsv2/src/test/python/util.py

@@ -0,0 +1,294 @@
+from typing import Any, Dict, TypeVar, Type
+
+import inspect
+
+import commands2
+from wpilib.simulation import DriverStationSim, pauseTiming, resumeTiming, stepTiming
+
+
+Y = TypeVar("Y")
+
+
+def full_subclass_of(cls: Type[Y]) -> Type[Y]:
+    # Pybind classes can't be monkeypatched.
+    # This generates a subclass with every method filled out
+    # so that it can be monkeypatched.
+    retlist = []
+    clsname = cls.__name__  # + "_Subclass"
+    classdef = f"class {clsname}(cls):\n"
+    for name in dir(cls):
+        # for name in set(dir(cls)):
+        value = getattr(cls, name)
+        if callable(value) and not inspect.isclass(value) and not name.startswith("_"):
+            classdef += f"  def {name}(self, *args, **kwargs):\n"
+            classdef += f"    return super().{name}(*args, **kwargs)\n"
+    classdef += "  ...\n"
+    classdef += f"retlist.append({clsname})\n"
+    print(classdef)
+    exec(classdef, globals(), locals())
+    return retlist[0]
+
+
+class ManualSimTime:
+    def __enter__(self) -> "ManualSimTime":
+        pauseTiming()
+        return self
+
+    def __exit__(self, *args):
+        resumeTiming()
+
+    def step(self, delta: float):
+        stepTiming(delta)
+
+
+class OOInteger:
+    def __init__(self, value: int = 0) -> None:
+        self.value = value
+
+    def get(self) -> int:
+        return self.value
+
+    def set(self, value: int):
+        self.value = value
+
+    def incrementAndGet(self) -> int:
+        self.value += 1
+        return self.value
+
+    def addAndGet(self, value: int) -> int:
+        self.value += value
+        return self.value
+
+    def __eq__(self, value: float) -> bool:
+        return self.value == value
+
+    def __lt__(self, value: float) -> bool:
+        return self.value < value
+
+    def __call__(self) -> int:
+        return self.value
+
+
+class OOBoolean:
+    def __init__(self, value: bool = False) -> None:
+        self.value = value
+
+    def get(self) -> bool:
+        return self.value
+
+    def set(self, value: bool):
+        self.value = value
+
+    def __eq__(self, value: object) -> bool:
+        return self.value == value
+
+    def __bool__(self) -> bool:
+        return self.value
+
+    def __call__(self) -> bool:
+        return self.value
+
+
+class InternalButton(commands2.button.Trigger):
+    def __init__(self):
+        super().__init__(self.isPressed)
+        self.pressed = False
+
+    def isPressed(self) -> bool:
+        return self.pressed
+
+    def setPressed(self, value: bool):
+        self.pressed = value
+
+    def __call__(self) -> bool:
+        return self.pressed
+
+
+class OOFloat:
+    def __init__(self, value: float = 0.0) -> None:
+        self.value = value
+
+    def get(self) -> float:
+        return self.value
+
+    def set(self, value: float):
+        self.value = value
+
+    def incrementAndGet(self) -> float:
+        self.value += 1
+        return self.value
+
+    def addAndGet(self, value: float) -> float:
+        self.value += value
+        return self.value
+
+    def __eq__(self, value: float) -> bool:
+        return self.value == value
+
+    def __lt__(self, value: float) -> bool:
+        return self.value < value
+
+    def __call__(self) -> float:
+        return self.value
+
+    def __name__(self) -> str:
+        return "OOFloat"
+
+
+##########################################
+# Fakito Framework
+
+
+def _get_all_args_as_kwargs(method, *args, **kwargs) -> Dict[str, Any]:
+    try:
+        import inspect
+
+        method_args = inspect.getcallargs(method, *args, **kwargs)
+
+        method_arg_names = list(inspect.signature(method).parameters.keys())
+
+        for idx, arg in enumerate(args):
+            method_args[method_arg_names[idx]] = arg
+
+        try:
+            del method_args["self"]
+        except KeyError:
+            pass
+        return method_args
+    except TypeError:
+        # Pybind methods can't be inspected
+        # The exact args/kwargs that are passed in are checked instead
+        r = {}
+        for idx, arg in enumerate(args):
+            r[idx] = arg
+        r.update(kwargs)
+        return r
+
+
+class MethodWrapper:
+    def __init__(self, method):
+        self.method = method
+        self.og_method = method
+        self.times_called = 0
+        self.call_log = []
+
+    def __call__(self, *args, **kwargs):
+        self.times_called += 1
+        method_args = _get_all_args_as_kwargs(self.method, *args, **kwargs)
+        self.call_log.append(method_args)
+        return self.method(*args, **kwargs)
+
+    def called_with(self, *args, **kwargs):
+        return _get_all_args_as_kwargs(self.method, *args, **kwargs) in self.call_log
+
+    def times_called_with(self, *args, **kwargs):
+        return self.call_log.count(
+            _get_all_args_as_kwargs(self.method, *args, **kwargs)
+        )
+
+
+def start_spying_on(obj: Any) -> None:
+    """
+    Mocks all methods on an object, so that that call info can be used in asserts.
+
+    Example:
+    ```
+    obj = SomeClass()
+    start_spying_on(obj)
+    obj.method()
+    obj.method = lambda: None # supports monkeypatching
+    assert obj.method.times_called == 2
+    assert obj.method.called_with(arg1=1, arg2=2)
+    assert obj.method.times_called_with(arg1=1, arg2=2) == 2
+    ```
+    """
+
+    for name in dir(obj):
+        value = getattr(obj, name)
+        if callable(value) and not inspect.isclass(value) and not name.startswith("_"):
+            setattr(obj, name, MethodWrapper(value))
+
+    if not hasattr(obj.__class__, "_is_being_spied_on"):
+        try:
+            old_setattr = obj.__class__.__setattr__
+        except AttributeError:
+            old_setattr = object.__setattr__
+
+        def _setattr(self, name, value):
+            if name in dir(self):
+                existing_value = getattr(self, name)
+                if isinstance(existing_value, MethodWrapper):
+                    existing_value.method = value
+                    return
+            old_setattr(self, name, value)
+
+        obj.__class__.__setattr__ = _setattr
+        obj.__class__._is_being_spied_on = True
+
+
+# fakito verify
+def reset(obj: Any) -> None:
+    """
+    Resets the call log of all mocked methods on an object.
+    Also restores all monkeypatched methods.
+    """
+    for name in dir(obj):
+        value = getattr(obj, name)
+        if isinstance(value, MethodWrapper):
+            value.method = value.og_method
+            value.times_called = 0
+            value.call_log = []
+
+
+class times:
+    def __init__(self, times: int) -> None:
+        self.times = times
+
+
+def never() -> times:
+    return times(0)
+
+
+class _verify:
+    def __init__(self, obj: Any, times: times = times(1)):
+        self.obj = obj
+        self.times = times.times
+
+    def __getattribute__(self, name: str) -> Any:
+        def self_dot(name: str):
+            return super(_verify, self).__getattribute__(name)
+
+        def times_string(times: int) -> str:
+            if times == 1:
+                return "1 time"
+            else:
+                return f"{times} times"
+
+        def check(*args, **kwargs):
+            __tracebackhide__ = True
+            # import code
+            # code.interact(local={**globals(), **locals()})
+            method = getattr(self_dot("obj"), name)
+            # method = getattr(self1.obj, name)
+            assert method.times_called_with(*args, **kwargs) == self_dot(
+                "times"
+            ), f"Expected {name} to be called {times_string(self_dot('times'))} with {args} {kwargs}, but was called {times_string(method.times_called_with(*args, **kwargs))}"
+
+        return check
+
+
+T = TypeVar("T")
+
+
+def verify(obj: T, times: times = times(1)) -> T:
+    # import code
+    # code.interact(local={**globals(), **locals()})
+    return _verify(obj, times)  # type: ignore
+
+
+def patch_via_decorator(obj: Any):
+    def decorator(method):
+        setattr(obj, method.__name__, method.__get__(obj, obj.__class__))
+        return method
+
+    return decorator