from machine import Pin,PWM,Timer,I2C
from time import sleep_ms
from HCSR04 import HCSR04
from ssd1306 import SSD1306_I2C
i2c=I2C(0,sda=Pin(4),scl=Pin(0),)
oled=SSD1306_I2C(128,64,i2c)
V1=PWM(Pin(2),freq=50,duty=128)
trig=Pin(12,Pin.OUT)
echo=Pin(14,Pin.IN)
HC=HCSR04(trig,echo)
def rotate(servo,degree):
    servo.duty(int(degree*0.54+25.6))
def fun(tim):
    distance=HC.getDistance()
    oled.fill(0)
    oled.text(str('%.2f'%(distance))+'cm',20,30)
    oled.show()
    
    if distance>2 and distance<=180:
        rotate(V1,distance)
    else:
        rotate(V1,180)

tim=Timer(1)
tim.init(period=500,mode=Timer.PERIODIC,callback=fun)