import RPi.GPIO as GPIO import time from time import sleep import cv2 import numpy as np from PARAM import * from motorm import turn from ultdis import ultsound #from LOCATE1 import locate from FXGO2 import FXGO2 from LOCATE_goal import locate_g #---------------SERVO pin--------- pin_servo = 26 GPIO.setmode(GPIO.BCM) GPIO.setup(pin_servo, GPIO.OUT) pwm_servo = GPIO.PWM(pin_servo, 50) # 50Hz frequency pwm_servo.start(0) #----------------IR pins---------- pin1= 15 pin2= 14 GPIO.setup(pin1,GPIO.IN,pull_up_down=GPIO.PUD_UP) GPIO.setup(pin2,GPIO.IN,pull_up_down=GPIO.PUD_UP) #---------------ultra-sound pins-- trig_pin = 20 echo_pin = 21 GPIO.setup(trig_pin, GPIO.OUT) GPIO.setup(echo_pin, GPIO.IN) def destroy(): pwm_servo.stop() GPIO.cleanup() def setDirection(angle): duty = 2 + (angle / 18) pwm_servo.ChangeDutyCycle(duty) sleep(0.3) # 消除抖動 pwm_servo.ChangeDutyCycle(0) if(1==2): print('pin_servo=',pin_servo) for i in range(9): angle=90*(i%3); print(i,angle); setDirection(angle); sleep(1) setDirection(90) input('Servo test...') def distance1(): GPIO.output(trig_pin, True) sleep(0.00001) GPIO.output(trig_pin, False) pulse_start = time.time() pulse_end = time.time() while GPIO.input(echo_pin) == 0: pulse_start = time.time() while GPIO.input(echo_pin) == 1: pulse_end = time.time() pulse_duration = pulse_end - pulse_start sound_v = 34300 #cm/s distance = pulse_duration*sound_v/2 dis1=round(distance,1) return dis1 #************* TEST all functions including motor, camera, IR, # ultra-sound and Servo motor 測試所有的函數功能,包括: # 1.輪子的馬達, 2.攝影鏡頭, 3.紅外線偵測器, 4.超音波偵測器, 5.伺服馬達 #--- test of motor(測試輪子的馬達)--- if(1==9): turn(0.3,0.3,1); turn(0,0,0.5) turn(-0.3,-0.3,1); turn(0,0,0.5) turn(0.3,-0.3,1); turn(0,0,0.5) turn(-0.3,0.3,1); turn(0,0,0.5) input('mmmmm...') #-----------測試攝影鏡頭 if(2==9): cap=cv2.VideoCapture(0) cap.set(cv2.CAP_PROP_FRAME_WIDTH, Lx) cap.set(cv2.CAP_PROP_FRAME_HEIGHT, Ly) sleep(2) for j in range(501): print('j=',j) ret,img=cap.read() if(ret==False): print('ret_IR=',ret); break cv2.imshow('img',img) if cv2.waitKey(1) & 0xFF == ord('q'): break cap.release() cv2.destroyAllWindows() input('cam done') #--------------測試紅外線偵測器 if(3==9): print('test IR pins-------') for j in range(401): VP1=GPIO.input(pin1) VP2=GPIO.input(pin2) print('j_IR=',j,VP1,VP2) sleep(0.5) input('IR,IR...') #-----------測試超音波偵測器 if(4==9): print('This is test of ultra...') sleep(1) for i in range(5000): dis1 = distance1() print('i_ult=',i,dis1) sleep(0.5) input('ultultult') #-----------測試伺服馬達(裝載攝影鏡頭) if(5==9): print('pin_servo=',pin_servo) for i in range(9): angle=90*(i%3); print(i,angle); setDirection(angle); sleep(1) setDirection(90) input('Servo test...') #-----輪子馬達運轉的4種模式DR=1=前進,DR=3=左轉,DR=7=右轉,DR=0 or 9=停止 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.25,0.25,0.05); turn(0,0,0.0) if(DR==3): turn(-0.5,0.5,0.10); turn(0,0,0.1) if(DR==7): turn(0.5,-0.5,0.10); turn(0,0,0.1) #-------------利用兩個紅外線偵測器讓自駕車在黑色的軌道線上行駛 if(6==9): print('---利用兩個紅外線偵測器讓自駕車在黑色的軌道線上行駛---') for j in range(10): VP1=GPIO.input(pin1) VP2=GPIO.input(pin2) if(VP1==0 and VP2==0): DR=1 if(VP1==0 and VP2==1): DR=7 if(VP1==1 and VP2==0): DR=3 if(VP1==1 and VP2==1): DR=9 moving(DR) print('j=',j,' DR=',DR,VP1,VP2) input('IR,IR...') if(7==9): print('---黑色的軌道+轉動鏡頭---') NR=0 for j in range(80): if(j%5==0): NR+=1 ang=NR%3*90; setDirection(ang); sleep(0.1) VP1=GPIO.input(pin1) VP2=GPIO.input(pin2) if(VP1==0 and VP2==0): DR=1 if(VP1==0 and VP2==1): DR=7 if(VP1==1 and VP2==0): DR=3 if(VP1==1 and VP2==1): DR=9 moving(DR) print('j=',j,' DR=',DR,VP1,VP2) input('IR,IR...') if(8==9): print('---在黑色的軌道+轉動鏡頭+遭遇障礙時停止---') NR=0 for j in range(80): if(j%5==0): NR+=1 ang=NR%3*90; setDirection(ang); sleep(0.1) VP1=GPIO.input(pin1) VP2=GPIO.input(pin2) if(VP1==0 and VP2==0): DR=1 if(VP1==0 and VP2==1): DR=7 if(VP1==1 and VP2==0): DR=3 if(VP1==1 and VP2==1): DR=9 dis1 = distance1() if(dis1<20): print('dis1=',dis1); DR=9; moving(DR) print('j=',j,' DR=',DR,VP1,VP2,dis1) input('IR,IR...') from aimgoal import aimgoal if(9==91): print('---軌道+轉動鏡頭+遇障停止+鏡頭發現目標,離開軌道追蹤目標---') cap=cv2.VideoCapture(0) cap.set(cv2.CAP_PROP_FRAME_WIDTH, Lx) cap.set(cv2.CAP_PROP_FRAME_HEIGHT, Ly) NR=0 for j in range(80): ret, frame = cap.read() if(ret==False): break img2=np.copy(frame) NN,xs,ys,WDS,img24=locate_g(img2) cv2.imshow('img24',img24) if cv2.waitKey(1) & 0xFF == ord('q'): break if(j%5==0): NR+=1 ang=NR%3*90; setDirection(ang); sleep(0.1) VP1=GPIO.input(pin1) VP2=GPIO.input(pin2) if(VP1==0 and VP2==0): DR=1 if(VP1==0 and VP2==1): DR=7 if(VP1==1 and VP2==0): DR=3 if(VP1==1 and VP2==1): DR=9 dis1 = distance1() if(dis1<20): print('dis1=',dis1); DR=9; moving(DR) print('j=',j,' DR=',DR,VP1,VP2,dis1) aimgoal(cap) cap.release() cv2.destroyAllWindows() input('IR,IR...') #----------------LOOP-1------------- cap=cv2.VideoCapture(0) cap.set(cv2.CAP_PROP_FRAME_WIDTH, Lx) cap.set(cv2.CAP_PROP_FRAME_HEIGHT, Ly) setDirection(90) print('LOOP-1, set initial direction as forward..') angle=0; dang=int(180/2); NF=5; ESC=0 for j in range(2000): if(j%NF==0): k=int(j/NF)%3 angle=k*90 print('j,k,angle=',j,k,angle) setDirection(angle) sleep(0.1) ang=angle angle+=dang #sleep(0.5) #continue ret, frame = cap.read() if(ret==False): break #cv2.imshow('frame',frame) img2=np.copy(frame) NN,xs,ys,WDS,img24=locate_g(img2) cv2.imshow('img24',img24) if cv2.waitKey(1) & 0xFF == ord('q'): break #continue if(NN>0): ESC=1 if(ang==90): print('NN=',NN,'ang=90') sleep(1) else: print('ang,angle=',ang,angle) sleep(2) if(int(ang)==0): print('a0,GOT it in side view! j=',j,NN,xs,ys,WDS); setDirection(90); sleep(1) turn(0.5,-0.5,0.4);turn(0,0,0) if(int(ang)==180): print('a180,GOT it in side view! j=',j,NN,xs,ys,WDS); setDirection(90); sleep(1) turn(-0.5,0.5,0.5);turn(0,0,0) #turn(0.3,0.3,0.4); turn(-0.3,-0.3,0.5); turn(0,0,0.1) #sleep(1) if(ESC==1): break VP1=GPIO.input(pin1) VP2=GPIO.input(pin2) if(VP1==0 and VP2==0): DR=1 if(VP1==0 and VP2==1): DR=7 if(VP1==1 and VP2==0): DR=3 if(VP1==1 and VP2==1): DR=9 print(j,VP1,VP2,' DR=',DR) VP1=GPIO.input(pin1) VP2=GPIO.input(pin2) print(j) moving(DR) sleep(0.05) FXGO2(cap) input('iiiii-111111') cap.release() cv2.destroyAllWindows()