""" 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'])