from machine import Pin, PWM
from time import sleep

servo_rojo = PWM(Pin(27), freq=50, duty=0)
servo_verde = PWM(Pin(26), freq=50, duty=0)
servo_azul = PWM(Pin(25), freq=50, duty=0)

def angle_servo(servo,angle):
    dutyC = int(((angle)/180 *2 + 0.5) / 20 * 1023)
    servo.duty(dutyC)

while True:
    angle_servo(servo_azul, 45)
    angle_servo(servo_rojo, 180)
    angle_servo(servo_verde,0)
    sleep(0.5)

    '''r, g, b = color_sensor.read('rgb')
        color = clasificar_color(r, g, b)

        motor_on(700)
        time.sleep(1)
        motor_off()

        if color == "rojo":
            angle_servo(servo_rojo, 90)
            conteo["rojo"] += 1
        elif color == "verde":
            angle_servo(servo_verde, 90)
            conteo["verde"] += 1
        elif color == "azul":
            angle_servo(servo_azul, 90)
            conteo["azul"] += 1

        time.sleep(1)
        angle_servo(servo_rojo, 0)
        angle_servo(servo_verde, 0)
        angle_servo(servo_azul, 0)

        mostrar_estado()

        if modo == "restringido" and sum(conteo.values()) >= limite_restringido:
            motor_off()
            oled.fill(0)
            oled.text("LIMITE ALCANZADO", 0, 0)
            oled.show()
            while True:
                pass

        time.sleep(0.1)'''

        
'''keypad = keypad_4x4.Keypad4x4(rows=[32, 33, 34, 35], cols=[36, 39, 19, 18])

conteo = {"rojo": 0, "verde": 0, "azul": 0}
modo = "tradicional"  # o "restringido"
limite_restringido = 0'''

'''def clasificar_color(r, g, b):
    if r > g and r > b:
        return "rojo"
    elif g > r and g > b:
        return "verde"
    elif b > r and b > g:
        return "azul"
    else:
        return "desconocido"

def mostrar_estado():
    oled.fill(0)
    oled.text("Modo: {}".format(modo), 0, 0)
    oled.text("Rojo: {}".format(conteo["rojo"]), 0, 20)
    oled.text("Verde: {}".format(conteo["verde"]), 0, 30)
    oled.text("Azul: {}".format(conteo["azul"]), 0, 40)
    oled.show()

def resetear():
    global conteo, modo, limite_restringido
    conteo = {"rojo": 0, "verde": 0, "azul": 0}
    modo = "tradicional"
    limite_restringido = 0
    motor_off()
    mostrar_estado()

oled.fill(0)
oled.text("1:T 2:R", 0, 0)
oled.show()
while True:
    tecla = keypad.get_key()
    if tecla == '1':
        modo = "tradicional"
        break
    elif tecla == '2':
        modo = "restringido"
        oled.fill(0)
        oled.text("Limite:", 0, 0)
        oled.show()
        limite = ""
        while True:
            k = keypad.get_key()
            if k and k.isdigit():
                limite += k
                oled.text(k, len(limite)*8, 10)
                oled.show()
            elif k == '#':
                limite_restringido = int(limite)
                break
        break'''