Adds TrapezoidProfileCommand

* robotpy#28

Co-authored-by: Dustin Spicuzza <[email protected]>

commands2/__init__.py

@@ -26,6 +26,7 @@
 from .subsystem import Subsystem
 from .swervecontrollercommand import SwerveControllerCommand
 from .timedcommandrobot import TimedCommandRobot
+from .trapezoidprofilecommand import TrapezoidProfileCommand
 from .trapezoidprofilesubsystem import TrapezoidProfileSubsystem
 from .waitcommand import WaitCommand
 from .waituntilcommand import WaitUntilCommand
@@ -61,6 +62,7 @@
     "Subsystem",
     "SwerveControllerCommand",
     "TimedCommandRobot",
+    "TrapezoidProfileCommand",
     "TrapezoidProfileSubsystem",
     "WaitCommand",
     "WaitUntilCommand",

commands2/trapezoidprofilecommand.py

@@ -0,0 +1,64 @@
+# validated: 2024-01-24 DS 192a28af4731 TrapezoidProfileCommand.java
+#
+# 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 Callable, Any
+
+from wpilib import Timer
+
+from .command import Command
+from .subsystem import Subsystem
+
+
+class TrapezoidProfileCommand(Command):
+    """
+    A command that runs a :class:`wpimath.trajectory.TrapezoidProfile`. Useful for smoothly controlling mechanism motion.
+    """
+
+    def __init__(
+        self,
+        profile,
+        output: Callable[[Any], Any],
+        goal: Callable[[], Any],
+        currentState: Callable[[], Any],
+        *requirements: Subsystem,
+    ):
+        """Creates a new TrapezoidProfileCommand that will execute the given :class:`wpimath.trajectory.TrapezoidProfile`.
+        Output will be piped to the provided consumer function.
+
+        :param profile:      The motion profile to execute.
+        :param output:       The consumer for the profile output.
+        :param goal:         The supplier for the desired state
+        :param currentState: The supplier for the current state
+        :param requirements: The subsystems required by this command.
+        """
+        super().__init__()
+        self._profile = profile
+        self._output = output
+        self._goal = goal
+        self._currentState = currentState
+        self._timer = Timer()
+
+        self.addRequirements(*requirements)
+
+    def initialize(self) -> None:
+        self._timer.restart()
+
+    def execute(self) -> None:
+        self._output(
+            self._profile.calculate(
+                self._timer.get(),
+                self._currentState(),
+                self._goal(),
+            )
+        )
+
+    def end(self, interrupted) -> None:
+        self._timer.stop()
+
+    def isFinished(self) -> bool:
+        return self._timer.hasElapsed(self._profile.totalTime())

tests/test_trapezoidprofilecommand.py

@@ -0,0 +1,142 @@
+# 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 typing import TYPE_CHECKING, List, Tuple
+import math
+
+import wpimath.controller as controller
+import wpimath.trajectory as trajectory
+import wpimath.geometry as geometry
+import wpimath.kinematics as kinematics
+from wpimath.trajectory import TrapezoidProfile as DimensionlessProfile
+from wpimath.trajectory import TrapezoidProfileRadians as RadiansProfile
+
+from wpilib import Timer
+
+from util import *  # type: ignore
+
+if TYPE_CHECKING:
+    from .util import *
+
+import pytest
+
+import commands2
+
+
+class TrapezoidProfileRadiansFixture:
+    def __init__(self):
+        constraints: RadiansProfile.Constraints = RadiansProfile.Constraints(
+            3 * math.pi, math.pi
+        )
+        self._profile: RadiansProfile = RadiansProfile(constraints)
+        self._goal_state = RadiansProfile.State(3, 0)
+
+        self._state = self._profile.calculate(
+            0, self._goal_state, RadiansProfile.State(0, 0)
+        )
+
+        self._timer = Timer()
+
+    def profileOutput(self, state: RadiansProfile.State) -> None:
+        self._state = state
+
+    def currentState(self) -> RadiansProfile.State:
+        return self._state
+
+    def getGoal(self) -> RadiansProfile.State:
+        return self._goal_state
+
+
+@pytest.fixture()
+def get_trapezoid_profile_radians() -> TrapezoidProfileRadiansFixture:
+    return TrapezoidProfileRadiansFixture()
+
+
+class TrapezoidProfileFixture:
+    def __init__(self):
+        constraints: DimensionlessProfile.Constraints = (
+            DimensionlessProfile.Constraints(3 * math.pi, math.pi)
+        )
+        self._profile: DimensionlessProfile = DimensionlessProfile(constraints)
+        self._goal_state = DimensionlessProfile.State(3, 0)
+
+        self._state = self._profile.calculate(
+            0, self._goal_state, DimensionlessProfile.State(0, 0)
+        )
+
+        self._timer = Timer()
+
+    def profileOutput(self, state: DimensionlessProfile.State) -> None:
+        self._state = state
+
+    def currentState(self) -> DimensionlessProfile.State:
+        return self._state
+
+    def getGoal(self) -> DimensionlessProfile.State:
+        return self._goal_state
+
+
+@pytest.fixture()
+def get_trapezoid_profile_dimensionless() -> TrapezoidProfileFixture:
+    return TrapezoidProfileFixture()
+
+
+def test_trapezoidProfileDimensionless(
+    scheduler: commands2.CommandScheduler, get_trapezoid_profile_dimensionless
+):
+    with ManualSimTime() as sim:
+        subsystem = commands2.Subsystem()
+
+        fixture_data = get_trapezoid_profile_dimensionless
+
+        command = commands2.TrapezoidProfileCommand(
+            fixture_data._profile,
+            fixture_data.profileOutput,
+            fixture_data.getGoal,
+            fixture_data.currentState,
+            subsystem,
+        )
+
+        fixture_data._timer.restart()
+
+        command.initialize()
+
+        count = 0
+        while not command.isFinished():
+            command.execute()
+            count += 1
+            sim.step(0.005)
+
+        fixture_data._timer.stop()
+        command.end(True)
+
+
+def test_trapezoidProfileRadians(
+    scheduler: commands2.CommandScheduler, get_trapezoid_profile_radians
+):
+    with ManualSimTime() as sim:
+        subsystem = commands2.Subsystem()
+
+        fixture_data = get_trapezoid_profile_radians
+
+        command = commands2.TrapezoidProfileCommand(
+            fixture_data._profile,
+            fixture_data.profileOutput,
+            fixture_data.getGoal,
+            fixture_data.currentState,
+            subsystem,
+        )
+
+        fixture_data._timer.restart()
+
+        command.initialize()
+
+        count = 0
+        while not command.isFinished():
+            command.execute()
+            count += 1
+            sim.step(0.005)
+
+        fixture_data._timer.stop()
+        command.end(True)