from machine import Pin, PWM, ADC
import time

# LDR
izq = ADC(26)
cen = ADC(27)
der = ADC(28)

umbral = 30000

# Ultrasonico
trig = Pin(3, Pin.OUT)
echo = Pin(4, Pin.IN)

# Motor (LED)
motor = Pin(5, Pin.OUT)

# Servos
servodir = PWM(Pin(21))
servodir.freq(50)

servo_puerta = PWM(Pin(20))
servo_puerta.freq(50)

# LEDs
led_izq = Pin(8, Pin.OUT)
led_der = Pin(9, Pin.OUT)
led_on  = Pin(10, Pin.OUT)

# Buzzer
buzzer = PWM(Pin(11))
buzzer.freq(1000)

# BOTON PULL_UP
boton = Pin(12, Pin.IN, Pin.PULL_UP)

encendido = False
estado_boton = 0

while True:

    # BOTON (PULL_UP)
    if boton.value() == 0 and estado_boton == 0:
        encendido = not encendido
        estado_boton = 1
        time.sleep(0.3)

    if boton.value() == 1:
        estado_boton = 0

    # ESTADO
    if encendido:
        led_on.high()
    else:
        led_on.low()
        motor.low()
        continue

    # DISTANCIA
    trig.low()
    time.sleep_us(2)
    trig.high()
    time.sleep_us(10)
    trig.low()

    while echo.value() == 0:
        inicio = time.ticks_us()

    while echo.value() == 1:
        fin = time.ticks_us()

    distancia = (fin - inicio) / 58

    # OBSTACULO
    if distancia < 15:
        motor.low()
        led_izq.high()
        led_der.high()

        for i in range(5):
            buzzer.duty_u16(30000)
            time.sleep(0.2)
            buzzer.duty_u16(0)
            time.sleep(0.2)

        time.sleep(5)

        led_izq.low()
        led_der.low()
        continue

    # LECTURA
    L = izq.read_u16()
    C = cen.read_u16()
    R = der.read_u16()

    # PARADA
    if L < umbral and C < umbral and R < umbral:
        motor.low()
        led_izq.high()
        led_der.high()

        for i in range(3):
            buzzer.duty_u16(30000)
            time.sleep(0.2)
            buzzer.duty_u16(0)
            time.sleep(0.2)

        servo_puerta.duty_u16(4000)
        time.sleep(5)
        servo_puerta.duty_u16(2000)

        led_izq.low()
        led_der.low()
        continue

    # SEGUIR LINEA
    if C < umbral:
        servodir.duty_u16(4000)
        motor.high()
        led_izq.low()
        led_der.low()

    elif L < umbral:
        servodir.duty_u16(3000)
        motor.high()
        led_izq.high()
        led_der.low()

    elif R < umbral:
        servodir.duty_u16(5000)
        motor.high()
        led_izq.low()
        led_der.high()

    else:
        servodir.duty_u16(4000)
        motor.high()
        led_izq.low()
        led_der.low()

    time.sleep(0.1)