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

# Define the GPIO pin numbers for the trigger and echo pins
ECHO_PIN = 26
TRIGGER_PIN = 27

pin7 = Pin(7, Pin.OUT)
pin8 = Pin(8, Pin.OUT)
pin9 = Pin(9, Pin.OUT)
pin10 = Pin(10, Pin.OUT)

# setup measurement sensor
trigger = Pin(26, Pin.OUT)
echo = Pin(27, Pin.IN)

def forwards(t):
  pin7.off()
  pin8.on()
  pin9.off()
  pin10.on()
  sleep(t)

def backwards(t):
  pin7.on()
  pin8.off()
  pin9.on()
  pin10.off()
  sleep(t)

def left(t):
  pin7.off()
  pin8.on()
  pin9.on()
  pin10.off()
  sleep(t)

def right(t):
  pin7.on()
  pin8.off()
  pin9.off()
  pin10.on()
  sleep(t)

def stop():
  pin7.off()
  pin8.off()
  pin9.off()
  pin10.off()

# Initialize trigger and echo pins
trigger = Pin(TRIGGER_PIN, Pin.OUT)
echo = Pin(ECHO_PIN, Pin.IN)

def measure_distance():
    # Ensure trigger is low initially
    trigger.low()
    sleep_us(2)

    # Send a 10 microsecond pulse to the trigger pin
    trigger.high()
    sleep_us(10)
    trigger.low()

    # Measure the duration of the echo pulse (in microseconds)
    pulse_duration = time_pulse_us(echo, Pin.high)

    # Calculate the distance (in centimeters) using the speed of sound (343 m/s)
    distance = pulse_duration * 0.0343 / 2
    return distance

while True:
    # Measure the distance and print the value in centimeters
    distance = measure_distance()
    print("Distance: {:.2f} cm".format(distance))
    
    # Wait for 1 second before taking the next measurement
    if distance >= 15:
        forwards(1)
    else:
        stop()