# Proyecto: Clasificador Automatizado de Medicamentos
from machine import Pin, SoftI2C, PWM
import time
import ssd1306 
#import tcs34725 
#import keypad_4x4

# MORADOS DE LOS SERVOS PWM
# CELESTES OLED SDA Y SCL
# LED PUENTE H

motor = PWM(Pin(32), freq=1000)  
i2c = SoftI2C(scl=Pin(22), sda=Pin(21))
oled_width = 128
oled_height = 64
oled = ssd1306.SSD1306_I2C(oled_width, oled_height, i2c)

servo_rojo = PWM(Pin(14), freq=50)
servo_verde = PWM(Pin(26), freq=50)
servo_azul = PWM(Pin(25), freq=50)

modo = 'Eliana sapa'

def angle_servo(servo, angle):
    print('Yoyito')
    minimum=26
    maximum=126
    dutyC = int(minimum+(angle/180)*(maximum-minimum))
    servo.duty(dutyC)

def mostrar_estado(v1):
    oled.fill(0)
    oled.text(f"Modo: {v1}", 0, 0)
    oled.show()

while True:
    angle_servo(servo_azul, 45)
    angle_servo(servo_rojo, 180)
    angle_servo(servo_verde, 0)
    time.sleep(0.5)
    mostrar_estado(modo)