import numpy as np
import cv2
from  motorm import turn
from time import sleep
from pynput import keyboard

Lx=640; Ly=480; F=4; LX2=int(Lx/F); LY2=int(Ly/F)
#建立兩個紅色遮罩的面具所需要的顏色範圍
low1=np.array([175,80 ,80 ])
upp1=np.array([180,255,255])
low2=np.array([0,  80, 80 ])
upp2=np.array([10, 255,255])
print('low1=',low1,upp1,' low2=',low2,upp2)
FW2=open('KBS.txt','w')

cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH,640)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT,480)
fourcc = cv2.VideoWriter_fourcc(*'XVID')
out = cv2.VideoWriter('redM.avi', fourcc, 20.0, (640, 480))
sleep(2)

#---------------------從hsv的編碼決定底線的紅色的像素
def RED(img):
    #dri=input('dri=')
    img1=np.copy(img)
    imgA=cv2.resize(img1,(320,240))
    img2=cv2.resize(img1,(80,60))
    hsv=cv2.cvtColor(img2,cv2.COLOR_BGR2HSV)
    mask1=cv2.inRange(hsv,low1,upp1)
    mask2=cv2.inRange(hsv,low2,upp2)
    mask3=mask1 | mask2
    #mlow=int(60/2); mask3[0:mlow,:]=0; 
    red=[]; red2=[]; red3=[]; red4=[]; Y=60-1; blk=[]
    Y1=59; Y2=49; Y3=39; Y4=29
    for x in range(65): 
        if(mask3[Y1,x]>100): red.append(x)
        if(mask3[Y2,x]>100): red2.append(x)
        if(mask3[Y3,x]>100): red3.append(x)
        if(mask3[Y4,x]>100): red4.append(x)
        if(15<x<65 and hsv[Y2,x,2]<50): blk.append(x)
        #print(x,img2[Y4,x,:],hsv[Y4,x,:],len(red4))
    #print('blk=',blk)
    '''
    if(len(blk)>25): 
        print(len(blk),'blk=',blk,'hsv=',hsv[Y2,:,2])
        cv2.line(imgA,(0,Y2*4),(319,Y2*4),(0,255,255),2)
        cv2.imshow('blk',imgA)
        cv2.waitKey(0)
        return -100,0,0,0,0,0,0,0,imgA
    '''
    lred1=len(red); lred2=len(red2); lred3=len(red3); lred4=len(red4)
    if(lred1>0): mid1=int(np.median(red))
    else: mid1=-1; 
    if(lred2>0): mid2=int(np.median(red2))
    else: mid2=-1
    if(lred3>0): mid3=int(np.median(red3))
    else: mid3=-1
    if(lred4>0): mid4=int(np.median(red4))
    else: mid4=-1
    #--------------------drdrdr------
    dr=-10
    #dr=1
    if(mid1>3 and -1<mid3<8 and mid3<mid4<mid3+5): dr=30
    if(mid1==-1 and -1<mid3<15 and mid4<=mid3): dr=31
    if(mid1==-1 and mid3==-1 and 0<mid4<10): dr=32
    if(mid3==-1 and mid4==-1): dr=33
    #...........................................
    if(mid1==-1 and mid3-mid2>20): dr=71
    if(mid1>1 and mid2>mid1+12 or mid3>mid1+22): dr=72
    if(mid1<10 and mid2>0 and (mid4-mid3)>(mid3-mid2)+5 ): dr=73
    if(mid1<5 and mid2<5 and mid3>-1 and mid4>mid3+15 ): dr=73
    if(mid1<5 and mid2<5 and mid3<5 and 30<mid4<60 ): dr=73
    if(mid1>30 and mid2>mid1+4): dr=74
    if(-1<mid1<10 and -1<mid2<10): dr=70
    if(mid1>8): dr=71
    '''
    if(mid1 == -1):
        if(mid2 == -1):
            if(-1<mid3<20 and mid3+6<mid4<25): dr=1
            if(-1<mid3<8 and mid3<mid4<mid3+5): dr=30
            if(-1<mid3<15 and mid4<=mid3): dr=31
            if(mid3==-1 and 0<mid4<10): dr=32
            if(mid3==-1 and 0<mid4>=10): dr=1
            if(mid3==-1 and mid4==-1): dr=33
        if(-1<mid2<10):
            if(-1<mid3<8 and mid3<mid4<mid3+5): dr=30
            if(-1<mid3<15 and mid4<=mid3): dr=31
            if(mid3==-1 and 0<mid4<10): dr=32
            if(mid3==-1 and 0<mid4>=10): dr=1
            if(mid3==-1 and mid4==-1): dr=33
    '''
    dri=dr
    a=[dri,mid1,mid2,mid3,mid4]
    sa=str(a)

    '''
    if(mid1==-1 and mid3-mid2>20): dr=71
    if(mid1>1 and mid2>mid1+12 or mid3>mid1+22): dr=72
    if(mid1>12 and mid2>mid1+4 and mid3>10): dr=73
    if(mid1>30 and mid2>mid1+4): dr=74
    if(dr==1 and mid1<6 and mid2+mid3<15 ): dr=81
    if(mid1+mid2<4 and mid3<12 ): dr=82
    if(mid1==-1 and mid2==-1 and (mid3 != -1 or mid4 !=-1)): dr=90

    if(9<mid2-mid1<13 or 9<mid3-mid2<13): dr=1
    if(mid1==-1): 
        if(11<mid3-mid2<16): dr=1
        if(mid2<30 and mid3-mid2>10): dr=7
    if(mid1 != -1): 
        if(mid2-mid1>10 and mid3-mid2>10): dr=7
    if(mid1==-1 and mid2==-1 and 10<mid3<20): dr=1 
    if(mid1==-1 and mid2==-1 and mid3<=10): dr=3 
    if(mid1 != -1 and mid2 != -1 and mid3 != -1):
        if(9<mid2-mid1<13 or 9<mid3-mid2<13): dr=1
    if(0<mid1<15 and 0<mid2<30):
        if(mid2-mid1>7): dr=1
        elif(0<mid2-mid1<=7): dr=3
    if(mid1>15 and mid2>30): dr=7
    if(mid1<15 and abs(mid2-mid1)<5 and abs(mid3-mid1)<5): dr=3
    '''

    #print('mid1,2=',mid1,mid2,lred1,lred2)
    if(dr==-10): 
        #print('reset dr=-10',sa); 
        FW2.write(sa+'\n'); dr=1
    mask3=cv2.resize(mask3,(320,240))
    redmask=cv2.bitwise_and(imgA,imgA,mask=mask3)
    cv2.circle(imgA,(mid1*4,Y1*4),8,(0,255,0),-1)
    cv2.circle(imgA,(mid2*4,Y2*4),8,(0,255,0),-1)
    cv2.circle(imgA,(mid3*4,Y3*4),8,(0,255,0),-1)
    cv2.circle(imgA,(mid4*4,Y4*4),8,(0,255,0),-1)
    cv2.line(imgA,(0,Y4*4),(79*4,Y4*4),(0,255,0),2)
    cv2.circle(redmask,(mid1*4,Y1*4),8,(0,255,0),-1)
    cv2.circle(redmask,(mid2*4,Y2*4),8,(0,255,0),-1)
    cv2.circle(redmask,(mid3*4,Y3*4),8,(0,255,0),-1)
    cv2.circle(redmask,(mid4*4,Y4*4),8,(0,255,0),-1)
    cv2.putText(imgA,sa,(1,20),cv2.FONT_HERSHEY_SIMPLEX,
         0.5, (0,255,255), 1, cv2.LINE_AA)
    cv2.putText(redmask,sa,(1,20),cv2.FONT_HERSHEY_SIMPLEX,
         0.5, (0,255,255), 1, cv2.LINE_AA)
    #cv2.imshow('REDimgA',imgA)
    #cv2.imshow('REDmask',redmask)
    #cv2.waitKey(0)
    return dr,mid1,mid2,mid3,mid4,lred1,lred2,lred3,redmask



KB=open('KB.txt','w')
break_program = True
sp='A'
def on_press(key):
    global break_program
    global sp
    sp=str(key).strip("'")
    #KS=str(t)+'  '+str(key).strip("'")
    #print ('t=',t,'  key=',key,KS,'  sk=',str(key),break_program)
    #KB.write(KS+'\n')
    if sp == 'w' and break_program:
        turn(0.3,0.3,0.1); turn(0,0,0.1)
    if sp == 's' and break_program:
        turn(-0.3,-0.3,0.1); turn(0,0,0.1)
    if sp == 'a' and break_program:
        turn(0.0,0.3,0.1); turn(0,0,0.1)
    if sp == 'd' and break_program:
        turn(0.3,0.0,0.1); turn(0,0,0.1)
    if key == keyboard.Key.f1 and break_program:
        print ('end pressed')
        break_program = False
    #print(type(sp),'sp=',sp)

listener =  keyboard.Listener(on_press=on_press)
listener.start()


#=====================KKKKKKKKKKKKKKKKKK
if(1==2):
    print('into KY_RED...')
    t=0
    while(t < 50 and cap.isOpened()):
        ret,img=cap.read(); 
        if ret == False: break
        dr,mid1,mid2,mid3,mid4,lred1,lred2,lred3,img1=RED(img)
        ta=[mid1,mid2,mid3,lred1,lred2,lred3]
        ws=str(t)+' '+sp+' '+str(ta)
        if(t>0): print(ws)
        KB.write(ws+'\n')
        out.write(img1)
        cv2.imshow('img',img)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        cv2.waitKey(0)
        t+=1
    cap.release()
    out.release()
    cv2.destroyAllWindows()
    KB.close()
    input('sssss')








#---------使用兩個紅色遮罩尋找紅色軌跡，從而決定行走的方向DRS
#------------------- keep red inRange --------------
def MASK3(img):
    Y=LY2-1; 
    mlow=int(120/2)
    img1=np.copy(img)
    img1=cv2.resize(img1,(160,120))
    hsv=cv2.cvtColor(img1,cv2.COLOR_BGR2HSV)
    mask1=cv2.inRange(hsv,low1,upp1)
    mask3=cv2.inRange(hsv,low2,upp2)
    mask2=mask1 | mask3
    mask2[0:mlow,:]=0
    img2=cv2.bitwise_and(img1, img1, mask=mask2)
    thresh=np.copy(mask2)
    contours,hierarchy=cv2.findContours(thresh, cv2.RETR_EXTERNAL,
                cv2.CHAIN_APPROX_NONE)
    img3=np.copy(img2)
    box=(0,0,0,0)
    if len(contours) != 0:
        c=max(contours, key = cv2.contourArea)
        if(cv2.contourArea(c)>40):
            cv2.drawContours(img2,c,-1,(0,255,0),2)
            x,y,w,h = cv2.boundingRect(c)
            img3=cv2.rectangle(img3,(x,y),(x+w,y+h),(255,0,0),2)
            box=(x,y,w,h)
            MIDL=x+int(w/2); Y=y+h
            if(0<MIDL<60): DRS=3
            elif(60<=MIDL<=100): DRS=1
            elif(100<MIDL): DRS=7
            else: DRS=-2
        else: DRS=-2; #input('aaaa')
    else: DRS=-2
    cv2.imshow('img3',img3)
    return DRS,mask2,img3,box

#-------------------車輛迴轉，當紅色最大輪廓的中心點在中間時，結束迴轉的迴圈
def QUT(cap):
    turn(-0.2,0.7,0.5); turn(0.3,0.3,1.6); turn(0,0,0.01);
    for j in range(100):
        ret,img=cap.read()
        #DRU,mask2,img3,box=MASK3(img)
        DRU,mid1,mid2,mid3,mid4,lred1,lred2,lred3,img1=RED(img)
        if(int(DRU/10)==7): DRU=7
        print('QUT,j=',j,DRU,mid1,mid2,mid3)
        img3s=cv2.resize(img1,(320,240))
        cv2.imshow('UT',img3s)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        cv2.waitKey(0)
        if(DRU==1 or DRU==7): return
        turn(-0.3,0.5,0.08); turn(0,0,0.05);
    print('Unfortunate QUT()...')
    return


def QLT31(cap):
    #turn(-0.2,0.7,0.5); turn(0.3,0.3,1.0); turn(0,0,0.01);
    turn(0.4,0.0,0.30); turn(-0.4,0.4,0.10); turn(0,0,0.05);
    for j in range(100):
        ret,img=cap.read()
        DRU,mid1,mid2,mid3,mid4,lred1,lred2,lred3,img1=RED(img)
        if(int(DRU/10)==7): DRU=7
        print('QLT31,j=',j,DRU,mid1,mid2,mid3,mid4)
        img3s=cv2.resize(img1,(320,240))
        cv2.imshow('LT',img3s)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        #cv2.waitKey(0)
        if(DRU==1 or DRU==7): 
            turn(0.5,0.0,0.05); turn(0,0,0.10);
            return
        turn(-0.4,0.4,0.05); turn(0,0,0.10);
    print('Unfortunate QLT()...')
    return




def QLT33(cap):
    #turn(-0.2,0.7,0.5); turn(0.3,0.3,1.0); turn(0,0,0.01);
    turn(0.4,0.0,0.20); turn(-0.4,0.4,0.10); turn(0,0,0.05);
    for j in range(100):
        ret,img=cap.read()
        DRU,mid1,mid2,mid3,mid4,lred1,lred2,lred3,img1=RED(img)
        if(int(DRU/10)==7): DRU=7
        print('QLT33,j=',j,DRU,mid1,mid2,mid3,mid4)
        img3s=cv2.resize(img1,(320,240))
        cv2.imshow('LT',img3s)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        #cv2.waitKey(0)
        if(DRU==1 or DRU==7): 
            turn(0.5,0.0,0.05); turn(0,0,0.10);
            return
        turn(-0.4,0.4,0.05); turn(0,0,0.10);
    print('Unfortunate QLT()...')
    return




def QLT(cap):
    #turn(-0.2,0.7,0.5); turn(0.3,0.3,1.0); turn(0,0,0.01);
    for j in range(100):
        ret,img=cap.read()
        DRU,mid1,mid2,mid3,mid4,lred1,lred2,lred3,img1=RED(img)
        if(int(DRU/10)==7): DRU=7
        print('QLT,j=',j,DRU,mid1,mid2,mid3,mid4)
        img3s=cv2.resize(img1,(320,240))
        cv2.imshow('LT',img3s)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        #cv2.waitKey(0)
        if(DRU !=3 or DRU==90): 
            #turn(0.5,0.0,0.10); turn(0,0,0.10);
            return
        turn(-0.4,0.4,0.05); turn(0,0,0.10);
    print('Unfortunate QLT()...')
    return






# method=1 僅使用底邊的hsv紅色決定行駛方向dr
# method=2 同時使用紅色底邊dr和最大輪廓DRS兩種行駛方向，共同決定最優的行駛方向, DR
method=2
Ymin=120-50
ret,img=cap.read()
#cv2.imshow('img_in',img)
if(1==1):
    ret,img=cap.read()
    DRj,mid1,mid2,mid3,mid4,lred1,lred2,lred3,im0=RED(img)
    DRj,mid1,mid2,mid3,mid4,lred1,lred2,lred3,im1=RED(img)
    DRj,mid1,mid2,mid3,mid4,lred1,lred2,lred3,im2=RED(img)
    DRj,mid1,mid2,mid3,mid4,lred1,lred2,lred3,im3=RED(img)
    DRj,mid1,mid2,mid3,mid4,lred1,lred2,lred3,im4=RED(img)
    DRj,mid1,mid2,mid3,mid4,lred1,lred2,lred3,im5=RED(img)
    imgM=np.hstack((im0,im1,im2,im3,im4,im5))
    cv2.imshow('imgM',imgM)
    FW1=open('redM.txt','w')
    #-------------------RRRRRRR------
    for j in range(90):
        ret,img=cap.read()
        cv2.imshow('imgin',img)
        j6=j%6
        DRj,mid1,mid2,mid3,mid4,lred1,lred2,lred3,img1=RED(img)
        if(j6==0): im0=np.copy(img1)
        if(j6==1): im1=np.copy(img1)
        if(j6==2): im2=np.copy(img1)
        if(j6==3): im3=np.copy(img1)
        if(j6==4): im4=np.copy(img1)
        if(j6==5): im5=np.copy(img1)
        imgM=np.hstack((im0,im1,im2,im3,im4,im5))
        #cv2.imshow('imgM_'+str(j6),img1)
        cv2.imshow('imgM',imgM)
        cv2.imwrite('DIR_'+str(j)+'.png',img1)
        a=[j,DRj,mid1,mid2,mid3,lred1,lred2,lred3]; sa=str(a)
        DR=DRj
        #if(int(DR/10)==7): DR=7
        if(int(DR/10)==8): DR=8
        print('j=',j,DRj,DR,mid1,mid2,mid3,mid4)
        FW1.write(sa+'\n')
        img2=cv2.resize(img1,(640,480))
        out.write(img2)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        #if(DR==31): cv2.waitKey(0)
        #......................CCCCCC
        #cv2.waitKey(0)
        #if(j==0): QUT(cap)
        #DR=0
        if(DR==-100): QUT(cap)
        if(DR==-10): turn(0.0,0.0,0.10); turn(0,0,0.05);
        if(DR==0): turn(0.0,0.0,0.10); turn(0,0,0.05);
        if(DR==1): turn(0.3,0.3,0.10); turn(0,0,0.00);
        if(DR==30): turn(0.0,0.4,0.10); turn(0,0,0.05);
        if(DR==31): sleep(0); turn(0.5,0.2,0.40); turn(0.2,0.5,0.45);
        #if(DR==31): turn(0.5,0.2,0.40); turn(0.4,0.4,0.1); turn(0.2,0.5,0.45);
        #if(DR==31): QLT31(cap)
        if(DR==32): turn(0.0,0.4,0.10); turn(0,0,0.05);
        if(DR==33): QLT33(cap)
        if(DR==3): 
            turn(0.0,0.4,0.10); turn(0,0,0.05);
            #QLT33(cap)
            #turn(0.4,0.4,0.30); turn(-0.4,0.4,0.15); turn(0,0,0.05);
        if(DR==70): turn(0.4,0.0,0.10); turn(0,0,0.10);
        if(DR==71): turn(0.4,0.0,0.10); turn(0,0,0.10);
        if(DR==72): turn(0.4,-0.2,0.15); turn(0,0,0.10);
        if(DR==73): turn(0.6,-0.2,0.15); turn(0,0,0.10);
        #if(DR==8):turn(0.4,-0.4,0.15);turn(0.4,0.4,0.30);turn(-0.4,0.4,0.15)
        #if(DR==90):turn(0.4,-0.4,0.10);turn(0.4,0.4,0.20);turn(-0.4,0.4,0.10)
        #if(DR==-2): turn(-0.3,-0.3,0.05); turn(0,0,0.05);
    FW1.close()
    FW2.close()
    cv2.destroyAllWindows()
    cap.release()
    input('2222')


















DRS,mask2,img3,box=MASK3(img)
cv2.waitKey(0)

FW=open('GL.txt','w')
for j in range(440):
    fl='GL_'+str(j)+'.png'
    ret,img=cap.read()
    #img=cv2.imread('G1.png')
    imgA=np.copy(img)
    cv2.imshow('imgA1',imgA)
    #imgA=cv2.resize(imgA,(160,120))
    #--------------------------------
    DRS,mask2,img3,box=MASK3(img)
    #--------------------------------
    x,y,w,h=box
    MIDL=x+int(w/2); Y=y+h
    imgA=cv2.circle(imgA,(MIDL,Y),5,(255,0,0),-1)
    #--------------------------------
    dr,lred,midl,sh,img1=RED_BOTH(img)
    #--------------------------------
    #print('dr,lred,midl,sh=',dr,lred,midl,sh)
    if(dr==100): print('QUT dr=',dr); sleep(0.5); QUT(cap); continue
    res2=cv2.bitwise_and(imgA,imgA,mask=mask2)
    img1=cv2.resize(img1,(160,120))
    img3=cv2.resize(img3,(160,120))
    hres=np.hstack((imgA,img1,img3))
    hstk1=cv2.resize(mask2,(320,240))
    hstk2=cv2.resize(hres,(960,240))
    #cv2.imshow('hstk1',hstk1)
    cv2.imshow('hstk2',hstk2)
    #cv2.imshow('img3',img3)
    if cv2.waitKey(1) & 0xFF == ord('q'): break
    cv2.imwrite(fl,hstk2)
    if(method==2):
        DR=DRS
        KK=0
        #只要dr > 1 表示底邊可以看見紅色，就接受底邊的判斷
        if(dr>0): KK=1; DR=dr 
        #else: print(j,'fail dr=',dr)
        #雖然底邊可以看見紅色區塊，但是輪廓顯示矩形的寬度非常寬，表示
        #底線前方的軌跡發生大角度的轉彎，因此要做特殊的處理
        #正常情況軌道的寬度只有20px，如果大於40就是大轉彎
        if(dr>0 and w>20*2 and h<40): 
            KK=0
            if(MIDL<midl-5): DR=13 #大角度的左轉定義為DR=13
            if(MIDL>midl+5): DR=17 #大角度的右轉定義為DR=17
        Sbox=np.sum(box) #Sbox=0 表示整個圖像中沒有足夠大的紅色區塊應該要後退,DR=-2
        if(Sbox==0):DR=-2;print('vacant box=',box,Sbox);#sleep(20);break 
        #if(Y<Ymin): DR=-2;print('ESCAPE from Y,Ymin=',Y,Ymin,' DR=',DR); 
    #
    if(method==1):  #為了驗證新的方法有比較高的優勢因此我們保留第一種方法的執行作為比較
        DR=dr
    a=str(([j,DR,dr,DRS],[lred,w,h],[midl,MIDL,Y],sh))
    print(a)
    FW.write(a+'\n')
    #cv2.waitKey(0)
    #DR=0
    if(DR==1): turn(0.3,0.3,0.10); turn(0,0,0.05);
    if(DR==3): turn(0.0,0.4,0.10); turn(0,0,0.05);
    if(DR==7): turn(0.4,0.0,0.10); turn(0,0,0.05);
    #DR=13 or DR=17 大角度的轉彎
    if(DR>10): print('*******大角度轉彎steep turn, DR=',DR)
    if(dr==-2 and DR!=-2):print('advantage:DR,dr,DRS=',DR,dr,DRS); sleep(1)
    if(DR==13): 
        ret,imgt=cap.read()
        img1ts=cv2.resize(imgt,(320,240))
        cv2.imshow('img1ts',img1ts)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        #cv2.waitKey(0)
        turn(0.4,0.4,0.30); turn(0,0,0.05);
        for t in range(20):
            ret,imgt=cap.read()
            drt,lredt,midlt,sht,img1t=RED_BOTH(imgt)
            DRSt,maskt,img3t,boxt=MASK3(imgt)
            img1ts=cv2.resize(img1t,(320,240))
            cv2.imshow('img1ts',img1ts)
            if cv2.waitKey(1) & 0xFF == ord('q'): break
            print(t,drt,lredt,midlt,sht,boxt)
            #if(drt==1 and boxt[2]<80): 
            if(drt==1): 
                cv2.destroyAllWindows()
                break
            turn(-0.4,0.4,0.05); turn(0,0,0.15)
        cv2.destroyAllWindows()

    if(DR==0): turn(0.0,0.0,0.10); turn(0,0,0.05);
    if(DR==17): 
        ret,imgt=cap.read()
        img1ts=cv2.resize(imgt,(320,240))
        cv2.imshow('img1ts',img1ts)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        #cv2.waitKey(0)
        turn(0.4,0.4,0.30); turn(0,0,0.05);
        for t in range(20):
            ret,imgt=cap.read()
            drt,lredt,midlt,sht,img1t=RED_BOTH(imgt)
            DRSt,maskt,img3t,boxt=MASK3(imgt)
            img1ts=cv2.resize(img1t,(320,240))
            cv2.imshow('img1ts',img1ts)
            if cv2.waitKey(1) & 0xFF == ord('q'): break
            print(t,drt,lredt,midlt,sht,boxt)
            #if(drt==1 and boxt[2]<80): 
            if(drt==1): 
                cv2.destroyAllWindows()
                break
            turn(0.4,-0.4,0.05); turn(0,0,0.15)
        cv2.destroyAllWindows()

    if(DR==0): turn(0.0,0.0,0.10); turn(0,0,0.05);
    if(DR==-2): turn(-0.3,-0.3,0.05); turn(0,0,0.05);

FW.close()