from machine import Pin, PWM, ADC, UART
import utime
#Define los pines Raspberry Pi PICO
#Definición de sensor ultrasónico
trig = Pin(6, Pin.OUT)
echo = Pin(7, Pin.IN)
#Definición de switch selectores de casos
sel_uno = Pin(15, Pin.IN)
sel_dos = Pin(16, Pin.IN)
led_uno = Pin(14, Pin.OUT)
led_dos = Pin(17, Pin.OUT)
#Definición de sensores detectores de línea
sen_izq = Pin(3, Pin.IN)
sen_der = Pin(2, Pin.IN)
#Definición de sensores detectores de luz
ldr_izq = ADC(28)
ldr_der = ADC(27)
#Define velocidad del puerto bluetooth
blue = UART(0,9600)
# Definición de motores representados gráficamente con leds indicadores
mizq_uno=PWM(Pin(9))
mizq_uno.freq(5500)
mizq_dos=PWM(Pin(10))
mizq_dos.freq(5500)
mder_uno=PWM(Pin(22))
mder_uno.freq(5500)
mder_dos=PWM(Pin(21))
mder_dos.freq(5500)

while 1:
# Definición de motores representados gráficamente con leds indicadores
    luz_izq = ldr_izq.read_u16()
    luz_der = ldr_der.read_u16()
# Evasor de obstaculos
    if sel_uno.value()==0 and sel_dos.value()==0: # Selección 0-0
        led_uno.value(0), led_dos.value(0) # Apaga leds indicadores de caso
        trig.value(0) # Setup se sensor ultrasónico
        utime.sleep_us(5)
        trig.value(1)
        utime.sleep_us(10)
        trig.value(0)
        while echo.value() == 0:
            pass
        Tmrstrt = utime.ticks_us()
        while echo.value() == 1:
            pass
        Tmrend = utime.ticks_us()
        Duration = utime.ticks_diff(Tmrend, Tmrstrt)
        distance = Duration * 0.0171
        print(distance) # Imprime la distancia registrada
        utime.sleep(.5)
        if distance < 25: # proceso si el valor de la distancia es < 25 
            mizq_uno.duty_u16(40000)
            mizq_dos.duty_u16(0)
            mder_uno.duty_u16(0)
            mder_dos.duty_u16(40000)
        if distance > 25 and distance < 50: # proceso si el valor de la distancia está entre 25 y 50 
            mizq_uno.duty_u16(0)
            mizq_dos.duty_u16(0)
            mder_uno.duty_u16(40000)
            mder_dos.duty_u16(0)
        if distance > 50: # proceso si el valor de la distancia es > 25 
            mizq_uno.duty_u16(0)
            mizq_dos.duty_u16(40000)
            mder_uno.duty_u16(40000)
            mder_dos.duty_u16(0)
# seguidor de linea
    elif sel_uno.value()==1 and sel_dos.value()==0:
        led_uno.value(1), led_dos.value(0)
        if  sen_der.value() == 0 and sen_izq.value() == 0:
            mizq_uno.duty_u16(0)
            mizq_dos.duty_u16(50000)
            mder_uno.duty_u16(50000)
            mder_dos.duty_u16(0)
        elif sen_der.value() == 1 and sen_izq.value() == 0:
            mizq_uno.duty_u16(0)
            mizq_dos.duty_u16(0)
            mder_uno.duty_u16(50000)
            mder_dos.duty_u16(0)
        elif sen_der.value() == 0 and sen_izq.value() == 1:
            mizq_uno.duty_u16(0)
            mizq_dos.duty_u16(50000)
            mder_uno.duty_u16(0)
            mder_dos.duty_u16(0)
        elif sen_der.value() == 1 and sen_izq.value() == 1:
            mizq_uno.duty_u16(0)
            mizq_dos.duty_u16(0)
            mder_uno.duty_u16(0)
            mder_dos.duty_u16(0)
# seguidor de luz
    elif sel_uno.value()==0 and sel_dos.value()==1:
        led_uno.value(0), led_dos.value(1)
        print(luz_izq)
        print(luz_der)
        print("**************")
        utime.sleep(.2)
        if luz_izq > 15000:
            mizq_uno.duty_u16(0)
            mizq_dos.duty_u16(50000)
        elif luz_izq < 15000:
            mizq_uno.duty_u16(0)
            mizq_dos.duty_u16(0)
        if luz_der > 15000:
            mder_uno.duty_u16(50000)
            mder_dos.duty_u16(0)
        elif luz_der < 15000:
            mder_uno.duty_u16(0)
            mder_dos.duty_u16(0)
# Bluetooth
    elif sel_uno.value()==1 and sel_dos.value()==1:
        led_uno.value(1), led_dos.value(1)
        if blue.any():
            msg = blue.readline()
            print(msg)
            if msg == b'A':
                mder_uno.duty_u16(40000)
                mder_dos.duty_u16(0)
            if msg == b'B':
                mder_uno.duty_u16(0)
                mder_dos.duty_u16(40000)
            if msg == b'C':
                mizq_uno.duty_u16(0)
                mizq_dos.duty_u16(40000)
            if msg == b'D':
                mizq_uno.duty_u16(40000)
                mizq_dos.duty_u16(0)
            if msg == b'E':
                mizq_uno.duty_u16(0)
                mizq_dos.duty_u16(0)
                mder_uno.duty_u16(0)
                mder_dos.duty_u16(0)