print("Hello, ESP32!")

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

# Definiciones iniciales
freq = 5000
modu1 = PWM(Pin(15), freq)
modu2 = PWM(Pin(16), 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
TRING = Pin(5, Pin.OUT)
ECHO = Pin(22, Pin.IN)
LED = Pin(27, Pin.OUT)
# variable para el pin del echo
# 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 detectte un objeto los motores funcionen al 100%
def avanzar1(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
#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    
    if distancia_cm <= 50:
        avanzar1()
    else:
        avanzar()
        derecha()
        izquierda()
        avanzar()
        


while True:
   ultrasonico()
   sleep(1)