from PARAM import *
from  motorm import turn
import cv2
from RED1 import RED1
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, Lx)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, Ly)
t=0
while(t < 1000 and cap.isOpened()):
    ret,frame = cap.read(); t+=1
    if ret == True:
        DR,LRED,MIDL,img1,img2=RED1(frame)
        print(t,DR); cv2.imshow('img1',img1)
        if(DR==1): turn(0.4,0.4,0.05); turn(0,0,0.01);
        if(DR==-2): turn(-0.4,-0.4,0.05); turn(0,0,0.01);
        if(DR==3): turn(0.0,0.4,0.05); turn(0,0,0.01);
        if(DR==7): turn(0.4,0.0,0.05); turn(0,0,0.01);
        if cv2.waitKey(1) & 0xFF == ord('q'): break
    else: break
cap.release()
cv2.destroyAllWindows()