from  motorm import turn
import numpy as np
import cv2
#from targx import targx
from targs import targs
from targfunc import targx
#from targfunc import targs
from time import sleep
low1=np.array([170,150,50])
upp1=np.array([180,255,255])
low2=np.array([0  ,150,50])
upp2=np.array([10 ,255,255])
MSK=2
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH,640)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT,480)
sleep(2)
for j in range(20):
    ret,img=cap.read()
    TDR,box,h2=targx(MSK,low1,upp1,low2,upp2,img)
    #box,h2=targs(MSK,low1,upp1,low2,upp2,img)
    print(j,box)
    h2=cv2.resize(h2,(320*3,240))
    cv2.imshow('h2',h2)
    cv2.waitKey(0)
    if(TDR==1): turn(0.4,0.4,0.05); turn(0,0,0.05)
    if(TDR==3): turn(-0.4,0.4,0.05); turn(0,0,0.05)
    if(TDR==7): turn(0.4,-0.4,0.05); turn(0,0,0.05)
    if(TDR==0): turn(0.0,0.0,0.05); turn(0,0,0.05)