From 4ceae137be3d77e3cf6f302fdec2acdbff590352 Mon Sep 17 00:00:00 2001 From: NewtonCrosby Date: Wed, 6 Dec 2023 08:32:26 -0500 Subject: [PATCH] Adds PIDSubsystem and TrapezoidProfileSubsystem to Commands2. robotpy/robotpy-commands-v2#28 --- commands2/__init__.py | 4 ++ commands2/pidsubsystem.py | 99 ++++++++++++++++++++++++++ commands2/trapezoidprofilesubsystem.py | 75 +++++++++++++++++++ 3 files changed, 178 insertions(+) create mode 100644 commands2/pidsubsystem.py create mode 100644 commands2/trapezoidprofilesubsystem.py 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..c63469a3 --- /dev/null +++ b/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 diff --git a/commands2/trapezoidprofilesubsystem.py b/commands2/trapezoidprofilesubsystem.py new file mode 100644 index 00000000..2b40fd53 --- /dev/null +++ b/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")