Adds PIDSubsystem and TrapezoidProfileSubsystem to Commands2. robotpy#28

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,91 @@
+# 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):
+    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.
+        """
+        self.m_controller: PIDController = controller
+        self.setSetpoint(initial_position)
+        self.addChild("PID Controller", self.m_controller)
+        self.m_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.m_enabled:
+            self.useOutput(
+                self.m_controller.calculate(self.getMeasurement()), self.getSetpoint()
+            )
+
+    def getController(self) -> PIDController:
+        """
+        Returns the PIDController used by the subsystem.
+
+        :return: The PIDController.
+        """
+        return self.m_controller
+
+    def setSetpoint(self, setpoint: float):
+        """
+        Sets the setpoint for the subsystem.
+
+        :param setpoint: The setpoint for the subsystem.
+        """
+        self.m_controller.setSetpoint(setpoint)
+
+    def getSetpoint(self) -> float:
+        """
+        Returns the current setpoint of the subsystem.
+
+        :return: The current setpoint.
+        """
+        return self.m_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.m_enabled = True
+        self.m_controller.reset()
+
+    def disable(self):
+        """Disables the PID control. Sets output to zero."""
+        self.m_enabled = False
+        self.useOutput(0, 0)
+
+    def isEnabled(self) -> bool:
+        """
+        Returns whether the controller is enabled.
+
+        :return: Whether the controller is enabled.
+        """
+        return self.m_enabled

commands2/trapezoidprofilesubsystem.py

@@ -0,0 +1,73 @@
+# 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 .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.
+
+    This class is provided by the NewCommands VendorDep
+    """
+    def __init__(
+            self, constraints: TrapezoidProfile.Constraints, 
+            initial_position: float, 
+            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: TrapezoidProfile.State):
+        """
+        Sets the goal state for the subsystem.
+
+        :param goal: The goal state for the subsystem's motion profile.
+        """
+        self.goal = goal
+
+    def setGoal(self, goal: 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.
+        """
+        self.setGoal(TrapezoidProfile.State(goal, 0))
+
+    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")