import?RPi.GPIO?as?GPIO
import?time?
import?cv2?
import?numpy?as?np?

#小車電機引腳定義（BCM編碼）（TB6612FNG驅(qū)動芯片）
IN1?=?20
IN2?=?21
IN3?=?19
IN4?=?26
ENA?=?16
ENB?=?13

#超聲波引腳定義
EchoPin?=?0
TrigPin?=?1

#設(shè)置GPIO口為BCM編碼方式
GPIO.setmode（GPIO.BCM）

#忽略警告信息
GPIO.setwarnings（False）

#電機引腳初始化操作
global?pwm_ENA
global?pwm_ENB
global?delaytime
GPIO.setup（ENAGPIO.OUTinitial=GPIO.HIGH）
GPIO.setup（IN1GPIO.OUTinitial=GPIO.LOW）
GPIO.setup（IN2GPIO.OUTinitial=GPIO.LOW）
GPIO.setup（ENBGPIO.OUTinitial=GPIO.HIGH）
GPIO.setup（IN3GPIO.OUTinitial=GPIO.LOW）
GPIO.setup（IN4GPIO.OUTinitial=GPIO.LOW）
GPIO.setup（EchoPinGPIO.IN）
GPIO.setup（TrigPinGPIO.OUT）
#設(shè)置pwm引腳和頻率為500hz
pwm_ENA?=?GPIO.PWM（ENA?500）
pwm_ENB?=?GPIO.PWM（ENB?500）
pwm_ENA.start（0）
pwm_ENB.start（0）

speed?=?0?#?直道加速的標志?
#?打開攝像頭，圖像尺寸640*480（長*高），opencv存儲值為480*640（行*列）
cap?=?cv2.VideoCapture（0）
cap.set（cv2.CAP_PROP_frame_WIDTH640）#設(shè)置圖像寬度
cap.set（cv2.CAP_PROP_frame_HEIGHT480）#設(shè)置圖像高度

#?超聲波測量障礙物距離
def?Distance_test（）:
????GPIO.output（TrigPinGPIO.HIGH）
????time.sleep（0.000015）
????GPIO.output（TrigPinGPIO.LOW）
????while?not?GPIO.input（EchoPin）:
????????pass
????t1?=?time.time（）
????while?GPIO.input（EchoPin）:
????????pass
????t2?=?time.time（）
#????print?“distance?is?%d?“?%?（（（t2?-?t1）*?340?/?2）?*?100）
????time.sleep（0.01）
????return?（（t2?-?t1）*?340?/?2）?*?100

def?stop（）:
????GPIO.output（IN1?GPIO.LOW）
????GPIO.output（IN2?GPIO.LOW）
????GPIO.output（IN3?GPIO.LOW）
????GPIO.output（IN4?GPIO.LOW）
????pwm_ENA.ChangeDutyCycle（0）
????pwm_ENB.ChangeDutyCycle（0）
????time.sleep（0.1）

#?根據(jù)采集信息調(diào)整方向
#?direction表示小車相對于車道線的距離，direction_line表示前方道路趨勢
def?run（directiondirection_line）:
????global?speed
????if?abs（direction）?>?30:?
????????stop（）
????#?右轉(zhuǎn)?
????elif?direction?>=?0:?
????????if?direction?>?25:?
????????????direction?=?25
????????if?abs（direction_line）?????????????speed?=?speed?+?1
????????????if?speed?>?4:
????????????????speed?=?4
????????????GPIO.output（IN1?GPIO.HIGH）
????????????GPIO.output（IN2?GPIO.LOW）
????????????GPIO.output（IN3?GPIO.HIGH）
????????????GPIO.output（IN4?GPIO.LOW）
????????????pwm_ENA.ChangeDutyCycle（5?+?speed）
????????????pwm_ENB.ChangeDutyCycle（7?+?speed?+?direction*0.5）
????????????time.sleep（0.1）
????????else:
????????????if?direction_line?????????????????direction_line?=?0
????????????speed?=?0
????????????temp?=?7?-?direction_line*0.03
????????????if?temp?????????????????temp?=?0
????????????GPIO.output（IN1?GPIO.HIGH）
?????????