diff --git a/commands2/__init__.py b/commands2/__init__.py index 1823bf1..cac0120 100644 --- a/commands2/__init__.py +++ b/commands2/__init__.py @@ -14,11 +14,7 @@ from .parallelcommandgroup import ParallelCommandGroup from .paralleldeadlinegroup import ParallelDeadlineGroup from .parallelracegroup import ParallelRaceGroup -from .pidcommand import PIDCommand -from .pidsubsystem import PIDSubsystem from .printcommand import PrintCommand -from .profiledpidcommand import ProfiledPIDCommand -from .profiledpidsubsystem import ProfiledPIDSubsystem from .proxycommand import ProxyCommand from .repeatcommand import RepeatCommand from .runcommand import RunCommand @@ -29,8 +25,6 @@ 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 from .wrappercommand import WrapperCommand @@ -52,11 +46,7 @@ "ParallelCommandGroup", "ParallelDeadlineGroup", "ParallelRaceGroup", - "PIDCommand", - "PIDSubsystem", "PrintCommand", - "ProfiledPIDCommand", - "ProfiledPIDSubsystem", "ProxyCommand", "RepeatCommand", "RunCommand", @@ -67,8 +57,6 @@ "Subsystem", "SwerveControllerCommand", "TimedCommandRobot", - "TrapezoidProfileCommand", - "TrapezoidProfileSubsystem", "WaitCommand", "WaitUntilCommand", "WrapperCommand", diff --git a/commands2/pidcommand.py b/commands2/pidcommand.py deleted file mode 100644 index 325dec5..0000000 --- a/commands2/pidcommand.py +++ /dev/null @@ -1,78 +0,0 @@ -# validated: 2024-01-19 DS f29a7d2e501b PIDCommand.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 Any, Callable, Union - -from .command import Command -from .subsystem import Subsystem - -from wpimath.controller import PIDController - - -class PIDCommand(Command): - """ - A command that controls an output with a :class:`wpimath.controller.PIDController`. Runs forever by default - to add - exit conditions and/or other behavior, subclass this class. The controller calculation and output - are performed synchronously in the command's execute() method. - """ - - def __init__( - self, - controller: PIDController, - measurementSource: Callable[[], float], - setpoint: Union[Callable[[], float], float, int], - useOutput: Callable[[float], Any], - *requirements: Subsystem, - ): - """ - Creates a new PIDCommand, which controls the given output with a :class:`wpimath.controller.PIDController`. - - :param controller: the controller that controls the output. - :param measurementSource: the measurement of the process variable - :param setpoint: the controller's setpoint (either a function that returns a) - number or a number - :param useOutput: the controller's output - :param requirements: the subsystems required by this command - """ - super().__init__() - - assert callable(measurementSource) - assert callable(useOutput) - - self._controller = controller - self._useOutput = useOutput - self._measurement = measurementSource - - if isinstance(setpoint, (float, int)): - setpoint = float(setpoint) - self._setpoint = lambda: setpoint - elif callable(setpoint): - self._setpoint = setpoint - else: - raise ValueError( - f"invalid setpoint (must be callable or number; got {type(setpoint)})" - ) - - self.addRequirements(*requirements) - - def initialize(self): - self._controller.reset() - - def execute(self): - self._useOutput( - self._controller.calculate(self._measurement(), self._setpoint()) - ) - - def end(self, interrupted): - self._useOutput(0) - - def getController(self): - """ - Returns the PIDController used by the command. - - :return: The PIDController - """ - return self._controller diff --git a/commands2/pidsubsystem.py b/commands2/pidsubsystem.py deleted file mode 100644 index b875efd..0000000 --- a/commands2/pidsubsystem.py +++ /dev/null @@ -1,100 +0,0 @@ -# validated: 2024-01-19 DS f29a7d2e501b PIDSubsystem.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 wpimath.controller import PIDController - -from .subsystem import Subsystem - - -class PIDSubsystem(Subsystem): - """ - A subsystem that uses a :class:`wpimath.controller.PIDController` to control an output. The - controller is run synchronously from the subsystem's periodic() method. - """ - - 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. - """ - super().__init__() - - self._controller = controller - self.setSetpoint(initial_position) - self.addChild("PID Controller", self._controller) - self._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._enabled: - self.useOutput( - self._controller.calculate(self.getMeasurement()), self.getSetpoint() - ) - - def getController(self) -> PIDController: - """ - Returns the PIDController used by the subsystem. - - :return: The PIDController. - """ - return self._controller - - def setSetpoint(self, setpoint: float): - """ - Sets the setpoint for the subsystem. - - :param setpoint: The setpoint for the subsystem. - """ - self._controller.setSetpoint(setpoint) - - def getSetpoint(self) -> float: - """ - Returns the current setpoint of the subsystem. - - :return: The current setpoint. - """ - return self._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(f"{self.__class__} must implement useOutput") - - def getMeasurement(self) -> float: - """ - Returns the measurement of the process variable used by the PIDController. - - :return: The measurement of the process variable. - """ - raise NotImplementedError(f"{self.__class__} must implement getMeasurement") - - def enable(self): - """Enables the PID control. Resets the controller.""" - self._enabled = True - self._controller.reset() - - def disable(self): - """Disables the PID control. Sets output to zero.""" - self._enabled = False - self.useOutput(0, 0) - - def isEnabled(self) -> bool: - """ - Returns whether the controller is enabled. - - :return: Whether the controller is enabled. - """ - return self._enabled diff --git a/commands2/profiledpidcommand.py b/commands2/profiledpidcommand.py deleted file mode 100644 index d7ce298..0000000 --- a/commands2/profiledpidcommand.py +++ /dev/null @@ -1,81 +0,0 @@ -# validated: 2024-01-24 DS f29a7d2e501b ProfiledPIDCommand.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 typing import Any, Generic - -from wpimath.controller import ProfiledPIDController, ProfiledPIDControllerRadians -from wpimath.trajectory import TrapezoidProfile, TrapezoidProfileRadians - -from .command import Command -from .subsystem import Subsystem -from .typing import ( - FloatOrFloatSupplier, - FloatSupplier, - TProfiledPIDController, - UseOutputFunction, -) - - -class ProfiledPIDCommand(Command, Generic[TProfiledPIDController]): - """A command that controls an output with a :class:`.ProfiledPIDController`. Runs forever by default - - to add exit conditions and/or other behavior, subclass this class. The controller calculation and - output are performed synchronously in the command's execute() method. - """ - - _stateCls: Any - - def __init__( - self, - controller: TProfiledPIDController, - measurementSource: FloatSupplier, - goalSource: FloatOrFloatSupplier, - useOutput: UseOutputFunction, - *requirements: Subsystem, - ): - """Creates a new ProfiledPIDCommand, which controls the given output with a ProfiledPIDController. Goal - velocity is specified. - - :param controller: the controller that controls the output. - :param measurementSource: the measurement of the process variable - :param goalSource: the controller's goal - :param useOutput: the controller's output - :param requirements: the subsystems required by this command - """ - - super().__init__() - if isinstance(controller, ProfiledPIDController): - self._stateCls = TrapezoidProfile.State - elif isinstance(controller, ProfiledPIDControllerRadians): - self._stateCls = TrapezoidProfileRadians.State - else: - raise ValueError(f"unknown controller type {controller!r}") - - self._controller: TProfiledPIDController = controller - self._useOutput = useOutput - self._measurement = measurementSource - if isinstance(goalSource, (float, int)): - self._goal = lambda: float(goalSource) - else: - self._goal = goalSource - - self.addRequirements(*requirements) - - def initialize(self): - self._controller.reset(self._measurement()) - - def execute(self): - self._useOutput.accept( - self._controller.calculate(self._measurement(), self._goal()), - self._controller.getSetpoint(), - ) - - def end(self, interrupted: bool): - self._useOutput(0.0, self._stateCls()) - - def getController(self): - """Gets the controller used by the command""" - return self._controller diff --git a/commands2/profiledpidsubsystem.py b/commands2/profiledpidsubsystem.py deleted file mode 100644 index 4a05baa..0000000 --- a/commands2/profiledpidsubsystem.py +++ /dev/null @@ -1,81 +0,0 @@ -# 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 Generic - -from wpimath.trajectory import TrapezoidProfile - -from .subsystem import Subsystem -from .typing import TProfiledPIDController, TTrapezoidProfileState - - -class ProfiledPIDSubsystem( - Subsystem, Generic[TProfiledPIDController, TTrapezoidProfileState] -): - """ - A subsystem that uses a :class:`wpimath.controller.ProfiledPIDController` - or :class:`wpimath.controller.ProfiledPIDControllerRadians` to - control an output. The controller is run synchronously from the subsystem's - :meth:`.periodic` method. - """ - - def __init__( - self, - controller: TProfiledPIDController, - initial_position: float = 0, - ): - """ - Creates a new Profiled PID Subsystem using the provided PID Controller - - :param controller: the controller that controls the output - :param initial_position: the initial value of the process variable - - """ - super().__init__() - self._controller: TProfiledPIDController = controller - self._enabled = False - self.setGoal(initial_position) - - def periodic(self): - """Updates the output of the controller.""" - if self._enabled: - self.useOutput( - self._controller.calculate(self.getMeasurement()), - self._controller.getSetpoint(), - ) - - def getController( - self, - ) -> TProfiledPIDController: - """Returns the controller""" - return self._controller - - def setGoal(self, goal): - """Sets the goal state for the subsystem.""" - self._controller.setGoal(goal) - - def useOutput(self, output: float, setpoint: TTrapezoidProfileState): - """Uses the output from the controller object.""" - raise NotImplementedError(f"{self.__class__} must implement useOutput") - - def getMeasurement(self) -> float: - """ - Returns the measurement of the process variable used by the - controller object. - """ - raise NotImplementedError(f"{self.__class__} must implement getMeasurement") - - def enable(self): - """Enables the PID control. Resets the controller.""" - self._enabled = True - self._controller.reset(self.getMeasurement()) - - def disable(self): - """Disables the PID control. Sets output to zero.""" - self._enabled = False - self.useOutput(0, TrapezoidProfile.State()) - - def isEnabled(self) -> bool: - """Returns whether the controller is enabled.""" - return self._enabled diff --git a/commands2/trapezoidprofilecommand.py b/commands2/trapezoidprofilecommand.py deleted file mode 100644 index 4ffff24..0000000 --- a/commands2/trapezoidprofilecommand.py +++ /dev/null @@ -1,64 +0,0 @@ -# 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/commands2/trapezoidprofilesubsystem.py b/commands2/trapezoidprofilesubsystem.py deleted file mode 100644 index 8efb263..0000000 --- a/commands2/trapezoidprofilesubsystem.py +++ /dev/null @@ -1,94 +0,0 @@ -# validated: 2024-02-20 DV 6cc7e52de74a TrapezoidProfileSubsystem.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 Any, Union - -from wpimath import units -from wpimath.trajectory import TrapezoidProfile, TrapezoidProfileRadians - - -from .subsystem import Subsystem - - -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. - """ - - _profile: Any - _stateCls: Any - - def __init__( - self, - constraints: Union[ - TrapezoidProfile.Constraints, TrapezoidProfileRadians.Constraints - ], - initial_position: float = 0.0, - period: units.seconds = 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. - """ - if isinstance(constraints, TrapezoidProfile.Constraints): - self._profile = TrapezoidProfile(constraints) - self._stateCls = TrapezoidProfile.State - elif isinstance(constraints, TrapezoidProfileRadians.Constraints): - self._profile = TrapezoidProfileRadians(constraints) - self._stateCls = TrapezoidProfileRadians.State - else: - raise ValueError(f"Invalid constraints {constraints}") - - self._state = self._stateCls(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._state, self._goal) - if self._enabled: - self.useState(self._state) - - def setGoal(self, goal): - """ - Sets the goal state for the subsystem. Goal velocity assumed to be zero. - - :param goal: The goal position for the subsystem's motion profile. The goal - can either be a `TrapezoidProfile.State` or `float`. If float is provided, - the assumed velocity for the goal will be 0. - """ - # If we got a float, instantiate the state - if isinstance(goal, (float, int)): - goal = self._stateCls(goal, 0) - - self._goal = goal - - 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): - """ - Users should override this to consume the current state of the motion profile. - - :param state: The current state of the motion profile. - """ - raise NotImplementedError(f"{self.__class__} must implement useState") diff --git a/commands2/typing.py b/commands2/typing.py index 6168330..ddec81a 100644 --- a/commands2/typing.py +++ b/commands2/typing.py @@ -1,29 +1,6 @@ -from typing import Callable, Protocol, TypeVar, Union +from typing import Callable, Union from typing_extensions import TypeAlias -from wpimath.controller import ProfiledPIDController, ProfiledPIDControllerRadians -from wpimath.trajectory import TrapezoidProfile, TrapezoidProfileRadians - -# Generic Types -TProfiledPIDController = TypeVar( - "TProfiledPIDController", ProfiledPIDControllerRadians, ProfiledPIDController -) -TTrapezoidProfileState = TypeVar( - "TTrapezoidProfileState", - TrapezoidProfileRadians.State, - TrapezoidProfile.State, -) - - -# Protocols - Structural Typing -class UseOutputFunction(Protocol): - - def __init__(self, *args, **kwargs) -> None: ... - - def __call__(self, t: float, u: TTrapezoidProfileState) -> None: ... - - def accept(self, t: float, u: TTrapezoidProfileState) -> None: ... - # Type Aliases FloatSupplier: TypeAlias = Callable[[], float] diff --git a/tests/test_pidcommand.py b/tests/test_pidcommand.py deleted file mode 100644 index 5ba5165..0000000 --- a/tests/test_pidcommand.py +++ /dev/null @@ -1,114 +0,0 @@ -from typing import TYPE_CHECKING - -from util import * # type: ignore -import wpimath.controller as controller -import commands2 - -if TYPE_CHECKING: - from .util import * - -import pytest - - -def test_pidCommandSupplier(scheduler: commands2.CommandScheduler): - with ManualSimTime() as sim: - output_float = OOFloat(0.0) - measurement_source = OOFloat(5.0) - setpoint_source = OOFloat(2.0) - pid_controller = controller.PIDController(0.1, 0.01, 0.001) - system = commands2.Subsystem() - pidCommand = commands2.PIDCommand( - pid_controller, - measurement_source, - setpoint_source, - output_float.set, - system, - ) - start_spying_on(pidCommand) - scheduler.schedule(pidCommand) - scheduler.run() - sim.step(1) - scheduler.run() - - assert scheduler.isScheduled(pidCommand) - - assert not pidCommand._controller.atSetpoint() - - # Tell the pid command we're at our setpoint through the controller - measurement_source.set(setpoint_source()) - - sim.step(2) - - scheduler.run() - - # Should be measuring error of 0 now - assert pidCommand._controller.atSetpoint() - - -def test_pidCommandScalar(scheduler: commands2.CommandScheduler): - with ManualSimTime() as sim: - output_float = OOFloat(0.0) - measurement_source = OOFloat(5.0) - setpoint_source = 2.0 - pid_controller = controller.PIDController(0.1, 0.01, 0.001) - system = commands2.Subsystem() - pidCommand = commands2.PIDCommand( - pid_controller, - measurement_source, - setpoint_source, - output_float.set, - system, - ) - start_spying_on(pidCommand) - scheduler.schedule(pidCommand) - scheduler.run() - sim.step(1) - scheduler.run() - - assert scheduler.isScheduled(pidCommand) - - assert not pidCommand._controller.atSetpoint() - - # Tell the pid command we're at our setpoint through the controller - measurement_source.set(setpoint_source) - - sim.step(2) - - scheduler.run() - - # Should be measuring error of 0 now - assert pidCommand._controller.atSetpoint() - - -def test_withTimeout(scheduler: commands2.CommandScheduler): - with ManualSimTime() as sim: - output_float = OOFloat(0.0) - measurement_source = OOFloat(5.0) - setpoint_source = OOFloat(2.0) - pid_controller = controller.PIDController(0.1, 0.01, 0.001) - system = commands2.Subsystem() - command1 = commands2.PIDCommand( - pid_controller, - measurement_source, - setpoint_source, - output_float.set, - system, - ) - start_spying_on(command1) - - timeout = command1.withTimeout(2) - - scheduler.schedule(timeout) - scheduler.run() - - verify(command1).initialize() - verify(command1).execute() - assert not scheduler.isScheduled(command1) - assert scheduler.isScheduled(timeout) - - sim.step(3) - scheduler.run() - - verify(command1).end(True) - verify(command1, never()).end(False) - assert not scheduler.isScheduled(timeout) diff --git a/tests/test_profiledpidsubsystem.py b/tests/test_profiledpidsubsystem.py deleted file mode 100644 index 8b896f3..0000000 --- a/tests/test_profiledpidsubsystem.py +++ /dev/null @@ -1,119 +0,0 @@ -from types import MethodType -from typing import Any - -import pytest -from wpimath.controller import ProfiledPIDController, ProfiledPIDControllerRadians -from wpimath.trajectory import TrapezoidProfile, TrapezoidProfileRadians - -from commands2 import ProfiledPIDSubsystem - -MAX_VELOCITY = 30 # Radians per second -MAX_ACCELERATION = 500 # Radians per sec squared -PID_KP = 50 - - -class EvalSubsystem(ProfiledPIDSubsystem): - def __init__(self, controller, state_factory): - self._state_factory = state_factory - super().__init__(controller, 0) - - -def simple_use_output(self, output: float, setpoint: Any): - """A simple useOutput method that saves the current state of the controller.""" - self._output = output - self._setpoint = setpoint - - -def simple_get_measurement(self) -> float: - """A simple getMeasurement method that returns zero (frozen or stuck plant).""" - return 0.0 - - -controller_types = [ - ( - ProfiledPIDControllerRadians, - TrapezoidProfileRadians.Constraints, - TrapezoidProfileRadians.State, - ), - (ProfiledPIDController, TrapezoidProfile.Constraints, TrapezoidProfile.State), -] -controller_ids = ["radians", "dimensionless"] - - -@pytest.fixture(params=controller_types, ids=controller_ids) -def subsystem(request): - """ - Fixture that returns an EvalSubsystem object for each type of controller. - """ - controller, profile_factory, state_factory = request.param - profile = profile_factory(MAX_VELOCITY, MAX_ACCELERATION) - pid = controller(PID_KP, 0, 0, profile) - return EvalSubsystem(pid, state_factory) - - -def test_profiled_pid_subsystem_init(subsystem): - """ - Verify that the ProfiledPIDSubsystem can be initialized using - all supported profiled PID controller / trapezoid profile types. - """ - assert isinstance(subsystem, EvalSubsystem) - - -def test_profiled_pid_subsystem_not_implemented_get_measurement(subsystem): - """ - Verify that the ProfiledPIDSubsystem.getMeasurement method - raises NotImplementedError. - """ - with pytest.raises(NotImplementedError): - subsystem.getMeasurement() - - -def test_profiled_pid_subsystem_not_implemented_use_output(subsystem): - """ - Verify that the ProfiledPIDSubsystem.useOutput method raises - NotImplementedError. - """ - with pytest.raises(NotImplementedError): - subsystem.useOutput(0, subsystem._state_factory()) - - -@pytest.mark.parametrize("use_float", [True, False]) -def test_profiled_pid_subsystem_set_goal(subsystem, use_float): - """ - Verify that the ProfiledPIDSubsystem.setGoal method sets the goal. - """ - if use_float: - subsystem.setGoal(1.0) - assert subsystem.getController().getGoal().position == 1.0 - assert subsystem.getController().getGoal().velocity == 0.0 - else: - subsystem.setGoal(subsystem._state_factory(1.0, 2.0)) - assert subsystem.getController().getGoal().position == 1.0 - assert subsystem.getController().getGoal().velocity == 2.0 - - -def test_profiled_pid_subsystem_enable_subsystem(subsystem): - """ - Verify the subsystem can be enabled. - """ - # Dynamically add useOutput and getMeasurement methods so the - # system can be enabled - setattr(subsystem, "useOutput", MethodType(simple_use_output, subsystem)) - setattr(subsystem, "getMeasurement", MethodType(simple_get_measurement, subsystem)) - # Enable the subsystem - subsystem.enable() - assert subsystem.isEnabled() - - -def test_profiled_pid_subsystem_disable_subsystem(subsystem): - """ - Verify the subsystem can be disabled. - """ - # Dynamically add useOutput and getMeasurement methods so the - # system can be enabled - setattr(subsystem, "useOutput", MethodType(simple_use_output, subsystem)) - setattr(subsystem, "getMeasurement", MethodType(simple_get_measurement, subsystem)) - # Enable and then disable the subsystem - subsystem.enable() - subsystem.disable() - assert not subsystem.isEnabled() diff --git a/tests/test_trapezoidprofilecommand.py b/tests/test_trapezoidprofilecommand.py deleted file mode 100644 index 730fc82..0000000 --- a/tests/test_trapezoidprofilecommand.py +++ /dev/null @@ -1,142 +0,0 @@ -# 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)