from machine import Pin, PWM, time_pulse_us
import time

# -------- SERVO --------
pin_servo = Pin(18)
servo = PWM(pin_servo, freq=50)

# -------- SENSOR ULTRASONICO --------
trigger = Pin(5, Pin.OUT)
echo = Pin(19, Pin.IN)

# -------- FUNCION MOVER SERVO --------
def mover_servo(angulo):
    duty = int((angulo / 180) * (123 - 26) + 26)
    servo.duty(duty)

# -------- MEDIR DISTANCIA --------
def medir_distancia():
    trigger.value(0)
    time.sleep_us(2)

    trigger.value(1)
    time.sleep_us(10)
    trigger.value(0)

    duracion = time_pulse_us(echo, 1, 30000)

    if duracion < 0:
        return 999

    distancia = (duracion / 2) / 29.1
    return distancia

print("Inicio del sistema")

try:
    while True:

        distancia = medir_distancia()
        print("Distancia:", distancia, "cm")

        # Detecta objeto a menos de 15 cm
        if distancia < 15:

            print("Objeto detectado")

            # Movimiento 0 → 180
            for angulo in range(0, 181, 5):
                mover_servo(angulo)
                time.sleep(0.05)

            # Mantener posición
            print("Esperando...")
            time.sleep(3)

            # Regresar 180 → 0
            for angulo in range(180, -1, -5):
                mover_servo(angulo)
                time.sleep(0.05)

            time.sleep(1)

        else:
            mover_servo(0)
            time.sleep(0.2)

except KeyboardInterrupt:
    servo.deinit()
    print("PWM detenido.")