113 lines
4.3 KiB
Python
113 lines
4.3 KiB
Python
"""Unit tests for :mod:`gowelcome.control.pid` (PIDController + normalize_angle).
|
|
|
|
Pure / stdlib only -- no cv2, ultralytics, SDK or hardware.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import math
|
|
|
|
import pytest
|
|
|
|
from gowelcome.control.pid import PIDController, normalize_angle
|
|
|
|
|
|
# --------------------------------------------------------------------------- #
|
|
# PIDController
|
|
# --------------------------------------------------------------------------- #
|
|
def test_proportional_output():
|
|
"""With ki=kd=0 the output is exactly kp * error."""
|
|
pid = PIDController(kp=2.0)
|
|
assert pid.update(3.0, dt=0.1) == pytest.approx(6.0)
|
|
# The sign of the output tracks the sign of the error.
|
|
assert pid.update(-1.5, dt=0.1) == pytest.approx(-3.0)
|
|
|
|
|
|
def test_integral_accumulates():
|
|
"""The integral term builds up over successive steps of constant error."""
|
|
pid = PIDController(kp=0.0, ki=1.0)
|
|
out1 = pid.update(1.0, dt=0.5) # integral = 0.5 -> out 0.5
|
|
out2 = pid.update(1.0, dt=0.5) # integral = 1.0 -> out 1.0
|
|
assert out2 > out1
|
|
assert out1 == pytest.approx(0.5)
|
|
assert out2 == pytest.approx(1.0)
|
|
|
|
|
|
def test_integral_limit_clamps_windup():
|
|
"""integral_limit caps the accumulated integral (anti-windup)."""
|
|
pid = PIDController(kp=0.0, ki=1.0, integral_limit=0.3)
|
|
# Drive a large constant error for several steps; integral must not exceed
|
|
# the clamp regardless of how long we accumulate.
|
|
out = 0.0
|
|
for _ in range(20):
|
|
out = pid.update(5.0, dt=0.5)
|
|
assert pid.integral == pytest.approx(0.3)
|
|
assert out == pytest.approx(0.3)
|
|
|
|
|
|
def test_deadband_zeroes_integral_and_derivative():
|
|
"""Inside the deadband the integral and derivative terms are suppressed.
|
|
|
|
The proportional term still acts (per the controller's documented
|
|
behaviour), but integral build-up and derivative kick are zeroed.
|
|
"""
|
|
pid = PIDController(kp=1.0, ki=10.0, kd=10.0, deadband=0.5)
|
|
# Error below the deadband: integral and derivative contributions dropped,
|
|
# so the output collapses to just kp * error.
|
|
out = pid.update(0.2, dt=0.1)
|
|
assert pid.integral == pytest.approx(0.0)
|
|
assert out == pytest.approx(0.2) # 1.0 * 0.2 only
|
|
|
|
|
|
def test_output_limits_clamp():
|
|
"""output_limits saturate the controller output on both sides."""
|
|
pid = PIDController(kp=10.0, output_limits=(-1.0, 1.0))
|
|
assert pid.update(5.0, dt=0.1) == pytest.approx(1.0) # would be 50 -> clamp hi
|
|
assert pid.update(-5.0, dt=0.1) == pytest.approx(-1.0) # would be -50 -> clamp lo
|
|
# Within the band, no clamping.
|
|
assert pid.update(0.05, dt=0.1) == pytest.approx(0.5)
|
|
|
|
|
|
def test_reset_clears_state():
|
|
"""reset() clears the integral accumulator and previous-error memory."""
|
|
pid = PIDController(kp=0.0, ki=1.0)
|
|
pid.update(1.0, dt=1.0)
|
|
assert pid.integral != 0.0
|
|
pid.reset()
|
|
assert pid.integral == pytest.approx(0.0)
|
|
assert pid.prev_error == pytest.approx(0.0)
|
|
|
|
|
|
def test_nonpositive_dt_is_guarded():
|
|
"""dt <= 0 must not corrupt the integral or blow up the derivative."""
|
|
pid = PIDController(kp=1.0, ki=1.0, kd=1.0)
|
|
out = pid.update(2.0, dt=0.0)
|
|
# Integral unchanged (still zero) and derivative treated as zero -> P only.
|
|
assert pid.integral == pytest.approx(0.0)
|
|
assert out == pytest.approx(2.0)
|
|
|
|
|
|
# --------------------------------------------------------------------------- #
|
|
# normalize_angle
|
|
# --------------------------------------------------------------------------- #
|
|
def test_normalize_angle_wraps_into_range():
|
|
"""Angles wrap into (-pi, pi]."""
|
|
assert normalize_angle(0.0) == pytest.approx(0.0)
|
|
assert normalize_angle(math.pi / 2) == pytest.approx(math.pi / 2)
|
|
# 3*pi wraps to +/- pi (same point on the circle).
|
|
assert abs(normalize_angle(3.0 * math.pi)) == pytest.approx(math.pi)
|
|
# Just past +pi wraps to near -pi.
|
|
assert normalize_angle(math.pi + 0.1) == pytest.approx(-math.pi + 0.1)
|
|
# A large negative angle (whole 2*pi turns) wraps back to the same point.
|
|
wrapped = normalize_angle(-4.0 * math.pi + 0.25)
|
|
assert -math.pi <= wrapped <= math.pi
|
|
assert wrapped == pytest.approx(0.25, abs=1e-6)
|
|
|
|
|
|
def test_normalize_angle_is_periodic():
|
|
"""Adding full 2*pi turns does not change the normalized result."""
|
|
for base in (0.3, -1.2, 2.9):
|
|
assert normalize_angle(base) == pytest.approx(
|
|
normalize_angle(base + 2.0 * math.pi), abs=1e-9
|
|
)
|