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

[copybara] Resync commandsv2 (#8535)

Browse Source 
This will be the last sync from the commandsv2
[repo](https://github.com/robotpy/robotpy-commands-v2), as it has now
been merged into
[mostrobotpy](https://github.com/robotpy/mostrobotpy/pull/226).

Doing this on its own to make the diffs smaller when doing a full
`mostrobotpy` sync, which will now have a lot of other new goodies
coming along for the ride.

---------

Co-authored-by: Default email <default@default.com>
This commit is contained in:
PJ Reiniger
2026-01-14 23:15:00 -05:00
committed by GitHub
parent 996af272e8
commit 1cbc25bc36
11 changed files with 1 additions and 901 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
commandsv2/src/main/python/commands2/__init__.py
View File

@@ -14,11 +14,7 @@ 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
@@ -28,8 +24,6 @@ from .sequentialcommandgroup import SequentialCommandGroup
from .startendcommand import StartEndCommand
from .subsystem import Subsystem
from .timedcommandrobot import TimedCommandRobot
from .trapezoidprofilecommand import TrapezoidProfileCommand
from .trapezoidprofilesubsystem import TrapezoidProfileSubsystem
from .waitcommand import WaitCommand
from .waituntilcommand import WaitUntilCommand
from .wrappercommand import WrapperCommand
@@ -51,11 +45,7 @@ __all__ = [
"ParallelCommandGroup",
"ParallelDeadlineGroup",
"ParallelRaceGroup",
"PIDCommand",
"PIDSubsystem",
"PrintCommand",
"ProfiledPIDCommand",
"ProfiledPIDSubsystem",
"ProxyCommand",
"RepeatCommand",
"RunCommand",
@@ -65,8 +55,6 @@ __all__ = [
"StartEndCommand",
"Subsystem",
"TimedCommandRobot",
"TrapezoidProfileCommand",
"TrapezoidProfileSubsystem",
"WaitCommand",
"WaitUntilCommand",
"WrapperCommand",

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

@@ -1,78 +0,0 @@
# 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 import PIDController
class PIDCommand(Command):
"""
A command that controls an output with a :class:`wpimath.controller.PIDController`. Runs forever by default - to add
exit conditions and/or other behavior, subclass this class. The controller calculation and output
are performed synchronously in the command's execute() method.
"""
def __init__(
self,
controller: PIDController,
measurementSource: Callable[[], float],
setpoint: Union[Callable[[], float], float, int],
useOutput: Callable[[float], Any],
*requirements: Subsystem,
):
"""
Creates a new PIDCommand, which controls the given output with a :class:`wpimath.controller.PIDController`.
:param controller: the controller that controls the output.
:param measurementSource: the measurement of the process variable
:param setpoint: the controller's setpoint (either a function that returns a)
number or a number
:param useOutput: the controller's output
:param requirements: the subsystems required by this command
"""
super().__init__()
assert callable(measurementSource)
assert callable(useOutput)
self._controller = controller
self._useOutput = useOutput
self._measurement = measurementSource
if isinstance(setpoint, (float, int)):
setpoint = float(setpoint)
self._setpoint = lambda: setpoint
elif callable(setpoint):
self._setpoint = setpoint
else:
raise ValueError(
f"invalid setpoint (must be callable or number; got {type(setpoint)})"
)
self.addRequirements(*requirements)
def initialize(self):
self._controller.reset()
def execute(self):
self._useOutput(
self._controller.calculate(self._measurement(), self._setpoint())
)
def end(self, interrupted):
self._useOutput(0)
def getController(self):
"""
Returns the PIDController used by the command.
:return: The PIDController
"""
return self._controller

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

@@ -1,100 +0,0 @@
# 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 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

80
commandsv2/src/main/python/commands2/profiledpidcommand.py
View File

@@ -1,80 +0,0 @@
# 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 import ProfiledPIDController, ProfiledPIDControllerRadians, TrapezoidProfile, TrapezoidProfileRadians
from .command import Command
from .subsystem import Subsystem
from .typing import (
FloatOrFloatSupplier,
FloatSupplier,
TProfiledPIDController,
UseOutputFunction,
)
class ProfiledPIDCommand(Command, Generic[TProfiledPIDController]):
"""A command that controls an output with a :class:`.ProfiledPIDController`. Runs forever by default -
to add exit conditions and/or other behavior, subclass this class. The controller calculation and
output are performed synchronously in the command's execute() method.
"""
_stateCls: Any
def __init__(
self,
controller: TProfiledPIDController,
measurementSource: FloatSupplier,
goalSource: FloatOrFloatSupplier,
useOutput: UseOutputFunction,
*requirements: Subsystem,
):
"""Creates a new ProfiledPIDCommand, which controls the given output with a ProfiledPIDController. Goal
velocity is specified.
:param controller: the controller that controls the output.
:param measurementSource: the measurement of the process variable
:param goalSource: the controller's goal
:param useOutput: the controller's output
:param requirements: the subsystems required by this command
"""
super().__init__()
if isinstance(controller, ProfiledPIDController):
self._stateCls = TrapezoidProfile.State
elif isinstance(controller, ProfiledPIDControllerRadians):
self._stateCls = TrapezoidProfileRadians.State
else:
raise ValueError(f"unknown controller type {controller!r}")
self._controller: TProfiledPIDController = controller
self._useOutput = useOutput
self._measurement = measurementSource
if isinstance(goalSource, (float, int)):
self._goal = lambda: float(goalSource)
else:
self._goal = goalSource
self.addRequirements(*requirements)
def initialize(self):
self._controller.reset(self._measurement())
def execute(self):
self._useOutput.accept(
self._controller.calculate(self._measurement(), self._goal()),
self._controller.getSetpoint(),
)
def end(self, interrupted: bool):
self._useOutput(0.0, self._stateCls())
def getController(self):
"""Gets the controller used by the command"""
return self._controller

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

@@ -1,81 +0,0 @@
# Copyright (c) FIRST and other WPILib contributors.
# Open Source Software; you can modify and/or share it under the terms of
# the WPILib BSD license file in the root directory of this project.
from typing import Generic
from wpimath 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

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

@@ -1,64 +0,0 @@
# 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())

93
commandsv2/src/main/python/commands2/trapezoidprofilesubsystem.py
View File

@@ -1,93 +0,0 @@
# 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 TrapezoidProfile, TrapezoidProfileRadians, units
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")

24
commandsv2/src/main/python/commands2/typing.py
View File

@@ -1,28 +1,6 @@
from typing import Callable, Protocol, TypeVar, Union
from typing import Callable, Union
from typing_extensions import TypeAlias
from wpimath import ProfiledPIDController, ProfiledPIDControllerRadians, 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]