from machine import Pin, PWM
from utime import sleep

RED_PIN = Pin(15, Pin.OUT)
GREEN_PIN = Pin(14, Pin.OUT)
BLUE_PIN = Pin(13, Pin.OUT)
redLED = Pin(5, Pin.OUT)
blueLED = Pin(4, Pin.OUT)
blueLED.on()

def set_color(red, green, blue):
    print(f"""{red=}
{green=}
{blue=}\n{'-' * 10}""")
    RED_PIN.value(red)
    GREEN_PIN.value(int(not GREEN_PIN.value()))
    BLUE_PIN.value(blue)

servoPin = 12
servo = PWM(Pin(servoPin))
servo.freq(50)

def set_servo_angle(angle):
    pulse_width = int(500 + (angle / 180) * 2000)
    servo.duty_u16(pulse_width * 65535 // 20000)

while True:
    for pos in range(0, 181, 1):
        set_servo_angle(pos)
        # sleep(0.015)
    
    for pos in range(180, -1, -1):
        set_servo_angle(pos)
        sleep(0.015)
    
    blueLED.toggle()
    set_color(int(not blueLED.value()), 0, int(not redLED.value()))
    redLED.toggle()
    set_color(int(not blueLED.value()), 0, int(not redLED.value()))
    sleep(1)