##ELECTROGONIOMETRO##
#CODO DE 0 GRADOS A 180 GRADOS#
from machine import Pin, PWM, ADC
from time import sleep 

pot= ADC(Pin(34))
pot.atten(ADC.ATTN_11DB)
joy= ADC(Pin(35))
joy.atten(ADC.ATTN_11DB)
servo= PWM(Pin(14), freq=50)
pulsador= Pin(27, Pin.IN, Pin.PULL_DOWN)
ledRojo= Pin(18, Pin.OUT)
ledVerde= Pin(19, Pin.OUT)
ledAzul= Pin(21, Pin.OUT)
ledEmer= Pin(22, Pin.OUT)

modo_emergencia= False
count= 0

def interrupcion(Pin):
    global modo_emergencia
    if modo_emergencia:
        modo_emergencia= False
    else: 
        modo_emergencia= True
    ledEmer.value(modo_emergencia)
pulsador.irq(trigger=Pin.IRQ_RISING, handler=interrupcion)

def angle_joystick():
    valor= joy.read()
    return(valor/4095)* 180

def angle_pot():
    valorPot = pot.read()
    minimo = 0
    maximo = 180
    angulo = minimo + (valorPot / 4095) * (maximo - minimo)
    return angulo

def angle_servo(angle):
    maximum=128
    minimum=26 #CONVERSION A BITS DEL PUNTO MINIMO Y MAXIMO
    dutyC=int(minimum+(angle/180)*(maximum-minimum))
    servo.duty(dutyC)

while True:
    if modo_emergencia:
        servo.duty(0)
        ledAzul.off()
        ledRojo.off()
        ledVerde.off()
        sleep(0.1)
        continue

    lectura_joy = joy.read()
    angulo_paciente = angle_pot()
    print(angulo_paciente)

    if lectura_joy > 3000:
        count += 1
        if count > 180: count = 180
    elif lectura_joy < 1000:
        count -= 1
        if count < 0: count = 0

    angle_servo(count) 
    print(count)

    diferencia = angulo_paciente - count

    if abs(diferencia) <= 10:
        ledVerde.on()
        ledAzul.off()
        ledRojo.off()
    elif diferencia > 10:
        ledRojo.on()
        ledAzul.off()
        ledVerde.off()
    else:
        ledAzul.on()
        ledRojo.off()
        ledVerde.off()

    sleep(0.1)