import RPi.GPIO as GPIO
import time
import cv2
from motorm import turn

def distance1():
    GPIO.output(trig_pin,True); time.sleep(0.00001)
    GPIO.output(trig_pin, False)
    pulse_start=time.time(); pulse_end=time.time()
    while GPIO.input(echo_pin) == 0: pulse_start=time.time()
    while GPIO.input(echo_pin) == 1: pulse_end=time.time()
    pulse_duration=pulse_end - pulse_start
    sound_v=34300 #cm/s
    dis1=round(pulse_duration*sound_v/2,2)
    return dis1
#---------------ultra-sound pins-- 
GPIO.setmode(GPIO.BCM)
trig_pin=21; echo_pin=20;
GPIO.setup(trig_pin,GPIO.OUT)
GPIO.setup(echo_pin,GPIO.IN)
#---ctrl-c to terminate the loop-----
for i in range(5000):
    dis1=distance1(); print('i_ult=',i,dis1)
    time.sleep(0.5)
    if(i<10): continue
    if(dis1<30): continue
    else: turn(0.2,0.2,0.1); turn(0,0,0.1)