Adds PIDSubsystem and TrapezoidProfileSubsystem to Commands2.

commands2/__init__.py

@@ -31,6 +31,7 @@
 from .parallelracegroup import ParallelRaceGroup
 from .perpetualcommand import PerpetualCommand
 from .pidcommand import PIDCommand
+from .pidsubsystem import PIDSubsystem
 from .printcommand import PrintCommand
 from .proxycommand import ProxyCommand
 from .proxyschedulecommand import ProxyScheduleCommand
@@ -42,6 +43,7 @@
 from .startendcommand import StartEndCommand
 from .subsystem import Subsystem
 from .timedcommandrobot import TimedCommandRobot
+from .trapezoidprofilesubsystem import TrapezoidProfileSubsystem
 from .waitcommand import WaitCommand
 from .waituntilcommand import WaitUntilCommand
 from .wrappercommand import WrapperCommand
@@ -62,6 +64,7 @@
     "ParallelRaceGroup",
     "PerpetualCommand",
     "PIDCommand",
+    "PIDSubsystem",
     "PrintCommand",
     "ProxyCommand",
     "ProxyScheduleCommand",
@@ -73,6 +76,7 @@
     "StartEndCommand",
     "Subsystem",
     "TimedCommandRobot",
+    "TrapezoidProfileSubsystem",
     "WaitCommand",
     "WaitUntilCommand",
     "WrapperCommand",

commands2/pidsubsystem.py

@@ -0,0 +1,99 @@
+# 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 {@link 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("Subclasses must implement this method")
+
+    def getMeasurement(self) -> float:
+        """
+        Returns the measurement of the process variable used by the PIDController.
+
+        :return: The measurement of the process variable.
+        """
+        raise NotImplementedError("Subclasses must implement this method")
+
+    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

commands2/trapezoidprofilesubsystem.py

@@ -0,0 +1,75 @@
+# 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 Union
+
+from .subsystem import Subsystem
+from wpimath.trajectory import TrapezoidProfile
+
+
+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.
+    """
+
+    def __init__(
+        self,
+        constraints: TrapezoidProfile.Constraints,
+        initial_position: float = 0.0,
+        period: float = 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.
+        """
+        self._profile = TrapezoidProfile(constraints)
+        self._state = TrapezoidProfile.State(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._goal, self._state)
+        if self._enabled:
+            self.useState(self._state)
+
+    def setGoal(self, goal: Union[TrapezoidProfile.State, float]):
+        """
+        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 = TrapezoidProfile.State(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: TrapezoidProfile.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("Subclasses must implement this method")