from machine import Pin, PWM
import time


# Configurar los pines
TRIG = Pin(4, Pin.OUT)
ECHO = Pin(5, Pin.IN)
motor_izquierda_avance = Pin(27, Pin.OUT)
motor_izquierda_reversa = Pin(14, Pin.OUT)
motor_derecha_avance = Pin(12, Pin.OUT)
motor_derecha_reversa = Pin(13, Pin.OUT)
servo = Pin(2)
pwm = PWM(servo, freq=50)
buzzer = 15

# set pin
motor_izquierda_avance.off()
motor_izquierda_reversa.off()
motor_derecha_avance.off()
motor_derecha_reversa.off()

# Variables definidas
distancia_sensor = 200
sonido = 0
# Funciones buzzer
def play(pin, frequency, duration_ms):
    buzzer = PWM(Pin(pin), freq=frequency, duty=512)
    time.sleep_ms(duration_ms)
    buzzer.deinit()

def play_detener(pin):
  tono = [(150, 100),(150, 100),(150, 100)]

  for frequency, duration_ms in tono:
    play(pin, frequency, duration_ms)
    time.sleep(0.1)  # Pausa de 0.1 segundos entre tonos

def play_avanzar(pin):
  tono = [(2500, 100),(1600, 100),(2500, 100)]

  for frequency, duration_ms in tono:
      play(pin, frequency, duration_ms)
      time.sleep(0.1)  # Pausa de 0.1 segundos entre tonos


# Función para mover el servo a una posición específica (ángulo)
def mover_servo(angulo):

    angulo = min(max(0, angulo), 180)
    duty = int((angulo / 180) * (115 - 40) + 40)
    pwm.duty(duty)
    time.sleep_ms(500) 

def medir_distancia():
    # Generar un pulso de 10µs en el pin TRIG para iniciar la medición
    TRIG.value(0)
    time.sleep_us(2)
    TRIG.value(1)
    time.sleep_us(10)
    TRIG.value(0)


    # Esperar a que el pin ECHO se ponga en alto
    while ECHO.value() == 0:
        pass
    start_time = time.ticks_us()

    # Esperar a que el pin ECHO se ponga en bajo
    while ECHO.value() == 1:
        pass
    end_time = time.ticks_us()

    # Calcular la duración del pulso y convertirlo a distancia en cm
    duracion = time.ticks_diff(end_time, start_time)
    distancia_cm = duracion * 0.0343 / 2

    return distancia_cm

while True:
    medir_distancia()
    if medir_distancia() > distancia_sensor:
        if sonido == 0:
            play_avanzar(buzzer)
            sonido = 1
        motor_izquierda_avance.on()
        motor_derecha_avance.on()
        time.sleep(1)
        
    if medir_distancia() < distancia_sensor:
        if sonido == 1:
            play_detener(buzzer)
            sonido = 0
        motor_izquierda_avance.off()
        motor_derecha_avance.off()
        time.sleep(1)
        mover_servo(35)
        medir_distancia()
        if medir_distancia() > distancia_sensor:    #
            motor_derecha_avance.on()
            mover_servo(90)
            time.sleep(1)
            motor_derecha_avance.off()
            time.sleep(1)
            motor_izquierda_avance.on()
            motor_derecha_avance.on()
            time.sleep(1)
        if medir_distancia() < distancia_sensor:
            motor_izquierda_avance.off()
            motor_derecha_avance.off()
            time.sleep(1)
            mover_servo(135)
            medir_distancia()
            if medir_distancia() > distancia_sensor:  #
                motor_izquierda_avance.on()
                mover_servo(90)
                time.sleep(1)
                motor_izquierda_avance.off()
                time.sleep(1)
                motor_izquierda_avance.on()
                motor_derecha_avance.on()
                time.sleep(1)
            if medir_distancia() < distancia_sensor:
                motor_izquierda_avance.off()
                motor_derecha_avance.off()
                time.sleep(1)
                mover_servo(90)