import RPi.GPIO as GPIO
from motorm import turn
import time
from time import sleep
GPIO.setmode(GPIO.BCM)

#-----------------ultrasound detector----
trig_pin = 18
echo_pin = 23
trig_pin2 = 14
echo_pin2 = 15
GPIO.setup(trig_pin, GPIO.OUT)
GPIO.setup(echo_pin, GPIO.IN)
GPIO.setup(trig_pin2, GPIO.OUT)
GPIO.setup(echo_pin2, GPIO.IN)

#----------------IR pins----------
pin1= 20
pin2= 21
GPIO.setup(pin1,GPIO.IN,pull_up_down=GPIO.PUD_UP)
GPIO.setup(pin2,GPIO.IN,pull_up_down=GPIO.PUD_UP)

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
    return distance

def distance2():
    GPIO.output(trig_pin2, True)
    sleep(0.00001)
    GPIO.output(trig_pin2, False)
    pulse_start = time.time()
    pulse_end = time.time()
    while GPIO.input(echo_pin2) == 0:
        pulse_start = time.time()
    while GPIO.input(echo_pin2) == 1:
        pulse_end = time.time()
    pulse_duration = pulse_end - pulse_start
    sound_v = 34300 #cm/s
    distance2 = pulse_duration*sound_v/2
    return distance2

def ultsound():
    D=20
    dis1 = distance1(); dis1=round(dis1,1)
    dis2 = distance2(); dis2=round(dis2,1)
    if(dis1>D and dis2 >D): DR=1
    if(dis1>D and dis2<=D): DR=7 
    if(dis1<D and dis2>=D): DR=3
    if(dis1<D and dis2 <D): DR=0
    return DR,dis1,dis2

t=0; tm0=time.time()
while True:
    tm=round(time.time()-tm0,3)
    hr=round(tm/3600,3)
    dis1=round(distance1(),1)
    dis2=round(distance2(),1)
    VP1=GPIO.input(pin1)
    VP2=GPIO.input(pin2)
    if(VP1==0 and VP2==0): DR=1
    if(VP1==0 and VP2==1): DR=7
    if(VP1==1 and VP2==0): DR=3
    if(VP1==1 and VP2==1): DR=9
    print(t,dis1,dis2,VP1,VP2,DR)
    sleep(1)
    t+=1