from machine import Pin, PWM
import time

# Define pins
led_pins = [Pin(i, Pin.OUT) for i in range(3)]  # GP0, GP1, GP2
rgb_pins = [Pin(i, Pin.OUT) for i in range(3, 6)]  # GP3, GP4, GP5

# Setup servos using PWM
servo1 = PWM(Pin(6))  # GP6
servo2 = PWM(Pin(7))  # GP7
servo1.freq(50)
servo2.freq(50)

# Initialize variables
current_led = 0
current_color = 0
servo_pos = 0
sweep_direction = -1

def set_servo_angle(servo, angle):
    # Convert angle (0-180) to duty cycle for 50Hz PWM
    # Typically 0 degrees = 1ms pulse, 180 degrees = 2ms pulse
    # For 16-bit PWM resolution (0-65535), this maps to approximately:
    duty = int(1640 + (angle / 180) * 6550)  # ~1640 (1ms) to ~8190 (2ms)
    servo.duty_u16(duty)

def set_color(color):
    for i in range(3):
        rgb_pins[i].value(0)
    rgb_pins[color].value(1)

def sweep_servos():
    global servo_pos, sweep_direction
    
    if servo_pos == 0 or servo_pos == 180:
        sweep_direction *= -1
        time.sleep(2)
    
    if sweep_direction == 1:
        while servo_pos < 180:
            servo_pos += sweep_direction * 10
            # Ensure we don't exceed 180
            if servo_pos > 180:
                servo_pos = 180
            set_servo_angle(servo1, servo_pos)
            set_servo_angle(servo2, servo_pos)
            time.sleep(0.1)
    
    if sweep_direction == -1:
        while servo_pos > 0:
            servo_pos += sweep_direction * 10
            # Ensure we don't go below 0
            if servo_pos < 0:
                servo_pos = 0
            set_servo_angle(servo1, servo_pos)
            set_servo_angle(servo2, servo_pos)
            time.sleep(0.1)

# Set initial positions
set_servo_angle(servo1, 0)
set_servo_angle(servo2, 0)
set_color(0)

# Main loop
while True:
    led_pins[current_led].value(1)
    set_color(current_color)
    sweep_servos()
    time.sleep(0.2)
    led_pins[current_led].value(0)
    current_led = (current_led + 1) % 3
    current_color = (current_color + 1) % 3