from machine import Pin, time_pulse_us, I2C
from time import sleep
import ssd1306


trig = Pin(22, Pin.OUT)
echo = Pin(19, Pin.IN)


def measure_distance():
    trig.value(0)
    sleep(0.002)
    trig.value(1)
    sleep(0.00001)
    trig.value(0)
    
    duration = time_pulse_us(echo, 1, 30000)  
    distance = (duration * 0.0343) / 2
    return distance


i2c = I2C(0, scl=Pin(1), sda=Pin(0))
oled = ssd1306.SSD1306_I2C(128, 64, i2c)


while True:
    distance_cm = measure_distance()
    print("Distance:", distance_cm, "cm")
    oled.fill(0)
    oled.text("Distance:", 0, 20)
    oled.text("{:.2f} cm".format(distance_cm), 0, 40)
    oled.show()
    sleep(1)