mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
1a99a348cb
Project import generated by Copybara. GitOrigin-RevId: 715c8e8372d936f447f2937aab6b1a22dc619126
120 lines
3.9 KiB
Python
120 lines
3.9 KiB
Python
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()
|