diff --git a/include/config.h b/include/config.h index 0a16650..d48bf90 100644 --- a/include/config.h +++ b/include/config.h @@ -53,6 +53,21 @@ #define LED_STRIP_NUM_LEDS 8u // 8-LED ring #define LED_STRIP_FREQ_HZ 800000u // 800 kHz PWM for NeoPixel (1.25 µs per bit) +// --- Servo Pan-Tilt (Issue #206) --- +// TIM4_CH1 (PB6) for pan servo, TIM4_CH2 (PB7) for tilt servo +#define SERVO_TIM TIM4 +#define SERVO_PAN_PORT GPIOB +#define SERVO_PAN_PIN GPIO_PIN_6 // TIM4_CH1 +#define SERVO_PAN_CHANNEL TIM_CHANNEL_1 +#define SERVO_TILT_PORT GPIOB +#define SERVO_TILT_PIN GPIO_PIN_7 // TIM4_CH2 +#define SERVO_TILT_CHANNEL TIM_CHANNEL_2 +#define SERVO_AF GPIO_AF2_TIM4 // Alternate function +#define SERVO_FREQ_HZ 50u // 50 Hz (20ms period, standard servo) +#define SERVO_MIN_US 500u // 500µs = 0° +#define SERVO_MAX_US 2500u // 2500µs = 180° +#define SERVO_CENTER_US 1500u // 1500µs = 90° + // --- OSD: MAX7456 (SPI2) --- #define OSD_SPI SPI2 #define OSD_CS_PORT GPIOB diff --git a/include/servo.h b/include/servo.h new file mode 100644 index 0000000..cd0d068 --- /dev/null +++ b/include/servo.h @@ -0,0 +1,110 @@ +#ifndef SERVO_H +#define SERVO_H + +#include +#include + +/* + * servo.h — Pan-tilt servo driver for camera head (Issue #206) + * + * Hardware: TIM4 PWM at 50 Hz (20 ms period) + * - CH1 (PB6): Pan servo (0-180°) + * - CH2 (PB7): Tilt servo (0-180°) + * + * Servo pulse mapping: + * - 500 µs → 0° (full left/down) + * - 1500 µs → 90° (center) + * - 2500 µs → 180° (full right/up) + * + * Smooth sweeping via servo_sweep() for camera motion. + */ + +/* Servo channels */ +typedef enum { + SERVO_PAN = 0, /* CH1 (PB6) */ + SERVO_TILT = 1, /* CH2 (PB7) */ + SERVO_COUNT +} ServoChannel; + +/* Servo state */ +typedef struct { + uint16_t current_angle_deg[SERVO_COUNT]; /* Current angle in degrees (0-180) */ + uint16_t target_angle_deg[SERVO_COUNT]; /* Target angle in degrees */ + uint16_t pulse_us[SERVO_COUNT]; /* Pulse width in microseconds (500-2500) */ + uint32_t sweep_start_ms; + uint32_t sweep_duration_ms; + bool is_sweeping; +} ServoState; + +/* + * servo_init() + * + * Initialize TIM4 PWM on PB6 (CH1, pan) and PB7 (CH2, tilt) at 50 Hz. + * Centers both servos at 90° (1500 µs). Call once at startup. + */ +void servo_init(void); + +/* + * servo_set_angle(channel, degrees) + * + * Set target angle for a servo (0-180°). + * Immediately updates PWM without motion ramping. + * Valid channels: SERVO_PAN, SERVO_TILT + * + * Examples: + * servo_set_angle(SERVO_PAN, 0); // Pan left + * servo_set_angle(SERVO_PAN, 90); // Pan center + * servo_set_angle(SERVO_TILT, 180); // Tilt up + */ +void servo_set_angle(ServoChannel channel, uint16_t degrees); + +/* + * servo_get_angle(channel) + * + * Return current servo angle in degrees (0-180). + */ +uint16_t servo_get_angle(ServoChannel channel); + +/* + * servo_set_pulse_us(channel, pulse_us) + * + * Set servo pulse width directly in microseconds (500-2500). + * Used for fine-tuning or direct control. + */ +void servo_set_pulse_us(ServoChannel channel, uint16_t pulse_us); + +/* + * servo_sweep(channel, start_deg, end_deg, duration_ms) + * + * Smooth linear sweep from start to end angle over duration_ms. + * Non-blocking: must call servo_tick() every ~10 ms to update PWM. + * + * Examples: + * servo_sweep(SERVO_PAN, 0, 180, 2000); // Pan left-to-right in 2 seconds + * servo_sweep(SERVO_TILT, 45, 135, 1000); // Tilt up-down in 1 second + */ +void servo_sweep(ServoChannel channel, uint16_t start_deg, uint16_t end_deg, uint32_t duration_ms); + +/* + * servo_tick(now_ms) + * + * Update servo sweep animation (if active). Call every ~10 ms from main loop. + * No-op if not currently sweeping. + */ +void servo_tick(uint32_t now_ms); + +/* + * servo_is_sweeping() + * + * Returns true if any servo is currently sweeping. + */ +bool servo_is_sweeping(void); + +/* + * servo_stop_sweep(channel) + * + * Stop sweep immediately, hold current position. + */ +void servo_stop_sweep(ServoChannel channel); + +#endif /* SERVO_H */ diff --git a/src/main.c b/src/main.c index 626214f..c3dcfd5 100644 --- a/src/main.c +++ b/src/main.c @@ -21,6 +21,7 @@ #include "audio.h" #include "buzzer.h" #include "led.h" +#include "servo.h" #include "power_mgmt.h" #include "battery.h" #include @@ -163,6 +164,9 @@ int main(void) { /* Init power management — STOP-mode sleep/wake, wake EXTIs configured */ power_mgmt_init(); + /* Init servo pan-tilt driver for camera head (TIM4 PWM on PB6/PB7, Issue #206) */ + servo_init(); + /* Init mode manager (RC/autonomous blend; CH6 mode switch) */ mode_manager_t mode; mode_manager_init(&mode); @@ -218,6 +222,9 @@ int main(void) { /* Advance LED animation sequencer (non-blocking, call every tick) */ led_tick(now); + /* Servo pan-tilt animation tick — updates smooth sweeps */ + servo_tick(now); + /* Sleep LED: software PWM on LED1 (active-low PC15) driven by PM brightness. * pm_pwm_phase rolls over each ms; brightness sets duty cycle 0-255. */ pm_pwm_phase++; diff --git a/src/servo.c b/src/servo.c new file mode 100644 index 0000000..59e42df --- /dev/null +++ b/src/servo.c @@ -0,0 +1,242 @@ +#include "servo.h" +#include "config.h" +#include "stm32f7xx_hal.h" +#include + +/* ================================================================ + * Servo PWM Protocol + * ================================================================ + * TIM4 at 50 Hz (20 ms period) + * APB1 clock: 54 MHz + * Prescaler: 53 (54 MHz / 54 = 1 MHz) + * ARR: 19999 (1 MHz / 20000 = 50 Hz) + * CCR: 500-2500 (0.5-2.5 ms out of 20 ms) + * + * Servo pulse mapping: + * 500 µs → 0° (full left/down) + * 1500 µs → 90° (center) + * 2500 µs → 180° (full right/up) + */ + +#define SERVO_PWM_FREQ 50u /* 50 Hz */ +#define SERVO_PERIOD_MS 20u /* 20 ms = 1/50 Hz */ +#define SERVO_CLOCK_HZ 1000000u /* 1 MHz timer clock */ +#define SERVO_PRESCALER 53u /* APB1 54 MHz / 54 = 1 MHz */ +#define SERVO_ARR 19999u /* 1 MHz / 20000 = 50 Hz */ + +typedef struct { + uint16_t current_angle_deg[SERVO_COUNT]; + uint16_t target_angle_deg[SERVO_COUNT]; + uint16_t pulse_us[SERVO_COUNT]; + + /* Sweep state */ + uint32_t sweep_start_ms[SERVO_COUNT]; + uint32_t sweep_duration_ms[SERVO_COUNT]; + uint16_t sweep_start_deg[SERVO_COUNT]; + uint16_t sweep_end_deg[SERVO_COUNT]; + bool is_sweeping[SERVO_COUNT]; +} ServoState; + +static ServoState s_servo = {0}; +static TIM_HandleTypeDef s_tim_handle = {0}; + +/* ================================================================ + * Helper functions + * ================================================================ + */ + +static uint16_t angle_to_pulse_us(uint16_t degrees) +{ + /* Linear interpolation: 0° → 500µs, 180° → 2500µs */ + if (degrees > 180) degrees = 180; + + uint32_t pulse = SERVO_MIN_US + (uint32_t)degrees * (SERVO_MAX_US - SERVO_MIN_US) / 180; + return (uint16_t)pulse; +} + +static uint16_t pulse_us_to_angle(uint16_t pulse_us) +{ + /* Inverse mapping: 500µs → 0°, 2500µs → 180° */ + if (pulse_us < SERVO_MIN_US) pulse_us = SERVO_MIN_US; + if (pulse_us > SERVO_MAX_US) pulse_us = SERVO_MAX_US; + + uint32_t angle = (uint32_t)(pulse_us - SERVO_MIN_US) * 180 / (SERVO_MAX_US - SERVO_MIN_US); + return (uint16_t)angle; +} + +static void update_pwm(ServoChannel channel) +{ + /* Convert pulse width (500-2500 µs) to CCR value */ + /* At 1 MHz timer clock: 1 µs = 1 count */ + uint32_t ccr_value = s_servo.pulse_us[channel]; + + if (channel == SERVO_PAN) { + __HAL_TIM_SET_COMPARE(&s_tim_handle, SERVO_PAN_CHANNEL, ccr_value); + } else { + __HAL_TIM_SET_COMPARE(&s_tim_handle, SERVO_TILT_CHANNEL, ccr_value); + } +} + +/* ================================================================ + * Public API + * ================================================================ + */ + +void servo_init(void) +{ + /* Initialize state */ + memset(&s_servo, 0, sizeof(s_servo)); + + /* Center both servos at 90° */ + for (int i = 0; i < SERVO_COUNT; i++) { + s_servo.current_angle_deg[i] = 90; + s_servo.target_angle_deg[i] = 90; + s_servo.pulse_us[i] = SERVO_CENTER_US; + } + + /* Configure GPIO PB6 (CH1) and PB7 (CH2) as TIM4 PWM */ + __HAL_RCC_GPIOB_CLK_ENABLE(); + + GPIO_InitTypeDef gpio_init = {0}; + gpio_init.Mode = GPIO_MODE_AF_PP; + gpio_init.Pull = GPIO_NOPULL; + gpio_init.Speed = GPIO_SPEED_FREQ_HIGH; + gpio_init.Alternate = SERVO_AF; + + /* Configure PB6 (pan) */ + gpio_init.Pin = SERVO_PAN_PIN; + HAL_GPIO_Init(SERVO_PAN_PORT, &gpio_init); + + /* Configure PB7 (tilt) */ + gpio_init.Pin = SERVO_TILT_PIN; + HAL_GPIO_Init(SERVO_TILT_PORT, &gpio_init); + + /* Configure TIM4: 50 Hz PWM */ + __HAL_RCC_TIM4_CLK_ENABLE(); + + s_tim_handle.Instance = SERVO_TIM; + s_tim_handle.Init.Prescaler = SERVO_PRESCALER; + s_tim_handle.Init.CounterMode = TIM_COUNTERMODE_UP; + s_tim_handle.Init.Period = SERVO_ARR; + s_tim_handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + s_tim_handle.Init.RepetitionCounter = 0; + + HAL_TIM_PWM_Init(&s_tim_handle); + + /* Configure TIM4_CH1 (pan) for PWM */ + TIM_OC_InitTypeDef oc_init = {0}; + oc_init.OCMode = TIM_OCMODE_PWM1; + oc_init.Pulse = SERVO_CENTER_US; + oc_init.OCPolarity = TIM_OCPOLARITY_HIGH; + oc_init.OCFastMode = TIM_OCFAST_DISABLE; + + HAL_TIM_PWM_ConfigChannel(&s_tim_handle, &oc_init, SERVO_PAN_CHANNEL); + HAL_TIM_PWM_Start(&s_tim_handle, SERVO_PAN_CHANNEL); + + /* Configure TIM4_CH2 (tilt) for PWM */ + oc_init.Pulse = SERVO_CENTER_US; + HAL_TIM_PWM_ConfigChannel(&s_tim_handle, &oc_init, SERVO_TILT_CHANNEL); + HAL_TIM_PWM_Start(&s_tim_handle, SERVO_TILT_CHANNEL); +} + +void servo_set_angle(ServoChannel channel, uint16_t degrees) +{ + if (channel >= SERVO_COUNT) return; + if (degrees > 180) degrees = 180; + + s_servo.current_angle_deg[channel] = degrees; + s_servo.target_angle_deg[channel] = degrees; + s_servo.pulse_us[channel] = angle_to_pulse_us(degrees); + + /* Stop any sweep in progress */ + s_servo.is_sweeping[channel] = false; + + /* Update PWM immediately */ + update_pwm(channel); +} + +uint16_t servo_get_angle(ServoChannel channel) +{ + if (channel >= SERVO_COUNT) return 0; + return s_servo.current_angle_deg[channel]; +} + +void servo_set_pulse_us(ServoChannel channel, uint16_t pulse_us) +{ + if (channel >= SERVO_COUNT) return; + if (pulse_us < SERVO_MIN_US) pulse_us = SERVO_MIN_US; + if (pulse_us > SERVO_MAX_US) pulse_us = SERVO_MAX_US; + + s_servo.pulse_us[channel] = pulse_us; + s_servo.current_angle_deg[channel] = pulse_us_to_angle(pulse_us); + s_servo.target_angle_deg[channel] = s_servo.current_angle_deg[channel]; + + /* Stop any sweep in progress */ + s_servo.is_sweeping[channel] = false; + + /* Update PWM immediately */ + update_pwm(channel); +} + +void servo_sweep(ServoChannel channel, uint16_t start_deg, uint16_t end_deg, uint32_t duration_ms) +{ + if (channel >= SERVO_COUNT) return; + if (duration_ms == 0) return; + if (start_deg > 180) start_deg = 180; + if (end_deg > 180) end_deg = 180; + + s_servo.sweep_start_deg[channel] = start_deg; + s_servo.sweep_end_deg[channel] = end_deg; + s_servo.sweep_duration_ms[channel] = duration_ms; + s_servo.sweep_start_ms[channel] = 0; /* Will be set on first tick */ + s_servo.is_sweeping[channel] = true; +} + +void servo_tick(uint32_t now_ms) +{ + for (int ch = 0; ch < SERVO_COUNT; ch++) { + if (!s_servo.is_sweeping[ch]) continue; + + /* Initialize start time on first call */ + if (s_servo.sweep_start_ms[ch] == 0) { + s_servo.sweep_start_ms[ch] = now_ms; + } + + uint32_t elapsed = now_ms - s_servo.sweep_start_ms[ch]; + uint32_t duration = s_servo.sweep_duration_ms[ch]; + + if (elapsed >= duration) { + /* Sweep complete */ + s_servo.is_sweeping[ch] = false; + s_servo.current_angle_deg[ch] = s_servo.sweep_end_deg[ch]; + s_servo.pulse_us[ch] = angle_to_pulse_us(s_servo.sweep_end_deg[ch]); + } else { + /* Linear interpolation */ + int16_t start = (int16_t)s_servo.sweep_start_deg[ch]; + int16_t end = (int16_t)s_servo.sweep_end_deg[ch]; + int32_t delta = end - start; + + /* angle = start + (delta * elapsed / duration) */ + int32_t angle_i32 = start + (delta * (int32_t)elapsed / (int32_t)duration); + s_servo.current_angle_deg[ch] = (uint16_t)angle_i32; + s_servo.pulse_us[ch] = angle_to_pulse_us(s_servo.current_angle_deg[ch]); + } + + /* Update PWM */ + update_pwm((ServoChannel)ch); + } +} + +bool servo_is_sweeping(void) +{ + for (int i = 0; i < SERVO_COUNT; i++) { + if (s_servo.is_sweeping[i]) return true; + } + return false; +} + +void servo_stop_sweep(ServoChannel channel) +{ + if (channel >= SERVO_COUNT) return; + s_servo.is_sweeping[channel] = false; +} diff --git a/test/test_servo.py b/test/test_servo.py new file mode 100644 index 0000000..1d767ee --- /dev/null +++ b/test/test_servo.py @@ -0,0 +1,345 @@ +""" +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'])