Ported the ProfiledPIDSubsystem from the wpilib java source to Python

Author: cwstryker

commands2/__init__.py

@@ -33,6 +33,7 @@
 from .pidcommand import PIDCommand
 from .pidsubsystem import PIDSubsystem
 from .printcommand import PrintCommand
+from .profiledpidsubsystem import ProfiledPIDSubsystem
 from .proxycommand import ProxyCommand
 from .proxyschedulecommand import ProxyScheduleCommand
 from .repeatcommand import RepeatCommand

commands2/profiledpidsubsystem.py

@@ -0,0 +1,62 @@
+# 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 wpimath.controller import ProfiledPIDController
+from .subsystem import Subsystem
+from wpimath.trajectory import TrapezoidProfile
+
+
+class ProfiledPIDSubsystem(Subsystem):
+    def __init__(self, controller: ProfiledPIDController, initial_position: float = 0):
+        super().__init__()
+        self._m_controller = controller
+        self._m_enabled = False
+        self.setGoal(initial_position)
+
+    def periodic(self):
+        """Updates the output of the controller."""
+        if self._m_enabled:
+            self._useOutput(self._m_controller.calculate(self._getMeasurement()),
+                            self._m_controller.getSetpoint())
+
+    def get_controller(self) -> ProfiledPIDController:
+        """Returns the ProfiledPIDController."""
+        return self._m_controller
+
+    def setGoal(self, goal: TrapezoidProfile.State | float):
+        """
+        Sets the goal state for the subsystem.
+        """
+        if isinstance(goal, TrapezoidProfile):
+            self._m_controller.setGoal(goal)
+        else:
+            self._m_controller.setGoal(TrapezoidProfile.State(goal, 0))
+
+    def _useOutput(self, output: float, setpoint: TrapezoidProfile.State):
+        """
+        Uses the output from the ProfiledPIDController.
+        """
+        raise NotImplementedError("Subclasses must implement this method")
+
+    def _getMeasurement(self) -> float:
+        """
+        Returns the measurement of the process variable used by the ProfiledPIDController.
+        """
+        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(self._getMeasurement())
+
+    def disable(self):
+        """Disables the PID control. Sets output to zero."""
+        self._m_enabled = False
+        self._useOutput(0, TrapezoidProfile.State())
+
+    def isEnabled(self) -> bool:
+        """
+        Returns whether the controller is enabled.
+        """
+        return self._m_enabled