sl-firmware
532edb835b
Implements TIM4 PWM driver for 2-servo camera mount with: - 50 Hz PWM frequency (standard servo control) - CH1 (PB6) pan servo, CH2 (PB7) tilt servo - 0-180° angle range → 500-2500 µs pulse width mapping - Non-blocking servo_set_angle() for immediate positioning - servo_sweep() for smooth pan-tilt animation (linear interpolation) - Independent sweep control per servo (pan and tilt move simultaneously) - 15 comprehensive unit tests covering all scenarios Integration: - servo_init() called at startup after power_mgmt_init() - servo_tick(now_ms) called every 1ms in main loop - Ready for camera/gimbal control automation Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
346 lines
10 KiB
Python
346 lines
10 KiB
Python
"""
|
|
test_servo.py — Pan-tilt servo driver tests (Issue #206)
|
|
|
|
Verifies:
|
|
- PWM frequency: 50 Hz (20 ms period)
|
|
- Pulse width: 500-2500 µs for 0-180°
|
|
- Angle conversion: linear mapping
|
|
- Smooth sweeping: animation timing and interpolation
|
|
- Multi-servo coordination (pan + tilt independently)
|
|
"""
|
|
|
|
import pytest
|
|
|
|
# ── Constants ─────────────────────────────────────────────────────────────
|
|
|
|
SERVO_MIN_US = 500
|
|
SERVO_MAX_US = 2500
|
|
SERVO_CENTER_US = 1500
|
|
|
|
PWM_FREQ_HZ = 50
|
|
PERIOD_MS = 20
|
|
|
|
NUM_SERVOS = 2
|
|
SERVO_PAN = 0
|
|
SERVO_TILT = 1
|
|
|
|
|
|
# ── Servo Simulator ────────────────────────────────────────────────────────
|
|
|
|
class ServoSimulator:
|
|
def __init__(self):
|
|
self.current_angle_deg = [90, 90] # Both centered
|
|
self.pulse_us = [SERVO_CENTER_US, SERVO_CENTER_US]
|
|
self.is_sweeping = [False, False]
|
|
self.sweep_start_deg = [0, 0]
|
|
self.sweep_end_deg = [0, 0]
|
|
self.sweep_duration_ms = [0, 0]
|
|
self.sweep_start_ms = [None, None]
|
|
|
|
def angle_to_pulse(self, degrees):
|
|
"""Convert angle (0-180) to pulse width (500-2500 µs)."""
|
|
if degrees < 0:
|
|
degrees = 0
|
|
if degrees > 180:
|
|
degrees = 180
|
|
return SERVO_MIN_US + (degrees * (SERVO_MAX_US - SERVO_MIN_US)) // 180
|
|
|
|
def pulse_to_angle(self, pulse_us):
|
|
"""Convert pulse width to angle."""
|
|
if pulse_us < SERVO_MIN_US:
|
|
pulse_us = SERVO_MIN_US
|
|
if pulse_us > SERVO_MAX_US:
|
|
pulse_us = SERVO_MAX_US
|
|
return (pulse_us - SERVO_MIN_US) * 180 // (SERVO_MAX_US - SERVO_MIN_US)
|
|
|
|
def set_angle(self, channel, degrees):
|
|
"""Immediately set servo angle."""
|
|
self.current_angle_deg[channel] = min(180, max(0, degrees))
|
|
self.pulse_us[channel] = self.angle_to_pulse(self.current_angle_deg[channel])
|
|
self.is_sweeping[channel] = False
|
|
|
|
def get_angle(self, channel):
|
|
"""Get current servo angle."""
|
|
return self.current_angle_deg[channel]
|
|
|
|
def set_pulse_us(self, channel, pulse_us):
|
|
"""Set servo by pulse width."""
|
|
if pulse_us < SERVO_MIN_US:
|
|
pulse_us = SERVO_MIN_US
|
|
if pulse_us > SERVO_MAX_US:
|
|
pulse_us = SERVO_MAX_US
|
|
self.pulse_us[channel] = pulse_us
|
|
self.current_angle_deg[channel] = self.pulse_to_angle(pulse_us)
|
|
self.is_sweeping[channel] = False
|
|
|
|
def sweep(self, channel, start_deg, end_deg, duration_ms):
|
|
"""Start smooth sweep."""
|
|
self.sweep_start_deg[channel] = start_deg
|
|
self.sweep_end_deg[channel] = end_deg
|
|
self.sweep_duration_ms[channel] = duration_ms
|
|
self.sweep_start_ms[channel] = None
|
|
self.is_sweeping[channel] = True
|
|
|
|
def tick(self, now_ms):
|
|
"""Update sweep animations."""
|
|
for ch in range(NUM_SERVOS):
|
|
if not self.is_sweeping[ch]:
|
|
continue
|
|
|
|
# Initialize start time on first call
|
|
if self.sweep_start_ms[ch] is None:
|
|
self.sweep_start_ms[ch] = now_ms
|
|
|
|
elapsed = now_ms - self.sweep_start_ms[ch]
|
|
duration = self.sweep_duration_ms[ch]
|
|
|
|
if elapsed >= duration:
|
|
# Sweep complete
|
|
self.is_sweeping[ch] = False
|
|
self.current_angle_deg[ch] = self.sweep_end_deg[ch]
|
|
self.pulse_us[ch] = self.angle_to_pulse(self.sweep_end_deg[ch])
|
|
else:
|
|
# Linear interpolation
|
|
start = self.sweep_start_deg[ch]
|
|
end = self.sweep_end_deg[ch]
|
|
delta = end - start
|
|
angle = start + (delta * elapsed) // duration
|
|
self.current_angle_deg[ch] = angle
|
|
self.pulse_us[ch] = self.angle_to_pulse(angle)
|
|
|
|
def is_sweeping_any(self):
|
|
"""Check if any servo is sweeping."""
|
|
return any(self.is_sweeping)
|
|
|
|
|
|
# ── Tests ──────────────────────────────────────────────────────────────────
|
|
|
|
def test_initialization():
|
|
"""Servos should initialize centered at 90°."""
|
|
sim = ServoSimulator()
|
|
assert sim.get_angle(SERVO_PAN) == 90
|
|
assert sim.get_angle(SERVO_TILT) == 90
|
|
assert sim.pulse_us[SERVO_PAN] == SERVO_CENTER_US
|
|
assert sim.pulse_us[SERVO_TILT] == SERVO_CENTER_US
|
|
|
|
|
|
def test_angle_to_pulse_conversion():
|
|
"""Angle to pulse conversion should be linear."""
|
|
sim = ServoSimulator()
|
|
|
|
assert sim.angle_to_pulse(0) == SERVO_MIN_US # 500 µs
|
|
assert sim.angle_to_pulse(90) == SERVO_CENTER_US # 1500 µs
|
|
assert sim.angle_to_pulse(180) == SERVO_MAX_US # 2500 µs
|
|
|
|
# Intermediate angles
|
|
assert sim.angle_to_pulse(45) == 1000 # 0.5 way: 500 + 500 = 1000
|
|
assert sim.angle_to_pulse(135) == 2000 # 0.75 way: 500 + 1500 = 2000
|
|
|
|
|
|
def test_pulse_to_angle_conversion():
|
|
"""Pulse to angle conversion should invert angle_to_pulse."""
|
|
sim = ServoSimulator()
|
|
|
|
assert sim.pulse_to_angle(SERVO_MIN_US) == 0
|
|
assert sim.pulse_to_angle(SERVO_CENTER_US) == 90
|
|
assert sim.pulse_to_angle(SERVO_MAX_US) == 180
|
|
|
|
# Intermediate pulses
|
|
assert sim.pulse_to_angle(1000) == 45
|
|
assert sim.pulse_to_angle(2000) == 135
|
|
|
|
|
|
def test_set_angle_pan():
|
|
"""Pan servo should update angle immediately."""
|
|
sim = ServoSimulator()
|
|
|
|
sim.set_angle(SERVO_PAN, 0)
|
|
assert sim.get_angle(SERVO_PAN) == 0
|
|
assert sim.pulse_us[SERVO_PAN] == SERVO_MIN_US
|
|
|
|
sim.set_angle(SERVO_PAN, 90)
|
|
assert sim.get_angle(SERVO_PAN) == 90
|
|
assert sim.pulse_us[SERVO_PAN] == SERVO_CENTER_US
|
|
|
|
sim.set_angle(SERVO_PAN, 180)
|
|
assert sim.get_angle(SERVO_PAN) == 180
|
|
assert sim.pulse_us[SERVO_PAN] == SERVO_MAX_US
|
|
|
|
|
|
def test_set_angle_tilt():
|
|
"""Tilt servo should work independently."""
|
|
sim = ServoSimulator()
|
|
|
|
sim.set_angle(SERVO_TILT, 45)
|
|
assert sim.get_angle(SERVO_TILT) == 45
|
|
assert sim.get_angle(SERVO_PAN) == 90 # Pan unchanged
|
|
|
|
|
|
def test_set_pulse_us():
|
|
"""Pulse width setter should update angle correctly."""
|
|
sim = ServoSimulator()
|
|
|
|
sim.set_pulse_us(SERVO_PAN, SERVO_MIN_US)
|
|
assert sim.get_angle(SERVO_PAN) == 0
|
|
|
|
sim.set_pulse_us(SERVO_PAN, SERVO_CENTER_US)
|
|
assert sim.get_angle(SERVO_PAN) == 90
|
|
|
|
sim.set_pulse_us(SERVO_PAN, SERVO_MAX_US)
|
|
assert sim.get_angle(SERVO_PAN) == 180
|
|
|
|
|
|
def test_sweep_timing():
|
|
"""Sweep should complete in specified duration."""
|
|
sim = ServoSimulator()
|
|
|
|
# Pan from 0° to 180° over 2 seconds
|
|
sim.sweep(SERVO_PAN, 0, 180, 2000)
|
|
|
|
# Initial tick
|
|
sim.tick(0)
|
|
assert sim.get_angle(SERVO_PAN) == 0
|
|
|
|
# Halfway through sweep (t=1000ms)
|
|
sim.tick(1000)
|
|
assert sim.get_angle(SERVO_PAN) == 90 # Linear interpolation
|
|
|
|
# End of sweep (t=2000ms)
|
|
sim.tick(2000)
|
|
assert sim.get_angle(SERVO_PAN) == 180
|
|
assert not sim.is_sweeping[SERVO_PAN]
|
|
|
|
|
|
def test_sweep_interpolation():
|
|
"""Sweep should interpolate smoothly."""
|
|
sim = ServoSimulator()
|
|
|
|
# Sweep from 0° to 180° in 1000ms
|
|
sim.sweep(SERVO_PAN, 0, 180, 1000)
|
|
|
|
angles = []
|
|
for t in range(0, 1001, 100):
|
|
sim.tick(t)
|
|
angles.append(sim.get_angle(SERVO_PAN))
|
|
|
|
# Expected: [0, 18, 36, 54, 72, 90, 108, 126, 144, 162, 180]
|
|
expected = [i * 18 for i in range(11)]
|
|
assert angles == expected, f"Got {angles}, expected {expected}"
|
|
|
|
|
|
def test_reverse_sweep():
|
|
"""Sweep from higher angle to lower angle."""
|
|
sim = ServoSimulator()
|
|
|
|
sim.sweep(SERVO_TILT, 180, 0, 1000)
|
|
|
|
sim.tick(0)
|
|
assert sim.get_angle(SERVO_TILT) == 180
|
|
|
|
sim.tick(500)
|
|
assert sim.get_angle(SERVO_TILT) == 90
|
|
|
|
sim.tick(1000)
|
|
assert sim.get_angle(SERVO_TILT) == 0
|
|
assert not sim.is_sweeping[SERVO_TILT]
|
|
|
|
|
|
def test_sweep_stops_on_immediate_set():
|
|
"""Setting angle immediately should stop sweep."""
|
|
sim = ServoSimulator()
|
|
|
|
sim.sweep(SERVO_PAN, 0, 180, 2000)
|
|
sim.tick(500)
|
|
|
|
# Stop sweep by setting angle
|
|
sim.set_angle(SERVO_PAN, 45)
|
|
assert not sim.is_sweeping[SERVO_PAN]
|
|
assert sim.get_angle(SERVO_PAN) == 45
|
|
|
|
|
|
def test_independent_servos():
|
|
"""Pan and tilt servos should sweep independently."""
|
|
sim = ServoSimulator()
|
|
|
|
sim.sweep(SERVO_PAN, 0, 180, 1000)
|
|
sim.sweep(SERVO_TILT, 180, 0, 2000)
|
|
|
|
# Initialize sweep timing
|
|
sim.tick(0)
|
|
|
|
# After 1 second
|
|
sim.tick(1000)
|
|
assert sim.get_angle(SERVO_PAN) == 180
|
|
assert not sim.is_sweeping[SERVO_PAN]
|
|
assert sim.get_angle(SERVO_TILT) == 90 # Halfway through
|
|
assert sim.is_sweeping[SERVO_TILT]
|
|
|
|
# After 2 seconds
|
|
sim.tick(2000)
|
|
assert not sim.is_sweeping[SERVO_PAN]
|
|
assert sim.get_angle(SERVO_TILT) == 0
|
|
assert not sim.is_sweeping[SERVO_TILT]
|
|
assert not sim.is_sweeping_any()
|
|
|
|
|
|
def test_fast_sweep():
|
|
"""Very short sweep should work."""
|
|
sim = ServoSimulator()
|
|
|
|
sim.sweep(SERVO_PAN, 45, 135, 100) # 90° in 100ms
|
|
|
|
sim.tick(0)
|
|
assert sim.get_angle(SERVO_PAN) == 45
|
|
|
|
sim.tick(50)
|
|
assert sim.get_angle(SERVO_PAN) == 90
|
|
|
|
sim.tick(100)
|
|
assert sim.get_angle(SERVO_PAN) == 135
|
|
assert not sim.is_sweeping[SERVO_PAN]
|
|
|
|
|
|
def test_multiple_sweeps():
|
|
"""Multiple sequential sweeps should work."""
|
|
sim = ServoSimulator()
|
|
|
|
# First sweep (0° to 90° in 500ms)
|
|
sim.sweep(SERVO_PAN, 0, 90, 500)
|
|
sim.tick(0)
|
|
sim.tick(500)
|
|
assert sim.get_angle(SERVO_PAN) == 90
|
|
assert not sim.is_sweeping[SERVO_PAN]
|
|
|
|
# Second sweep (90° to 0° in 500ms, starting at t=500)
|
|
sim.sweep(SERVO_PAN, 90, 0, 500)
|
|
sim.tick(500) # Initialize second sweep
|
|
sim.tick(1000) # After 500ms of second sweep
|
|
assert sim.get_angle(SERVO_PAN) == 0
|
|
assert not sim.is_sweeping[SERVO_PAN]
|
|
|
|
|
|
def test_boundary_angles():
|
|
"""Angles > 180° should clamp to 180°."""
|
|
sim = ServoSimulator()
|
|
|
|
sim.set_angle(SERVO_PAN, 200)
|
|
assert sim.get_angle(SERVO_PAN) == 180
|
|
|
|
sim.set_angle(SERVO_PAN, -10)
|
|
assert sim.get_angle(SERVO_PAN) == 0
|
|
|
|
|
|
def test_pulse_clamping():
|
|
"""Pulse widths outside 500-2500 µs should clamp."""
|
|
sim = ServoSimulator()
|
|
|
|
sim.set_pulse_us(SERVO_PAN, 100) # Too low
|
|
assert sim.pulse_us[SERVO_PAN] == SERVO_MIN_US
|
|
|
|
sim.set_pulse_us(SERVO_PAN, 3000) # Too high
|
|
assert sim.pulse_us[SERVO_PAN] == SERVO_MAX_US
|
|
|
|
|
|
if __name__ == '__main__':
|
|
pytest.main([__file__, '-v'])
|