from machine import Pin,PWM,Timer,I2C
from time import sleep_ms
from HCSR04 import HCSR04
from ssd1306 import SSD1306_I2C

S1=PWM(Pin(18),freq=50,duty=128)

trig=Pin(14,Pin.OUT)
echo=Pin(12,Pin.IN)
hc=HCSR04(trig,echo)
i2c=I2C(0,sda=Pin(26),scl=Pin(27))
oled=SSD1306_I2C(128,64,i2c)

def rotate(servo,degree):
    servo.duty(int(degree*0.54+25.6))
t=45

def fun(tim):
    distance=hc.getDistance()
    oled.fill(0)
    oled.text(str('%.2f'%(distance))+' CM',0,30)
    oled.show()


    if distance>2 and distance<=180:
          rotate(S1,distance) 
    else:
        rotate(S1,180)

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