Adds TrapezoidProfileSubsystem to Commands2. robotpy#28

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