from machine import Pin, PWM
import time

# Pins used (GPIO 14, 15, 16, 17)
servo_pins = [14, 15, 16, 17]

# Create PWM objects for each servo
servos = []
for pin in servo_pins:
    pwm = PWM(Pin(pin))
    pwm.freq(50)  # 50Hz frequency suitable for servos
    servos.append(pwm)

# Function to convert angle (0-180) to duty cycle
def angle_to_duty(angle):
    # Pulse width between 0.5ms and 2.5ms, full cycle 20ms (50Hz)
    # duty = (pulse_ms / 20) * 65535
    pulse_us = 500 + (angle / 180) * 2000  # 500us for 0°, 2500us for 180°
    duty = int(pulse_us / 20000 * 65535)
    return duty

# Sweep all four servos together
print("Running four servos...")

while True:
    # From 0 to 180 degrees
    for angle in range(0, 181, 5):
        duty = angle_to_duty(angle)
        for s in servos:
            s.duty_u16(duty)
        time.sleep(0.02)
    
    # From 180 back to 0 degrees
    for angle in range(180, -1, -5):
        duty = angle_to_duty(angle)
        for s in servos:
            s.duty_u16(duty)
        time.sleep(0.02)