from machine import Pin
import utime


# GPIO pins
TRIGGER_PIN = 3
ECHO_PIN = 2
DIR_PIN = 9
STEP_PIN = 8
BUTTON_PIN = 15

# Speed of sound constant in cm/us
SPEED_OF_SOUND_CM_PER_US = 0.0343

# Threshold distance in cm
THRESHOLD_DISTANCE_CM = 50

# Initialize GPIO pins
trigger = Pin(TRIGGER_PIN, Pin.OUT)
echo = Pin(ECHO_PIN, Pin.IN)
dirp = Pin(DIR_PIN, Pin.OUT)
step = Pin(STEP_PIN, Pin.OUT)
button = Pin(BUTTON_PIN, Pin.IN, Pin.PULL_DOWN)

def setup():
  step.low()

def motor():
    dirp.high()
    for x in range(1,100):
      step.high()
      step.low()
      utime.sleep_us(5000)  
    utime.sleep_ms(500)  

    dirp.low()
    for x in range(1,100):
      step.high()
      step.low()
      utime.sleep_us(5000)
    utime.sleep_ms(1000)

def measure():
    trigger.low()
    utime.sleep_us(2)
    trigger.high()
    utime.sleep_us(5)
    trigger.low()

    while echo.value() == 0:
        signal_off = utime.ticks_us()
    while echo.value() == 1:
        signal_on = utime.ticks_us()
    
    time_passed = signal_on - signal_off
    distance = (time_passed * SPEED_OF_SOUND_CM_PER_US) / 2

    return distance

while True:
  if button.value() == 1:
    motor()
  
  distance = measure()
  if distance < THRESHOLD_DISTANCE_CM:
    motor()
    #utime.sleep_ms(60000)