from machine import Pin, ADC
from machine import Pin, PWM
from time import sleep_ms
from time import sleep_us
import utime
from utime import sleep
# Sensor
pulse = Pin(26, Pin.OUT)
receiver = Pin(27, Pin.IN, Pin.PULL_DOWN)

# Buzzer

buzzer = PWM(Pin(15))

# Stepper Motor
step = Pin(19, Pin.OUT)
direction = Pin(18, Pin.OUT)
locked = False

def CheckDistance():
  SpeedOfSoundInMM = 0.343
  pulse.low()
  utime.sleep_us(20)
  pulse.high()
  utime.sleep_us(10)
  pulse.low()
  exitLoop = False
  loopcount = 0 
  while receiver.value() == 0 and exitLoop == False:
    loopcount = loopcount + 1
    delaytime = utime.ticks_us()
    
    if loopcount > 3000 : exitLoop == True
  
  while receiver.value() == 1 and exitLoop == False:
    loopcount = loopcount + 1
    receivetime = utime.ticks_us()
    if loopcount > 3000 : exitLoop == True
  
  if exitLoop == True: 
    return 0
  else:
    distance = ((receivetime - delaytime) * SpeedOfSoundInMM) / 2
    return distance

def stepper(steps, dir='clockwise'):
  global current_step
  if dir == 'clockwise':
    direction.value(0)
    for i in range(0, steps):
      step.value(1)
      sleep_us(1500)
      step.value(0)
      sleep_us(1500)
      current_step = i + 1
  if dir == 'counter clockwise':
    direction.value(1)
    for i in range(steps - 1, -1, -1):
      step.value(1)
      sleep_us(1500)
      step.value(0)
      sleep_us(1500)
      current_step = i + 1


backing_distance = 152.4

right_rotation = 50
backing_direction = 100
current_step = 0

def alarm():
    buzzer.duty_u16(1000)

def bequiet():
    buzzer.duty_u16(0)

while True:
  distance = CheckDistance()
  print('Distance: ', distance)
  if distance <= backing_distance:
    if current_step != backing_direction:
      stepper(backing_direction)
      print('An obstacle is detected. The robot is backing up.')
      alarm()
  if distance > backing_distance:
    if current_step == backing_direction:
      stepper(right_rotation, 'counter clockwise')
      print('Turning right.')
      bequiet()
    
  sleep_ms(1000)