From b645b6b5c275d9d005746829846a8af516ba466a 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 | 91 ++++++++++++++++++++++++++ commands2/trapezoidprofilesubsystem.py | 73 +++++++++++++++++++++ 3 files changed, 168 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..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")