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

# Initialize the I2C bus
i2c = I2C(scl=Pin(5), sda=Pin(4))

# Initialize the OLED display
oled = SSD1306_I2C(128, 64, i2c)

# Initialize the ultrasonic sensor
trigger = Pin(16, Pin.OUT)
echo = Pin(17, Pin.IN)

# Function to measure distance
def measure_distance():
    trigger.value(0)
    time.sleep_us(2)
    trigger.value(1)
    time.sleep_us(10)
    trigger.value(0)
    while echo.value() == 0:
        start = time.ticks_us()
    while echo.value() == 1:
        stop = time.ticks_us()
    duration = stop - start
    distance = duration * 0.034 / 2
    return distance

# Main loop
while True:
    # Measure distance
    distance = measure_distance()
    
    # Display distance on LCD
    oled.fill(0)
    oled.text("Distance:", 0, 0)
    oled.text(str(distance) + " cm", 0, 16)
    oled.show()
    
    # Wait for 1 second
    time.sleep(1)