Added PIDSubsystem to Commands2. robotpy#28

Author: NewtonCrosby

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