diff --git a/commands2/__init__.py b/commands2/__init__.py index 445638be..2d605373 100644 --- a/commands2/__init__.py +++ b/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", diff --git a/commands2/trapezoidprofilecommand.py b/commands2/trapezoidprofilecommand.py new file mode 100644 index 00000000..4ffff24e --- /dev/null +++ b/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()) diff --git a/tests/test_trapezoidprofilecommand.py b/tests/test_trapezoidprofilecommand.py new file mode 100644 index 00000000..730fc821 --- /dev/null +++ b/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)