from machine import Pin, I2C
from ssd1306 import SSD1306_I2C
import machine
import time

trigger_pin = Pin(16, Pin.OUT)
echo_pin = Pin(17, Pin.IN)

i2c = I2C(0, scl=Pin(22), sda=Pin(21), freq=400000)

oled_width = 128
oled_height = 64
oled = SSD1306_I2C(oled_width, oled_height, i2c)

while True:
    trigger_pin.value(0)
    time.sleep_us(2)
    trigger_pin.value(1)
    time.sleep_us(10)
    trigger_pin.value(0)

    total_time = machine.time_pulse_us(echo_pin, 1)
    distance = (total_time * 0.034) / 2

    print("Distance:", distance, "cm")

    oled.fill(0)
    oled.text("Distance:", 0, 0)
    oled.text("{:.2f} cm".format(distance), 0, 16)
    oled.show()

    time.sleep(2)