diff --git a/commands2/__init__.py b/commands2/__init__.py
index cc8e6a4c..4d9c2e8c 100644
--- a/commands2/__init__.py
+++ b/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",
diff --git a/commands2/pidsubsystem.py b/commands2/pidsubsystem.py
new file mode 100644
index 00000000..9b0ec3b5
--- /dev/null
+++ b/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
diff --git a/commands2/trapezoidprofilesubsystem.py b/commands2/trapezoidprofilesubsystem.py
new file mode 100644
index 00000000..8e687d1b
--- /dev/null
+++ b/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")