import RPi.GPIO as GPIO
import time
from time import sleep
import cv2
import numpy as np
from PARAM import *
from motorm import turn
from LOCATE1 import locate
from UturnBK import UturnBK

trig_pin = 20
echo_pin = 21
GPIO.setup(trig_pin, GPIO.OUT)
GPIO.setup(echo_pin, GPIO.IN)

def distance1():
    GPIO.output(trig_pin, True)
    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
    distance = pulse_duration*sound_v/2
    dis1=round(distance,1)
    return dis1