from machine import Pin, PWM, time_pulse_us
from time import sleep_us, sleep_ms, sleep

# Definiciones iniciales
freq = 5000
modu1 = PWM(Pin(13), freq)
modu2 = PWM(Pin(12), freq)
miz1 = Pin(25, Pin.OUT)
miz2 = Pin(33, Pin.OUT)
mderi = Pin(18, Pin.OUT)
mder2 = Pin(21, Pin.OUT)
SONIDO = 340
PULSO = 10
#ultrasonico 1
TRING = Pin(5, Pin.OUT)  # ROSADO
ECHO = Pin(22, Pin.IN)  # GRIS
#infrarojos
infraTRA = [Pin(35, Pin.IN), Pin(32, Pin.IN)]
infraDELA = [Pin(26, Pin.IN), Pin(27, Pin.IN)]
# ultrasonico 2
TRING2 = Pin(14, Pin.OUT)  # AMARILLO
ECO2 = Pin(34, Pin.IN)  # BLANCO


# Funciones para los motores
def avanzar(duty_cycle=512):
    miz1.on()
    miz2.off()
    mderi.on()
    mder2.off()
    modu1.duty(duty_cycle)  # Controla el motor izquierdo
    modu2.duty(duty_cycle)  # Controla el motor derecho

def retroceder(duty_cycle=512):
    miz1.off()
    miz2.on()
    mderi.off()
    mder2.on()
    modu1.duty(duty_cycle)  # Controla el motor izquierdo
    modu2.duty(duty_cycle)  # Controla el motor derecho

def derecha(duty_cycle=512):
    miz1.on()
    miz2.off()
    mderi.off()
    mder2.on()
    modu1.duty(duty_cycle)  # Controla el motor izquierdo
    modu2.duty(int(duty_cycle/2))  # Reduce la velocidad del motor derecho para girar

def izquierda(duty_cycle=512):
    miz1.off()
    miz2.on()
    mderi.on()
    mder2.off()
    modu1.duty(int(duty_cycle/2))  # Reduce la velocidad del motor izquierdo para girar
    modu2.duty(duty_cycle)  # Controla el motor derecho

# objeto encontrado, para cuando el carro detecte un objeto los motores funcionen al 100%
def avan1(duty_cycle=1023):
    miz1.on()
    miz2.off()
    mderi.on()
    mder2.off()
    modu1.duty(duty_cycle)  # Controla el motor izquierdo
    modu2.duty(duty_cycle)  # Controla el motor derecho

# objeto encontrado, para cuando el carro detecte un objeto los motores funcionen al 100% retrocediendo
def retroceder1(duty_cycle=1023):
    miz1.off()
    miz2.on()
    mderi.off()
    mder2.on()
    modu1.duty(duty_cycle)  # Controla el motor izquierdo
    modu2.duty(duty_cycle)  # Controla el motor derecho


# Ultrasonico
def ultrasonico():
    TRING.value(0)
    sleep_us(5)
    TRING.value(1)
    sleep_us(PULSO)
    TRING.value(0)
    duracion = time_pulse_us(ECHO, 1, 30000)
    distancia_cm = SONIDO * duracion/20000
    print(f"distancia:{distancia_cm}cm")
    sleep_ms(500)  # CAMBIAR POR TIMER
    return distancia_cm

# Ultrasonico 2
def ultrasonico2():
    TRING2.value(0)
    sleep_us(5)
    TRING2.value(1)
    sleep_us(PULSO)
    TRING2.value(0)
    duracion = time_pulse_us(ECO2, 1, 30000)
    distancia_cm = SONIDO * duracion/20000
    print(f"distancia:{distancia_cm}cm")
    sleep_ms(500)  
    return distancia_cm

#INFRARROJOS AVANZA

def sensor(pin):           
    if pin.value() == 1:  # Verifica si el pin que generó la interrupción está en alto
        avan1()
        sleep(0.6)

for infrarrojo in infraTRA:
    infrarrojo.irq(trigger=Pin.IRQ_FALLING, handler=sensor)

#INFRAROJO RETROCEDE
def sensor2(pin):           
    if pin.value() == 1:  # Verifica si el pin que generó la interrupción está en alto
        retroceder1()
        sleep(0.6)

for infrarrojo in infraDELA:
    infrarrojo.irq(trigger=Pin.IRQ_FALLING, handler=sensor2)

#infrarrojosdelan.irq(trigger=Pin.IRQ_FALLING, handler=sensor)
    
# Bucle con while True para todas las funciones
while True:
    distancia1 = ultrasonico()
    distancia2 = ultrasonico2()

    if distancia1 <= 55:
        avan1()
    elif distancia2 <= 55:
        retroceder1()
    else:
        avanzar()
        sleep(0.5)
        retroceder()
        sleep(0.5)
        distancia1 = ultrasonico()
        distancia2 = ultrasonico2()
        derecha()
        sleep(1)
        izquierda()
        sleep(1)