from machine import Pin, PWM
import time
servoB= PWM(Pin(15))
servoB.freq(50)

# Configuración del sensor ultrasónico
TRIG_PIN = 16  # GPIO para el Trigger
ECHO_PIN = 17  # GPIO para el Echo
trig = Pin(TRIG_PIN, Pin.OUT)
echo = Pin(ECHO_PIN, Pin.IN)

# Función para obtener la distancia
def get_distance():
    trig.value(0)
    
    time.sleep_us(2)
    trig.value(1)
    time.sleep_us(10)
    trig.value(0)
    while not echo.value():
        pass
    start = time.ticks_us()
    while echo.value():
        pass
    end = time.ticks_us()
    duration = end - start
    distance = duration * 0.034 / 2
    return distance

# Bucle principal
while True:
# Obtener la distancia
    distance = get_distance()
    print(f"Distancia: {distance:.2f} cm")
    time.sleep_ms(1)
   
    for pulso in range (500000, 2500000, 500):
        servoB.duty_ns(pulso)
        time.sleep_ms(1)