from machine import PWM, Pin, I2C
from hcsr04 import HCSR04
import ssd1306
import time

i2c = I2C(0, scl=Pin(22), sda=Pin(21))
oled_width = 128
oled_height = 64
oled = ssd1306.SSD1306_I2C(oled_width, oled_height, i2c)

sensor = HCSR04(trigger_pin=5, echo_pin=4, echo_timeout_us=10000)
pwm = PWM(Pin(15))
pwm.freq(1000)

def setServoCycle (position):
    pwm.duty_u16(position)
    time.sleep(0.01)

while True:
    print('jarak', sensor.distance_cm(), 'cm')
    time.sleep(2)
    if sensor.distance_cm()>=30:
        setServoCycle(1000)
        oled.text('Awas ada Halangan', 10, 10)
        oled.show()

    else:
        setServoCycle(6000)
        oled.text('Jalan Aman', 10, 10)
        oled.show()