from machine import Pin, I2C, time_pulse_us
from time import sleep
from pico_i2c_lcd import I2cLcd

# Setup LCD
i2c = I2C(0, sda=Pin(4), scl=Pin(5), freq=400000)  # Adjust pins if needed
I2C_ADDR = i2c.scan()[0]
lcd = I2cLcd(i2c, I2C_ADDR, 2, 16)  # Assuming a 2x16 LCD

# Setup Ultrasonic Sensor
trigger = Pin(3, Pin.OUT)
echo = Pin(2, Pin.IN)

def get_distance():
    trigger.low()
    sleep(0.002)
    trigger.high()
    sleep(0.00001)
    trigger.low()

    duration = time_pulse_us(echo, 1, 30000)
    distance_cm = (duration * 0.0343) / 2
    return distance_cm

# Main loop
while True:
    distance = get_distance()
    lcd.clear()
    lcd.move_to(0, 0)
    lcd.putstr("{:.4f} cm".format(distance))
    sleep(1)