from machine import Pin, time_pulse_us, I2C
from time import sleep_us, sleep
import ssd1306

trig_pin = Pin(22, Pin.OUT)
echo_pin = Pin(21, Pin.IN)

MAX_DISTANCE=20
MAX_EXCURSION=40
BASELINE=15

c=0.034 #speed of sound in cm/us

i2c = I2C(0)
display = ssd1306.SSD1306_I2C(128, 64, i2c)

def init_display():
    display.fill(0)
    display.text('Distance Sensor', 3, 0, 1)
    display.show()

init_display()

x=0
y0=BASELINE
y=0
while True:

    trig_pin.value(0)
    sleep_us(2)
    
    

    # trigger the sensor
    trig_pin.value(1)
    sleep_us(10)
    
    

    # stop trigger
    trig_pin.value(0)
    
    

    #echo_pin becomes 1, and again 0 when receives an echo

    # measure the time delay
    
    delay_us = time_pulse_us(echo_pin, 1)
    
    
    
    # calculate the distance based on the duration
    distance_cm = round(c * delay_us / 2,1)

    if distance_cm>MAX_DISTANCE:
        distance_cm=MAX_DISTANCE

    y=BASELINE+round(distance_cm/MAX_DISTANCE*MAX_EXCURSION)
    #display.pixel(count, y, 1) 
    display.line(x, y0, x+1,y, 1)
    display.show()

    y0=y    
    x=x+1

    if x==128:
        x=0
        init_display()

    
    print("Distance:", distance_cm, "cm")
    
    sleep(0.001)