import machine
import time
import ssd1306

# Setup for the Ultrasonic Sensor
trigger_pin = machine.Pin(5, machine.Pin.OUT)  # Trigger pin
echo_pin = machine.Pin(18, machine.Pin.IN)    # Echo pin

# Setup for the Display (SSD1306 OLED)
i2c = machine.I2C(scl=machine.Pin(22), sda=machine.Pin(21))  # I2C pins
display = ssd1306.SSD1306_I2C(128, 64, i2c)

def get_distance():
    # Send a pulse to the trigger pin
    trigger_pin.value(0)
    time.sleep_us(2)
    trigger_pin.value(1)
    time.sleep_us(10)
    trigger_pin.value(0)

    # Measure the pulse width on the echo pin
    pulse_duration = machine.time_pulse_us(echo_pin, 1)
    
    # Calculate distance in cm (speed of sound = 34300 cm/s)
    distance = (pulse_duration * 0.0343) / 2
    return distance

while True:
    distance = get_distance()
    print("Distance: {:.2f} cm".format(distance))

    # Clear the display and show the distance
    display.fill(0)
    display.text('Distance: {:.2f} cm'.format(distance), 0, 0)
    display.show()

    # Wait for 1 second before measuring again
    time.sleep(1)