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")