樹莓派+二自由度雲臺製作智能小車（三）——小車運動+簡單的自動避障

小車與避障原理

一、小車運動模塊

小車主要是分爲動力模塊和控制模塊，動力模塊由驅動板，直流電機加輪子以及外接供電的鋰電池組成。控制模塊就是樹莓派了，連接驅動板對電機進行控制，實現對小車運動的控制。
注意事項：當小車不轉的時候，一定要注意，驅動板和樹莓派是否構成一個完整的迴路（共地）。
其次是不同的直流電機和供電，會導致小車速度不同，然後會影響避障策略的設置，此時需要調整延時時間即可，若各位有比較好的意見，請在評論區留言，歡迎進行交流。
本文涉及的驅動板是L298n驅動板，長下面這個樣子，有IN1–IN4共4個輸入接口，對應OUT1–OUT4，ENA、ENB共兩個使能接口，一個5V，一個12V和一個GND的電源接口，具體參數，有興趣的可以下載看一下，由於無法直接放文件，所以可以點這裏下載

二、避障原理

在瞭解避障決策之前，讓我們先對障礙物類型進行一個分類，各位看官道友也可以根據自己的喜好對障礙物進行分類，然後設置不同的避障決策。

箭頭代表了傳感器的探測方向，本文正前方的箭頭設置爲紅外傳感器，左右的爲超聲波傳感器。由於紅外傳感器只有一個，由於紅外傳感器設計的探測範圍問題，容易出現側前方出現障礙物，識別不準障礙物類型的情況，可適當添加傳感器。
==建議：==使用同款傳感器和T型板的話，建議對紅外傳感器的連接線進行一下改動，將母頭改成公頭。方便和T型板進行連接。
採用的是一種比較簡單的避障決策，用一張圖，可以用以下這張圖來體現。

三、實現

從這裏開始就要注意各個部件連接的接口號了，我們爲方便統一管理，創建一個config.ini的配置文件，用於管理硬件連接的pin口編號。雲臺後續再進行添加管理。之後的連線，也主要都是通過看配置文件裏面，就比較方便，不用一個個文件看。

[car]
# This is the parmaters that will control the car's wheels
# The number is the interface number if GPIO(GPIO.BCM)
#board 11 12 13 15
IN1 = 17
IN2 = 18
IN3 = 27
IN4 = 22

[ultrasonic]
#BCM 25 6 12 5
#control ultrasonic detection distance for obstacle avoidance
Trigger_l = 25
Trigger_r =6
Echo_l = 12
Echo_r = 5

[infrared]
#BCM  20 detected = 20CM
single_pin = 20

使用了配置文件之後，就需要import configparser。
然後讀取配置文件。

        config = configparser.ConfigParser()
        config.read("config.ini")

（1）將傳感器整合。

將各個障礙物類型進行分類後，設置幾個傳感器配合，對障礙物類型進行判別，同時可以傳參給樹莓派進行避障策略選擇。創建名爲sense.py。

import RPi.GPIO as GPIO
import time
import configparser
from infrared import Infrared
from ultrasonic import Ultrasonic

#Define the number of all the GPIO taht used for 4wd car
class Sense():
    
    def __init__(self):
        config = configparser.ConfigParser()
        config.read("config.ini")
        
        trigger_l=config.getint("ultrasonic","Trigger_l")
        trigger_r=config.getint("ultrasonic","Trigger_r")
        echo_l=config.getint("ultrasonic","Echo_l")
        echo_r=config.getint("ultrasonic","Echo_r")
        single_pin=config.getint("infrared","single_pin")
        
        self.Sonic_l = Ultrasonic(trigger_l,echo_l)
        self.Sonic_r = Ultrasonic(trigger_r,echo_r)
        self.Infrared = Infrared(single_pin)
    #get obstacles type   
    def detection(self):
        
        #NO Obstacles or 20cm safe distance  type 0
        if self.Infrared.check_distance() == "OK":
            return "Fgo"
        #Obstacles ahead
        #self.Infrared.check_distance() == "Warning"
        else :
            l_d = self.Sonic_l.get_distance()
            r_d = self.Sonic_r.get_distance()
        #Obstacles ahead&&right  =safe       type a
            if ((l_d>=30)and(r_d>=30)):
                return "Lgo"
        #Obstacles ahead&&right  R>L         type b
            elif ((l_d<=30)
                and(r_d>30)):
                return "Rgo"
        #Obstacles ahead&&right  L>R         type c
            elif ((l_d>30)
                and(r_d<=30)):
                return "Lgo"
        #Obstacles ahead&&right  L&R<safe         type d
            elif ((l_d<30)
                and(r_d<30)):
                return "Bgo"
    
    def ask_distance_l(self):
        return self.Sonic_l.get_distance()
        
    def ask_distance_r(self):
        return self.Sonic_r.get_distance()
    
if __name__ == '__main__':
    t = Sense()
    while True:
        s=t.detection()
        print(s)
        time.sleep(0.5)

（2）小車的運動代碼

此時小車這邊的基本控制代碼如下：
創建一個carCtrl.py
然後裏面的有一些註釋，沒有刪，各位將就着看。

import RPi.GPIO as GPIO
import time
import configparser
from sense import Sense
#Define the number of all the GPIO taht used for 4wd car
class CAR():
    
    delay_tl = 0.45
    delay_tr = 0.5
    delay_30 = 0.4
    
    def __init__(self):
        config = configparser.ConfigParser()
        config.read("config.ini")
        self.IN1=config.getint("car","IN1")
        self.IN2=config.getint("car","IN2")
        self.IN3=config.getint("car","IN3")
        self.IN4=config.getint("car","IN4")
        GPIO.setmode(GPIO.BCM)
        GPIO.setwarnings(False)  #avoid GPIO warning
        GPIO.setup(self.IN1,GPIO.OUT)
        GPIO.setup(self.IN2,GPIO.OUT)
        GPIO.setup(self.IN3,GPIO.OUT)
        GPIO.setup(self.IN4,GPIO.OUT)
        self.reset()
        
        self.sense = Sense()
        
    def reset(self):
        #Rest all the GPIO as LOW
        GPIO.output(self.IN1,False)
        GPIO.output(self.IN2,False)
        GPIO.output(self.IN3,False)
        GPIO.output(self.IN4,False)
        
    def __forward(self):
 #       self.reset()
        GPIO.output(self.IN1,True)
        GPIO.output(self.IN2,False)
        GPIO.output(self.IN3,True)
        GPIO.output(self.IN4,False)
    
    def __backward(self):
 #       self.reset()
        GPIO.output(self.IN1,False)
        GPIO.output(self.IN2,True)
        GPIO.output(self.IN3,False)
        GPIO.output(self.IN4,True)
    
    def __turnright(self):
 #       self.reset()
        GPIO.output(self.IN1,True)
        GPIO.output(self.IN2,False)
        GPIO.output(self.IN3,False)
        GPIO.output(self.IN4,True)
    
    def __turnleft(self):
 #       self.reset()
        GPIO.output(self.IN1,False)
        GPIO.output(self.IN2,True)
        GPIO.output(self.IN3,True)
        GPIO.output(self.IN4,False)
        
    def __stop(self):
        self.reset()
        
    def CarMove(self,direction,loop):
        if direction == 'F':
            self.__forward()
        elif direction == 'B':
            self.__backward()
        elif direction == 'R':
            self.__turnright()
        elif direction == 'L':
            self.__turnleft()
        elif direction == 'Auto':
            self.autorun(loop)
        elif direction == 'S':
            self.__stop()
        elif direction == 'E':
            GPIO.cleanup()
            exit()
#        else:
#            print("The input error! please input:F,B,L,R,S")
    
#下面的代碼都是用於避障的函數，裏面所有的loop都是之後要用到的。    
    #the methods is used to strategy and run
    def backx(self,loop):
        while loop:
            self.__backward()
            time.sleep(0.2)
            self.reset()
            l_d = self.sense.ask_distance_l()
            r_d = self.sense.ask_distance_r()
            if r_d > 30:
                #get a safe distance
                self.__backward()
                time.sleep(0.2)
                #turn right 90
                self.__turnright()
                time.sleep(self.delay_tr)
                #advance 30cm
                self.__forward()
                time.sleep(self.delay_30)
                #turn left 90
                self.__turnleft()
                time.sleep(self.delay_tl)
                break
            if l_d > 30:
                #get a safe distance
                self.__backward()
                time.sleep(0.2)
                #turn left 90
                self.__turnleft()
                time.sleep(self.delay_tl)
                #advance 30cm
                self.__forward()
                time.sleep(self.delay_30)
                #turn right 90
                self.__turnright()
                time.sleep(self.delay_tr)
                break
    def Lgox(self,loop):
        while loop:
            self.__forward()
            time.sleep(0.2)
            r_d = self.sense.ask_distance_r()
            if r_d > 30:
                #advance 15cm
                self.__forward()
                time.sleep(0.2)
                #turn right 90
                self.__turnright()
                time.sleep(self.delay_tr)
                break
            
    def Rgox(self,loop):
        while loop:
            self.__forward()
            time.sleep(0.2)
            l_d = self.sense.ask_distance_l()
            if l_d > 30:
                #advance 15cm
                self.__forward()
                time.sleep(0.2)
                #turn left 90
                self.__turnleft()
                time.sleep(self.delay_tl)
                break
            
     #run and strategy
    def autorun(self,loop):
#        self.sense = Sense()
 #       while True:
            #get obstacle type
        self.reset()
        strategy = self.sense.detection()
        
        if strategy == "Fgo":
            self.__forward()
            time.sleep(0.05)
            
        elif strategy == "Bgo":
            self.backx(loop)
            
        elif strategy == "Lgo":
            self.__backward()
            time.sleep(0.1)
            #turn left 90
            self.__turnleft()
            time.sleep(self.delay_tl)
            #return original direction
            self.Lgox(loop)
        
        elif strategy == "Rgo":
            self.__backward()
            time.sleep(0.1)
            #turn left 90
            self.__turnright()
            time.sleep(self.delay_tr)
            #return original direction
            self.Rgox(loop)

#通過調整佔空比來改變小車速度，行自行發揮    
'''def changeSpeed():
    leftpwm = GPIO.PWM(ENA,50)
    leftpwm = stop()
    leftpwm = start(100)
    leftpwm = ChangeDutyCycle(50)
    print('changeSpeed'+50)    
   '''

'''
#control test

  #  changeSpeed()
    RaspCar=CAR()
    RaspCar.autorun()
#turn angle test
'''
if __name__=='__main__':
    RaspCar=CAR()
    while(True):
        direction = input("Please input direction:")
        print(direction)
        RaspCar.CarMove(direction,True)
        time.sleep(0.5)
        direction = "S"
        RaspCar.CarMove(direction,True)

在測試的時候，請注意loop在這裏是無用的，在後面是會用到的，不然不好退出自動避障切換成手動操作。