'''
El siguiente codigo es un prototipo de resolverdor de laberinto usando una
raspberry pi pico, dos sensores ultrasonicos para sensar la distancia y dos motores
a paso para la movilidad del robot, si los sensores detectan 
una distancia muy cercana tomará la accion de aumentar la velocidad de uno de los 
dos motores, dependiendo de que sensor se encuentre mas cerca de una pared tomará
la desición de girar a la izq o derecha
'''

import machine
import utime

DIR_PINA = 2
DIR_PINB = 4
STEP_PINA = 3
STEP_PINB = 5
TRIGA = 6
ECHA = 7
TRIGB = 8
ECHB = 9

def setup():
    machine.Pin(STEP_PINA, machine.Pin.OUT)
    machine.Pin(DIR_PINA, machine.Pin.OUT)
    machine.Pin(STEP_PINB, machine.Pin.OUT)
    machine.Pin(DIR_PINB, machine.Pin.OUT)
    machine.Pin(TRIGA, machine.Pin.OUT)
    machine.Pin(TRIGB, machine.Pin.OUT)
    machine.Pin(ECHA, machine.Pin.IN)
    machine.Pin(ECHB, machine.Pin.IN)
   

def main():
    distancia1 = obtener_distancia(TRIGA, ECHA)
    distancia2 = obtener_distancia(TRIGB, ECHB)

    if distancia1 > 20 and distancia2 > 20:
        avanzar()
    elif distancia1 <= 20:
        girar_derecha()
    elif distancia2 <= 20:
        girar_izquierda()

    print("Distancia1: {} cm".format(distancia1))
    print("Distancia2: {} cm".format(distancia2))
    utime.sleep_ms(100)

# Función para obtener la distancia
def obtener_distancia(trig_pin, echo_pin):
    
    trig = machine.Pin(trig_pin, machine.Pin.OUT)
    echo = machine.Pin(echo_pin, machine.Pin.IN)

    trig.off()
    utime.sleep_us(2)
    trig.on()
    utime.sleep_us(10)
    trig.off()

    duracion = machine.time_pulse_us(echo, 1)
    distancia = duracion * 0.034 / 2

    return distancia

def avanzar():
    
    machine.Pin(DIR_PINA, machine.Pin.OUT).on()
    machine.Pin(DIR_PINB, machine.Pin.OUT).on()
    machine.Pin(STEP_PINA, machine.Pin.OUT).on()
    machine.Pin(STEP_PINB, machine.Pin.OUT).on()
    utime.sleep_ms(5)
    machine.Pin(STEP_PINA, machine.Pin.OUT).off()
    machine.Pin(STEP_PINB, machine.Pin.OUT).off()
    utime.sleep_ms(5)

def girar_izquierda():
    
    machine.Pin(DIR_PINA, machine.Pin.OUT).on()
    machine.Pin(DIR_PINB, machine.Pin.OUT).off()
    machine.Pin(STEP_PINA, machine.Pin.OUT).on()
    machine.Pin(STEP_PINB, machine.Pin.OUT).on()
    utime.sleep_ms(5)
    machine.Pin(STEP_PINA, machine.Pin.OUT).off()
    machine.Pin(STEP_PINB, machine.Pin.OUT).off()
    utime.sleep_ms(5)

def girar_derecha():
    
    machine.Pin(DIR_PINA, machine.Pin.OUT).off()
    machine.Pin(DIR_PINB, machine.Pin.OUT).on()
    machine.Pin(STEP_PINA, machine.Pin.OUT).on()
    machine.Pin(STEP_PINB, machine.Pin.OUT).on()
    utime.sleep_ms(5)
    machine.Pin(STEP_PINA, machine.Pin.OUT).off()
    machine.Pin(STEP_PINB, machine.Pin.OUT).off()
    utime.sleep_ms(5)

while True:
    main()