# LIUS.py

import RPi.GPIO as GPIO
import time
from time import sleep
import cv2
Lx=800; Ly=600;
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, Lx)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, Ly)
GPIO.setmode(GPIO.BCM)
#L--------------- LED pins LIUS=Led + Infared + Ultrasound + Servo
RED=16; BLUE=20; GREEN=21;
GPIO.setup(RED,GPIO.OUT); GPIO.output(RED,0);
GPIO.setup(GREEN,GPIO.OUT); GPIO.output(GREEN,0);
GPIO.setup(BLUE,GPIO.OUT); GPIO.output(BLUE,0)
#I----------------IR pins----------
IRpin1=13
IRpin2=6
GPIO.setup(IRpin1,GPIO.IN,pull_up_down=GPIO.PUD_UP)
GPIO.setup(IRpin2,GPIO.IN,pull_up_down=GPIO.PUD_UP)
#U---------------ultra-sound pins-- 
trig_pin=19; echo_pin=26;
GPIO.setup(trig_pin,GPIO.OUT)
GPIO.setup(echo_pin,GPIO.IN)
#S---------------SERVO pin---------
pin_servo=12
GPIO.setmode(GPIO.BCM)
GPIO.setup(pin_servo, GPIO.OUT)
pwm_servo = GPIO.PWM(pin_servo, 50)  # 50Hz frequency
pwm_servo.start(0)

############################################################
def LED(j):
    r=j%2; g=j//2%2; b=j//4%2;
    GPIO.output(RED,r);
    GPIO.output(GREEN,g);
    GPIO.output(BLUE,b);
    if(j==5): ts=4
    elif(j==6): ts=2
    else: ts=0.5
    return ts
############################################################
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
    dis1=round(pulse_duration*sound_v/2,2)
    return dis1
############################################################
def setDirection(angle):
    duty = 2 + (angle / 18)
    pwm_servo.ChangeDutyCycle(duty)
    sleep(0.3) # avoid vibration
    pwm_servo.ChangeDutyCycle(0)
############################################################

for j in range(1000):
    _,img=cap.read(); cv2.imshow('img',img);
    cv2.imshow('img',img);
    if cv2.waitKey(1) & 0xFF == ord('q'): break
    jc=j%8; ts=LED(jc); sleep(ts)
    VP1=GPIO.input(IRpin1)
    VP2=GPIO.input(IRpin2)
    angle=10*(j%19); setDirection(angle); 
    dis1=distance1(); 
    print('j=',j,'jc=',jc,'VP1,2=',VP1,VP2,'dis1=',dis1,'angle=',angle)
cap.release()
cv2.destroyAllWindows()