from machine import Pin, PWM
import time

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

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

def get_distance():
    trig.value(0)
    time.sleep_us(2)
    trig.value(1)
    time.sleep_us(10)
    trig.value(0)
    
    while echo.value() == 0:
        pass
    t1 = time.ticks_us()
    while echo.value() == 1:
        pass
    t2 = time.ticks_us()
    
    duration = time.ticks_diff(t2, t1)
    return (duration * 0.0343) / 2

def set_servo_angle(angle):
    duty = int(40 + (angle / 180) * 75)
    servo.duty(duty)

while True:
    dist = get_distance()
    print("Distance: {:.1f} cm".format(dist))
    
    if dist < 20:
        set_servo_angle(90)
    else:
        set_servo_angle(0)
        
    time.sleep(0.5)