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

# Initialize I2C interface
i2c = I2C(0, scl=Pin(22), sda=Pin(21))

# Initialize OLED display
oled_width = 128
oled_height = 32
oled = SSD1306_I2C(oled_width, oled_height, i2c)

# Define HC-SR04 pins
trigger_pin = Pin(4, Pin.OUT)
echo_pin = Pin(5, Pin.IN)

# Define speed of sound (in meters per second)
speed_of_sound = 343.2

# Function to measure distance using HC-SR04
def measure_distance():
    # Send a 10us pulse to the trigger pin
    trigger_pin.value(1)
    time.sleep_us(10)
    trigger_pin.value(0)

    # Measure the time it takes for the echo pin to go high and then low
    while echo_pin.value() == 0:
        pulse_start = time.ticks_us()
    while echo_pin.value() == 1:
        pulse_end = time.ticks_us()

    # Calculate the pulse duration and distance
    pulse_duration = pulse_end - pulse_start
    distance = pulse_duration / 2 / 1000000 * speed_of_sound

    return distance

# Main loop
while True:
    # Measure distance and display on OLED
    distance = measure_distance()
    oled.fill(0)
    oled.text("Distance:", 0, 0)
    oled.text(str(round(distance, 2)) + " m", 0, 10)
    oled.show()
    
    # Wait for 1 second before measuring again
    time.sleep(1)