Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[copybara] Resync commandsv2 (#8535)

Browse Source 
This will be the last sync from the commandsv2
[repo](https://github.com/robotpy/robotpy-commands-v2), as it has now
been merged into
[mostrobotpy](https://github.com/robotpy/mostrobotpy/pull/226).

Doing this on its own to make the diffs smaller when doing a full
`mostrobotpy` sync, which will now have a lot of other new goodies
coming along for the ride.

---------

Co-authored-by: Default email <default@default.com>
This commit is contained in:
PJ Reiniger
2026-01-14 23:15:00 -05:00
committed by GitHub
parent 996af272e8
commit 1cbc25bc36
11 changed files with 1 additions and 901 deletions
Download Patch File Download Diff File Expand all files Collapse all files

115
commandsv2/src/test/python/test_pidcommand.py
View File

@@ -1,115 +0,0 @@
from typing import TYPE_CHECKING
from util import * # type: ignore
import commands2
import wpimath
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 = wpimath.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 = wpimath.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 = wpimath.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)

118
commandsv2/src/test/python/test_profiledpidsubsystem.py
View File

@@ -1,118 +0,0 @@
from types import MethodType
from typing import Any
import pytest
from wpimath import ProfiledPIDController, ProfiledPIDControllerRadians, 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()

137
commandsv2/src/test/python/test_trapezoidprofilecommand.py
View File

@@ -1,137 +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
from wpimath import TrapezoidProfile as DimensionlessProfile, 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)