import cv2
import numpy as np
import time
from time import sleep
from motorm import turn
from LOCATE_green import locate_green

def moving(DR):
    if(DR==0 or DR==9): turn(0.0,0.0,0.05); turn(0,0,0.0)
    if(DR==1): turn(0.30,0.30,0.05); turn(0,0,0.0)
    if(DR==3): turn(-0.3,0.3,0.10); turn(0,0,0.1)
    if(DR==7): turn(0.3,-0.3,0.10); turn(0,0,0.1)

#----------------LOOP-1-------------
F=8; LX=640; LY=480; LX2=int(LX/F); LY2=int(LY/F)
cap=cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, LX)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, LY)
ret,frame=cap.read()
cv2.imshow('img24',frame)
if cv2.waitKey(2000) & 0xFF == ord('q'): pass
jt=0; NSTEP=200
while(jt<NSTEP):
    jt+=1
    ret,frame=cap.read()
    if(ret==False): break
    img2=np.copy(frame)
    NN,xx,xx8,yy8,yr8,xs,ys,asps,area,img24=locate_green(img2)
    DR=0
    if(0<=xx8<=30): DR=3
    if(30<xx8<50): DR=1
    if(50<=xx8<80): DR=7
    print(jt,NN,xx8,yy8,yr8,DR) #,xs[xx],ys[xx],WDS[xx],' area=',area[xx],asps[xx])
    cv2.imshow('img24',img24)
    #cv2.waitKey(0)
    if cv2.waitKey(100) & 0xFF == ord('q'): break
    if(yr8>45): DR=0
    moving(DR)
    #sleep(0.05)
input('iiiii-111111')
cap.release()
cv2.destroyAllWindows()