from machine import Pin, PWM
import time

servo = PWM(Pin(18), freq=50)

trig = Pin(5, Pin.OUT)
echo = Pin(19, Pin.IN)

def distance():
    trig.off()
    time.sleep_us(2)
    trig.on()
    time.sleep_us(10)
    trig.off()

    while echo.value() == 0:
        pass
    start = time.ticks_us()

    while echo.value() == 1:
        pass
    end = time.ticks_us()

    duration = time.ticks_diff(end, start)
    return duration * 0.0343 / 2

while True:
    d = distance()
    print(d)

    if d < 20:
        servo.duty(115)   
    else:
        servo.duty(40)  

    time.sleep(1)