From 0cfa3b446d5ccf76d5ff4d1ec0d8c4bae9dd5ad7 Mon Sep 17 00:00:00 2001 From: Chadwick Stryker Date: Wed, 24 Jan 2024 20:20:19 -0800 Subject: [PATCH 1/4] Created a generatic profiled PID controller var type and applied it to the profiled PID subsystem. --- commands2/profiledpidsubsystem.py | 11 ++++++----- commands2/typing.py | 7 +++++++ 2 files changed, 13 insertions(+), 5 deletions(-) create mode 100644 commands2/typing.py diff --git a/commands2/profiledpidsubsystem.py b/commands2/profiledpidsubsystem.py index f2c7069e..6b201bf1 100644 --- a/commands2/profiledpidsubsystem.py +++ b/commands2/profiledpidsubsystem.py @@ -2,14 +2,15 @@ # 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 Union, cast +from typing import Generic from wpimath.trajectory import TrapezoidProfile from .subsystem import Subsystem +from .typing import GenericProfiledPIDController -class ProfiledPIDSubsystem(Subsystem): +class ProfiledPIDSubsystem(Subsystem, Generic[GenericProfiledPIDController]): """ A subsystem that uses a :class:`wpimath.controller.ProfiledPIDController` or :class:`wpimath.controller.ProfiledPIDControllerRadians` to @@ -19,12 +20,12 @@ class ProfiledPIDSubsystem(Subsystem): def __init__( self, - controller, + controller: GenericProfiledPIDController, initial_position: float = 0, ): """Creates a new PIDSubsystem.""" super().__init__() - self._controller = controller + self._controller: GenericProfiledPIDController = controller self._enabled = False self.setGoal(initial_position) @@ -38,7 +39,7 @@ def periodic(self): def getController( self, - ): + ) -> GenericProfiledPIDController: """Returns the controller""" return self._controller diff --git a/commands2/typing.py b/commands2/typing.py new file mode 100644 index 00000000..9835e809 --- /dev/null +++ b/commands2/typing.py @@ -0,0 +1,7 @@ +from typing import TypeVar + +from wpimath.controller import ProfiledPIDController, ProfiledPIDControllerRadians + +GenericProfiledPIDController = TypeVar( + "GenericProfiledPIDController", ProfiledPIDControllerRadians, ProfiledPIDController +) From e6132d53aa4a0c1bae162f4b5f98ea8b22fb003d Mon Sep 17 00:00:00 2001 From: Chadwick Stryker Date: Wed, 7 Feb 2024 21:03:46 -0800 Subject: [PATCH 2/4] Changed GenericProfiledPIDController to TProfiledPIDController follow Python naming convention. Added Generic annotations and a Protocol to profiledpidcommand.py. --- commands2/profiledpidcommand.py | 18 ++++++++++-------- commands2/profiledpidsubsystem.py | 10 +++++----- commands2/typing.py | 21 ++++++++++++++++++--- 3 files changed, 33 insertions(+), 16 deletions(-) diff --git a/commands2/profiledpidcommand.py b/commands2/profiledpidcommand.py index c2ece076..232eabf8 100644 --- a/commands2/profiledpidcommand.py +++ b/commands2/profiledpidcommand.py @@ -5,16 +5,17 @@ # the WPILib BSD license file in the root directory of this project. # -from typing import Any, Callable, Union - -from .command import Command -from .subsystem import Subsystem +from typing import Any, Callable, Generic, Union from wpimath.controller import ProfiledPIDController, ProfiledPIDControllerRadians from wpimath.trajectory import TrapezoidProfile, TrapezoidProfileRadians +from .command import Command +from .subsystem import Subsystem +from .typing import TProfiledPIDController, UseOutputFunction + -class ProfiledPIDCommand(Command): +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. @@ -24,10 +25,10 @@ class ProfiledPIDCommand(Command): def __init__( self, - controller, + controller: TProfiledPIDController, measurementSource: Callable[[], float], goalSource: Union[float, Callable[[], float]], - useOutput: Callable[[float, Any], Any], + useOutput: UseOutputFunction, *requirements: Subsystem, ): """Creates a new ProfiledPIDCommand, which controls the given output with a ProfiledPIDController. Goal @@ -40,6 +41,7 @@ def __init__( :param requirements: the subsystems required by this command """ + super().__init__() if isinstance(controller, ProfiledPIDController): self._stateCls = TrapezoidProfile.State elif isinstance(controller, ProfiledPIDControllerRadians): @@ -47,7 +49,7 @@ def __init__( else: raise ValueError(f"unknown controller type {controller!r}") - self._controller = controller + self._controller: TProfiledPIDController = controller self._useOutput = useOutput self._measurement = measurementSource if isinstance(goalSource, (float, int)): diff --git a/commands2/profiledpidsubsystem.py b/commands2/profiledpidsubsystem.py index 6b201bf1..2064d5be 100644 --- a/commands2/profiledpidsubsystem.py +++ b/commands2/profiledpidsubsystem.py @@ -7,10 +7,10 @@ from wpimath.trajectory import TrapezoidProfile from .subsystem import Subsystem -from .typing import GenericProfiledPIDController +from .typing import TProfiledPIDController -class ProfiledPIDSubsystem(Subsystem, Generic[GenericProfiledPIDController]): +class ProfiledPIDSubsystem(Subsystem, Generic[TProfiledPIDController]): """ A subsystem that uses a :class:`wpimath.controller.ProfiledPIDController` or :class:`wpimath.controller.ProfiledPIDControllerRadians` to @@ -20,12 +20,12 @@ class ProfiledPIDSubsystem(Subsystem, Generic[GenericProfiledPIDController]): def __init__( self, - controller: GenericProfiledPIDController, + controller: TProfiledPIDController, initial_position: float = 0, ): """Creates a new PIDSubsystem.""" super().__init__() - self._controller: GenericProfiledPIDController = controller + self._controller: TProfiledPIDController = controller self._enabled = False self.setGoal(initial_position) @@ -39,7 +39,7 @@ def periodic(self): def getController( self, - ) -> GenericProfiledPIDController: + ) -> TProfiledPIDController: """Returns the controller""" return self._controller diff --git a/commands2/typing.py b/commands2/typing.py index 9835e809..357b928b 100644 --- a/commands2/typing.py +++ b/commands2/typing.py @@ -1,7 +1,22 @@ -from typing import TypeVar +from typing import Protocol, TypeVar from wpimath.controller import ProfiledPIDController, ProfiledPIDControllerRadians +from wpimath.trajectory import TrapezoidProfile, TrapezoidProfileRadians -GenericProfiledPIDController = TypeVar( - "GenericProfiledPIDController", ProfiledPIDControllerRadians, ProfiledPIDController +TProfiledPIDController = TypeVar( + "TProfiledPIDController", ProfiledPIDControllerRadians, ProfiledPIDController ) +TTrapezoidProfileState = TypeVar( + "TTrapezoidProfileState", + TrapezoidProfileRadians.State, + TrapezoidProfile.State, +) + + +class UseOutputFunction(Protocol): + + def __init__(self): ... + + def __call__(self, t: float, u: TTrapezoidProfileState) -> None: ... + + def accept(self, t: float, u: TTrapezoidProfileState) -> None: ... From 910783cefef6b535aa62f54c50adbd9ab5478ec3 Mon Sep 17 00:00:00 2001 From: Chadwick Stryker Date: Wed, 7 Feb 2024 21:23:34 -0800 Subject: [PATCH 3/4] Added TTrapezoidProfile to profiledpidsubsystem.py --- commands2/profiledpidsubsystem.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/commands2/profiledpidsubsystem.py b/commands2/profiledpidsubsystem.py index 2064d5be..2e970baa 100644 --- a/commands2/profiledpidsubsystem.py +++ b/commands2/profiledpidsubsystem.py @@ -7,10 +7,12 @@ from wpimath.trajectory import TrapezoidProfile from .subsystem import Subsystem -from .typing import TProfiledPIDController +from .typing import TProfiledPIDController, TTrapezoidProfileState -class ProfiledPIDSubsystem(Subsystem, Generic[TProfiledPIDController]): +class ProfiledPIDSubsystem( + Subsystem, Generic[TProfiledPIDController, TTrapezoidProfileState] +): """ A subsystem that uses a :class:`wpimath.controller.ProfiledPIDController` or :class:`wpimath.controller.ProfiledPIDControllerRadians` to @@ -49,7 +51,7 @@ def setGoal(self, goal): """ self._controller.setGoal(goal) - def useOutput(self, output: float, setpoint: TrapezoidProfile.State): + def useOutput(self, output: float, setpoint: TTrapezoidProfileState): """ Uses the output from the controller object. """ From 604af69bc82534745cfdbfdafb3363292973d8ad Mon Sep 17 00:00:00 2001 From: Chadwick Stryker Date: Sat, 10 Feb 2024 06:28:54 -0800 Subject: [PATCH 4/4] Added more docstrings to ProfiledPIDController. Created and used TypeAliases (FloatSupplier & FloatOrFloatSupplier). Updated UseOutputFucntion protocol based on testing. --- commands2/__init__.py | 1 + commands2/profiledpidcommand.py | 13 +++++++++---- commands2/profiledpidsubsystem.py | 20 ++++++++++---------- commands2/typing.py | 12 ++++++++++-- 4 files changed, 30 insertions(+), 16 deletions(-) diff --git a/commands2/__init__.py b/commands2/__init__.py index 1c243a72..2b649507 100644 --- a/commands2/__init__.py +++ b/commands2/__init__.py @@ -2,6 +2,7 @@ from . import button from . import cmd +from . import typing from .commandscheduler import CommandScheduler from .conditionalcommand import ConditionalCommand diff --git a/commands2/profiledpidcommand.py b/commands2/profiledpidcommand.py index 232eabf8..d7ce298b 100644 --- a/commands2/profiledpidcommand.py +++ b/commands2/profiledpidcommand.py @@ -5,14 +5,19 @@ # the WPILib BSD license file in the root directory of this project. # -from typing import Any, Callable, Generic, Union +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 TProfiledPIDController, UseOutputFunction +from .typing import ( + FloatOrFloatSupplier, + FloatSupplier, + TProfiledPIDController, + UseOutputFunction, +) class ProfiledPIDCommand(Command, Generic[TProfiledPIDController]): @@ -26,8 +31,8 @@ class ProfiledPIDCommand(Command, Generic[TProfiledPIDController]): def __init__( self, controller: TProfiledPIDController, - measurementSource: Callable[[], float], - goalSource: Union[float, Callable[[], float]], + measurementSource: FloatSupplier, + goalSource: FloatOrFloatSupplier, useOutput: UseOutputFunction, *requirements: Subsystem, ): diff --git a/commands2/profiledpidsubsystem.py b/commands2/profiledpidsubsystem.py index 2e970baa..4a05baa7 100644 --- a/commands2/profiledpidsubsystem.py +++ b/commands2/profiledpidsubsystem.py @@ -25,7 +25,13 @@ def __init__( controller: TProfiledPIDController, initial_position: float = 0, ): - """Creates a new PIDSubsystem.""" + """ + 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 @@ -46,15 +52,11 @@ def getController( return self._controller def setGoal(self, goal): - """ - Sets the goal state for the subsystem. - """ + """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. - """ + """Uses the output from the controller object.""" raise NotImplementedError(f"{self.__class__} must implement useOutput") def getMeasurement(self) -> float: @@ -75,7 +77,5 @@ def disable(self): self.useOutput(0, TrapezoidProfile.State()) def isEnabled(self) -> bool: - """ - Returns whether the controller is enabled. - """ + """Returns whether the controller is enabled.""" return self._enabled diff --git a/commands2/typing.py b/commands2/typing.py index 357b928b..61683308 100644 --- a/commands2/typing.py +++ b/commands2/typing.py @@ -1,8 +1,10 @@ -from typing import Protocol, TypeVar +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 ) @@ -13,10 +15,16 @@ ) +# Protocols - Structural Typing class UseOutputFunction(Protocol): - def __init__(self): ... + 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]]