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) #.................2 masks for red color low1=np.array([170,60 ,90 ]) upp1=np.array([180,255,220]) low2=np.array([0, 60, 90 ]) 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) #---------------------find red pixels from hsv COLOR=0 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): #since we focus on the left-hand side of the view 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): if(len(red4)>0 and red4[-1]25): cv2.line(imgA,(0,Y3*4),(79*4,Y3*4),(255,255,0),2) cv2.imshow('blk_U_turn',imgA) return -100,0,0,0,0,imgA #return dr,mid1,mid2,mid3,mid4,redmask lred1=len(red); lred2=len(red2); lred3=len(red3); lred4=len(red4) if(lred1>1): mid1=int(np.median(red)) else: mid1=-1; if(lred2>1): mid2=int(np.median(red2)) else: mid2=-1 if(lred3>1): mid3=int(np.median(red3)) else: mid3=-1 if(lred4>1): mid4=int(np.median(red4)) else: mid4=-1 #--------------------drdrdr------ dr=-10 if(mid1>3 and -120): 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=72 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 3030 and mid2>mid1+4): dr=74 if(-18): dr=71 mask3=cv2.resize(mask3,(320,240)) redmask=cv2.bitwise_and(imgA,imgA,mask=mask3) dri=dr; a=[dri,mid1,mid2,mid3,mid4]; sa=str(a) 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.line(redmask,(0,Y4*4),(79*4,Y4*4),(0,255,0),1) 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) if(COLOR==1): a=[dr,mid1,mid2,mid3,mid4,lred1,lred2,lred3,lred4] print('a=',a) cv2.imshow('redmask',redmask) cv2.waitKey(0) input('1111') return dr,mid1,mid2,mid3,mid4,redmask KB=open('KB.txt','w') break_program = True sp='A' def on_press(key): global break_program global sp sp=str(key).strip("'") #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 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,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') def QLT33(cap): turn(0.7,0.2,0.2); turn(0.3,0.3,0.1); turn(-0.3,0.3,0.1); #turn(0.4,0.0,0.15); turn(-0.3,0.3,0.35); turn(0,0,0.05); for j in range(100): ret,img=cap.read() DRU,mid1,mid2,mid3,mid4,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): if(DRU==1 or DRU==-10 or DRU==7 or DRU==-100): turn(0.5,0.0,0.05); turn(0,0,0.10); print('QLT33,end_j=',j,DRU,mid1,mid2,mid3,mid4) #sleep(2) return turn(-0.4,0.4,0.05); turn(0,0,0.05); print('Unfortunate QLT33()...') return ret,img=cap.read() #cv2.imshow('img_in',img) if(1==1): ret,img=cap.read() DRj,mid1,mid2,mid3,mid4,m0=RED(img) m1=np.copy(m0);m2=np.copy(m0);m3=np.copy(m0);m4=np.copy(m0);m5=np.copy(m0); imgM=np.hstack((m0,m1,m2,m3,m4,m5)) cv2.imshow('imgM',imgM) FW1=open('redM.txt','w') #-------------------RRRRRRR------ for j in range(680): ret,img=cap.read() imgi2=cv2.resize(img,(320,240)) cv2.imshow('imgin',imgi2) j6=j%6 DRj,mid1,mid2,mid3,mid4,img1=RED(img) if(j6==0): m0=np.copy(img1) if(j6==1): m1=np.copy(img1) if(j6==2): m2=np.copy(img1) if(j6==3): m3=np.copy(img1) if(j6==4): m4=np.copy(img1) if(j6==5): m5=np.copy(img1) imgM=np.hstack((m0,m1,m2,m3,m4,m5)) #cv2.imshow('imgM_'+str(j6),img1) cv2.imshow('imgM',imgM) #cv2.imwrite('DIR_'+str(j).zfill(3)+'.png',img1) DR=DRj a=[j,DRj,DR,mid1,mid2,mid3,mid4]; sa=str(a) #if(int(DR/10)==7): DR=7 if(int(DR/10)==8): DR=8 if(DR==-10): DR=1 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 #......................CCCCCC #cv2.waitKey(0) #DR=0 if(DR==-100): turn(-0.4,0.4,0.50);turn(0.4,0.4,0.6);turn(-0.4,0.4,0.5) 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==3): turn(0.0,0.4,0.10); turn(0,0,0.00); if(DR==30): turn(0.0,0.4,0.15); turn(0,0,0.00); if(DR==31): turn(0.5,0.2,0.20); turn(0.2,0.5,0.05); #if(DR==32): sleep(2); turn(0.0,0.4,0.10); turn(0,0,0.05); if(DR==33): QLT33(cap) if(DR==70): turn(0.4,0.0,0.10); turn(0,0,0.0); if(DR==71): turn(0.4,0.0,0.10); turn(0,0,0.0); if(DR==72): turn(0.4,-0.0,0.10); turn(0,0,0.0); if(DR==73): turn(0.4,-0.2,0.10); turn(0,0,0.05); #if(DR==-2): turn(-0.3,-0.3,0.05); turn(0,0,0.05); #cv2.waitKey(0) FW1.close() FW2.close() cv2.destroyAllWindows() cap.release()