from machine import Pin, PWM
import time

# ── Clase Servo ───────────────────────────────
class Servo:
    def __init__(self, pin):
        self.pwm = PWM(Pin(pin), freq=50)

    def write(self, angulo):
        angulo = max(0, min(180, angulo))
        pulso_us = 500 + (2000 * angulo // 180)
        duty = pulso_us * 1023 // 20000
        self.pwm.duty(duty)

    def off(self):
        self.pwm.deinit()

# ── Configuración de hardware ─────────────────
servo = Servo(pin=18)   # Servo en GPIO 18

trigger = Pin(2, Pin.OUT)   # Trigger del HC-SR04
echo    = Pin(25, Pin.IN)    # Echo del HC-SR04

# ── Función para medir distancia ─────────────
def medir_distancia():
    trigger.value(0)
    time.sleep_us(5)
    trigger.value(1)
    time.sleep_us(10)
    trigger.value(0)

    while echo.value() == 0:
        inicio = time.ticks_us()
    while echo.value() == 1:
        fin = time.ticks_us()

    duracion = time.ticks_diff(fin, inicio)
    distancia = (duracion / 10) / 50   # cm
    return distancia

# ── Programa principal ───────────────────────
print("Iniciando sistema...")
servo.write(90)   # Posición inicial
time.sleep(1)

while True:
    d = medir_distancia()
    print("Distancia:", d, "cm")

    # Lógica de control del servo según distancia
    if d < 10:          # Objeto muy cerca
        servo.write(180)  # Cierra

    else:               # Objeto lejos
        servo.write(90) # Abre

    time.sleep_ms(200)