import time
from machine import Pin,PWM

trig = Pin(5,Pin.OUT)
echo = Pin(4,Pin.IN)
servo = PWM(Pin(0, Pin.OUT))
servo.freq(50)
servo.duty_u16(1000)

def sensor_init():
    trig.value(0)
    time.sleep_us(2)
    trig.value(1)
    time.sleep_us(7)
    trig.value(0)

def cal_dis():
    while echo.value() == 0:
        time_off = time.ticks_us()
    while echo.value() == 1:
        time_on = time.ticks_us()

    time_diff = time_on - time_off
    dist = (time_diff*0.034)/2
    if round(dist) <= 100:
        return True
    return False
       

while True:
    sensor_init()
    if cal_dis():
        for i in range(1000,9000,5):
            servo.duty_u16(i)
        time.sleep(2)
        for j in range(9000,1000,-5):
            servo.duty_u16(j)

    time.sleep(2.5)