import machine
from machine import Pin,PWM
import time

ioioblue = Pin(14,Pin.OUT)
ioiored = Pin(15,Pin.OUT)

A = PWM(Pin(13))
B = PWM(Pin(12))
A.duty_u16(100)

servo = PWM(Pin(21))  # Use GPIO 5 for the servo
servo.freq(50)       # Servo motors typically use a 50Hz frequency

mega_led = Pin(13,Pin.OUT)
mega_led.value(1)

red = PWM(Pin(20))
green = PWM(Pin(19))
blue = PWM(Pin(18))
red.freq(1000)
green.freq(1000)
blue.freq(1000)
def servo_angle(angle):
    servo.duty_u16(int(1495 + (angle / 180) * (8153 - 1495)))

def set_color(r, g, b):
    """
    Set RGB LED color.
    :param r: Intensity of red (0-65535)
    :param g: Intensity of green (0-65535)
    :param b: Intensity of blue (0-65535)
    """
    red.duty_u16(r)
    green.duty_u16(g)
    blue.duty_u16(b)
# Function to set the servo angle


set_color(65535, 65535, 65535)  # Red
while True:
    ioioblue.value(1)
    set_color(65535, 0, 0)
    servo_angle(0)
    time.sleep(0.5)
    ioioblue.value(0)
    ioiored.value(1)
    set_color(0, 0, 65535)
    servo_angle(180)
    time.sleep(0.5)
    ioiored.value(0)

    


#while True:
    # Move servo to 0 degrees
    #set_servo_angle(0)
    #print("set to 0")
    #time.sleep(1)
    
    # Move servo to 90 degrees
    #set_servo_angle(90)
    #print("set to 90")
    #time.sleep(1)
    
    # Move servo to 180 degrees
    #set_servo_angle(180)
    #print("set to 180")
    #time.sleep(1)