一朽缴、介紹

??閱讀本篇文章前建議先參考前期文章：
??樹莓派基礎(chǔ)實驗34：L298N模塊驅(qū)動直流電機實驗，學(xué)習(xí)了單個電機的簡單驅(qū)動樊卓。
??樹莓派綜合項目2：智能小車（一）四輪驅(qū)動，實現(xiàn)了代碼輸入對四個電機的簡單控制短蜕。
??樹莓派綜合項目2：智能小車（二）tkinter圖形界面控制氢架，實現(xiàn)了本地圖形界面控制小車的前進(jìn)后退、轉(zhuǎn)向和原地轉(zhuǎn)圈朋魔。
??樹莓派綜合項目2：智能小車（三）無線電遙控岖研，實現(xiàn)了無線電遙控設(shè)備控制小車的前進(jìn)后退、轉(zhuǎn)向和原地轉(zhuǎn)圈警检。

??本實驗中將使用HC-SR04超聲波傳感器實時感知小車前方障礙物的距離孙援，當(dāng)距離近于某個閾值時，小車自動減速扇雕，再低于某個閾值時自動剎車拓售，然后倒車至安全距離。

??其它基礎(chǔ)內(nèi)容可以參考文集：樹莓派基礎(chǔ)實驗镶奉。

二础淤、組件

★Raspberry Pi 3 B+全套*1

★睿思凱Frsky XM+ 迷你接收機*1

★電平反向器模塊*1

★睿思凱Frsky Taranis X9D PLUS SE2019遙控器*1

★L(fēng)298N擴展板模塊*1

★智能小車底板模塊*1

★減速電機和車輪*4

★HC-SR04超聲波模塊*1

★跳線若干

三、實驗原理

HC-SR04超聲波傳感器的Echo 返回的是 5v信號哨苛，而樹莓派的 GPIO 接收超過 3.3v 的信號可能會被燒毀值骇，因此需要加一個分壓電路，這里由于返回的脈沖時間非常短移国，我沒有加，能正常運行道伟，但還是冒險了迹缀！

HC-SR04超聲波傳感器

建議樹莓派實驗選用US-100超聲波傳感器，使用3.3V電源時蜜徽，輸出也是3.3V電源祝懂，更安全。某寶上20幾元拘鞋，只比HC-SR04貴十幾元哈砚蓬。

US-100超聲波傳感器

??關(guān)于超聲波傳感器的基礎(chǔ)知識請參見樹莓派基礎(chǔ)實驗24：超聲波測距傳感器實驗。

四盆色、實驗步驟

??第1步： 連接電路灰蛙。在樹莓派綜合項目2：智能小車（一）四輪驅(qū)動中的接線基礎(chǔ)上，接入電平反向器隔躲、無線電接收機摩梧。

關(guān)于這里樹莓派GND、L298N小車擴展板的電池組供電負(fù)極相連宣旱，是特殊情況下的情況仅父，經(jīng)測試發(fā)現(xiàn)：
如果樹莓派用的是充電頭供電，而L298N擴展板用的是電池組供電，這兩個負(fù)極必須相連笙纤，否則馬達(dá)不動耗溜。
如果樹莓派用的是L298N擴展板接出來的5V供電，即兩者同一個電源省容，則這里不用連接抖拴。

這里也要注意，由于樹莓派的GPIO只能接收3.3V的最高輸入蓉冈，所以電平反相器的電源也只能使用3.3V城舞，若反向后接收的信號需要是5V，則電平反相器的電源就使用5V寞酿。

加裝超聲波模塊小車電路圖

加裝超聲波模塊的小車

這里我將18650電池組換成了航模使用的格氏ACE鋰電池（3S/11.1V/2200MAH/40C）家夺，電壓更高，能給樹莓派提供更穩(wěn)定的電源伐弹，動力性也更好拉馋，效果非常不錯。

??第2步： 編寫電機的驅(qū)動程序惨好，文件名為motor_4w.py煌茴。與樹莓派綜合項目2：智能小車（一）四輪驅(qū)動中的程序基本相同。

??該車的行進(jìn)控制與履帶車的行進(jìn)控制類似：

前進(jìn)和后退很簡單日川，左右兩邊的方向都朝前或朝后蔓腐，速度一致；
原地順時針旋轉(zhuǎn)時龄句，左邊輪子前進(jìn)回论，右邊輪子后退，速度一致分歇；
原地逆時針旋轉(zhuǎn)時傀蓉，左邊輪子后退，右邊輪子前進(jìn)职抡，速度一致葬燎；
偏左前進(jìn)時，左右兩邊的方向都朝前缚甩，左輪速度比右輪速度慢一點谱净；
偏右前進(jìn)時，左右兩邊的方向都朝前擅威，左輪速度比右輪速度快一點岳遥；
偏左后退時，左右兩邊的方向都朝后裕寨，左輪速度比右輪速度慢一點浩蓉；
偏右后退時派继，左右兩邊的方向都朝后，左輪速度比右輪速度快一點捻艳；

下面的部分程序在前面的文章中已有驾窟，這里做了部分優(yōu)化，主要的超聲波模塊程序认轨，和主程序在最后面绅络。

motor_4w.py：

#!/usr/bin/env python
#-*- coding: utf-8 -*-
#本模塊只含Motor_4w()一個類，用于樹莓派對電機信號的控制
#通過GPIO輸出信號嘁字，直接對某個電機的轉(zhuǎn)動方向恩急、速度進(jìn)行控制

import RPi.GPIO as GPIO

class Motor_4w():
    '''控制小車四輪動作的類'''
    ENA = 11  #使能信號A，左邊兩輪
    IN1 = 13  #信號輸入1
    IN2 = 15  #信號輸入2
    ENB = 12  #使能信號B纪蜒，右邊兩輪
    IN3 = 16  #信號輸入3
    IN4 = 18  #信號輸入4

    GPIO.setwarnings(False) #關(guān)閉警告

    def setGPIO(self):
        '''初始化引腳'''
        GPIO.setmode(GPIO.BOARD)
        GPIO.setup(Motor_4w.ENA, GPIO.OUT)
        GPIO.setup(Motor_4w.IN1, GPIO.OUT)
        GPIO.setup(Motor_4w.IN2, GPIO.OUT)
        
        GPIO.setup(Motor_4w.ENB, GPIO.OUT)
        GPIO.setup(Motor_4w.IN3, GPIO.OUT)
        GPIO.setup(Motor_4w.IN4, GPIO.OUT)
    
    def pwm(self,pwm): 
        '''初始化PWM（脈寬調(diào)制）衷恭，返回PWM對象'''
        EN_pwm = GPIO.PWM(pwm, 500)
        EN_pwm.start(0)
        return EN_pwm

    def changespeed(self,pwm,speed):
        '''通過改變占空比改變馬達(dá)轉(zhuǎn)速'''
        pwm.ChangeDutyCycle(speed)
        #print('speed=',speed) #測試數(shù)據(jù)用
    
    def clockwise(self,in1_pin,in2_pin):
        '''馬達(dá)順時針轉(zhuǎn)的信號'''
        GPIO.output(in1_pin, 1)
        GPIO.output(in2_pin, 0)
 
    def counter_clockwise(self,in1_pin,in2_pin):
        '''馬達(dá)逆時針轉(zhuǎn)的信號'''
        GPIO.output(in1_pin, 0)
        GPIO.output(in2_pin, 1)
    
    def stop_car(self,in1_pin,in2_pin):
        '''馬達(dá)制動的信號'''
        GPIO.output(in1_pin, 0)
        GPIO.output(in2_pin, 0)
     #使能信號為低電平，或者高電平（占空比設(shè)為100纯续，IN1和IN2都為0或1時）馬達(dá)制動
        
    def destroy(self,A,B):
        '''結(jié)束程序時清空GPIO狀態(tài)'''
        A.stop()
        B.stop()
        GPIO.cleanup()    # Release resource


if __name__ == '__main__':     # 本模塊單獨測試時運行使用

    try:
        motor_4w = Motor_4w() #創(chuàng)建樹莓派小車對象
        motor_4w.setGPIO()  #初始化引腳

        ENA_pwm=motor_4w.pwm(motor_4w.ENA) #初始化使能信號PWM随珠，A為左邊車輪
        ENB_pwm=motor_4w.pwm(motor_4w.ENB) #初始化使能信號PWM，B為右邊車輪
        
        while True:
            '''通過輸入的命令改變馬達(dá)轉(zhuǎn)動'''
            cmd = input("Command, E.g. ff30ff30 :")
            direction = cmd[0] #只輸入字母b時猬错，小車剎車
            A_direction = cmd[0:2] #字符串0/1兩位為控制A（左邊車輪）方向信號
            B_direction = cmd[4:6] #4/5位為控制B（右邊車輪）方向信號
        
            A_speed = cmd[2:4] #字符串2/3兩位為控制A（左邊車輪）速度信號
            B_speed = cmd[6:8] #字符串6/7兩位為控制B（右邊車輪）速度信號
            print (A_direction,B_direction,A_speed,B_speed) #測試用
        
            if A_direction == "ff": #控制A（左邊車輪）順時針信號
                motor_4w.clockwise(motor_4w.IN1,motor_4w.IN2)
            if A_direction == "00": #控制A（左邊車輪）逆時針信號
                motor_4w.counter_clockwise(motor_4w.IN1,motor_4w.IN2)
            if B_direction == "ff": #控制B（右邊車輪）順時針信號
                motor_4w.clockwise(motor_4w.IN3,motor_4w.IN4)
            if B_direction == "00": #控制B（右邊車輪）逆時針信號
                motor_4w.counter_clockwise(motor_4w.IN3,motor_4w.IN4)

            if direction == "b": #小車剎車窗看，IN1和IN2都為0，馬達(dá)制動
                motor_4w.stop_car(motor_4w.IN1,motor_4w.IN2)
                motor_4w.stop_car(motor_4w.IN3,motor_4w.IN4)
                continue #跳出本次循環(huán)

            # 通過輸入的兩位數(shù)字設(shè)置占空比倦炒，改變馬達(dá)轉(zhuǎn)速
            motor_4w.changespeed(ENA_pwm,int(A_speed))
            motor_4w.changespeed(ENB_pwm,int(B_speed))

    except KeyboardInterrupt:  # When 'Ctrl+C' is pressed, the child program destroy() will be  executed.
        motor_4w.destroy(ENA_pwm,ENB_pwm)
    finally:
        motor_4w.destroy(ENA_pwm,ENB_pwm)

??第3步： 小車行進(jìn)控制模塊显沈，文件名為moving_control.py，設(shè)計小車的移動行為逢唤，方向控制分為：原地左轉(zhuǎn)彎构罗、原地右轉(zhuǎn)彎、直線前進(jìn)智玻、直線后退、剎車芙代，而向前左偏移開進(jìn)或右偏移開進(jìn)等吊奢，由左右兩邊不同的速度（油門）來控制。
moving_control.py:

#-*- coding: utf-8 -*-
#本模塊只含MovingControl()一個類纹烹，針對遙控器的控制方式页滚，設(shè)計小車的移動行為
#方向控制分為：原地左轉(zhuǎn)彎、原地右轉(zhuǎn)彎铺呵、直線前進(jìn)裹驰、直線后退、剎車
#而向前左偏移開進(jìn)或右偏移開進(jìn)等片挂，由左右兩邊不同的速度（油門）來控制

class MovingControl():
    def __init__(self, smpcar,pwm1,pwm2):
        self.smpcar=smpcar
        self.ENA_pwm=pwm1
        self.ENB_pwm=pwm2
        self.rudder_value = 0
        self.acc_value = 0
        
    def accelerator(self,rate_left=1,rate_right=1):
        '''速度（油門）控制'''
        #rate_left為向左偏移行進(jìn)時幻林，降低左邊輪的速度
        #rate_right為向右偏移行進(jìn)時贞盯，降低右邊輪的速度
        self.smpcar.changespeed(self.ENA_pwm,self.acc_value * rate_left)
        self.smpcar.changespeed(self.ENB_pwm,self.acc_value * rate_right)
        #print('acc_value=',self.acc_value)
        
    def leftTurn(self):
        '''原地左轉(zhuǎn)彎'''
        self.smpcar.counter_clockwise(self.smpcar.IN1,self.smpcar.IN2) #左邊車輪后退
        self.smpcar.clockwise(self.smpcar.IN3,self.smpcar.IN4) #右邊車輪前進(jìn)
        
    def rightTurn(self):
        '''原地右轉(zhuǎn)彎'''
        self.smpcar.clockwise(self.smpcar.IN1,self.smpcar.IN2)
        self.smpcar.counter_clockwise(self.smpcar.IN3,self.smpcar.IN4)

    def forward(self):
        '''直線前進(jìn)'''
        self.smpcar.clockwise(self.smpcar.IN1,self.smpcar.IN2)
        self.smpcar.clockwise(self.smpcar.IN3,self.smpcar.IN4)
    
    def reverse(self):
        '''直線后退'''
        self.smpcar.counter_clockwise(self.smpcar.IN1,self.smpcar.IN2)
        self.smpcar.counter_clockwise(self.smpcar.IN3,self.smpcar.IN4)
       
    def brake(self):
        '''剎車'''
        self.smpcar.stop_car(self.smpcar.IN1,self.smpcar.IN2)
        self.smpcar.stop_car(self.smpcar.IN3,self.smpcar.IN4)
     

??第4步： 編寫SBUS信號接收解析模塊，文件名為sbus_receiver_pi.py沪饺，與樹莓派基礎(chǔ)實驗39：解析無線電接收機PWM躏敢、SBUS信號中的Python2程序有所不同，下面的程序在Python3中運行整葡，并標(biāo)注了兩者的不同之處件余。
sbus_receiver_pi.py：

#!/usr/bin/env python
#-*- coding: utf-8 -*-
#本模塊只含SBUSReceiver()一個類，用于獲取和解析遙控器接收機的SBUS輸出信號
#并能返回每個通道的數(shù)值遭居，遙控器的failsafe信號狀態(tài)啼器，每次獲得數(shù)據(jù)幀的長度和這次數(shù)據(jù)的延遲時間

#import array #array模塊是python中實現(xiàn)的一種高效的數(shù)組存儲類型
import serial #serial模塊封裝了對串行端口的訪問
#import binascii #python2運行時需要，binascii模塊包含很多在二進(jìn)制和ASCII編碼的二進(jìn)制表示轉(zhuǎn)換的方法
#import codecs #python2運行時需要俱萍，Python中專門用作編碼轉(zhuǎn)換的模塊
import time

class SBUSReceiver():
    def __init__(self, _uart_port='/dev/ttyAMA0'):

        #初始化樹莓派串口參數(shù)
        self.ser = serial.Serial(
            port=_uart_port,            #樹莓派的硬件串口/dev/ttyAMA0
            baudrate = 100000,          #波特率為100k
            parity=serial.PARITY_EVEN,  #偶校驗
            stopbits=serial.STOPBITS_TWO,#2個停止位
            bytesize=serial.EIGHTBITS,   #8個數(shù)據(jù)位
            timeout = 0,
        )

        # 常數(shù)
        #這里注意：Python2 與Python3 的編碼方式是不同的
        #self.START_BYTE = b'\x0f'   #python2運行時用這句端壳，起始字節(jié)為0x0f
        #self.END_BYTE = b'\x00'     #python2運行時用這句，結(jié)束字節(jié)為0x00
        self.START_BYTE = 0x0f  #python3運行時用這句鼠次，起始字節(jié)為0x0f
        self.END_BYTE = 0x00    #python3運行時用這句更哄，結(jié)束字節(jié)為0x00
        self.SBUS_FRAME_LEN = 25    #SBUS幀有25個字節(jié)
        self.SBUS_NUM_CHAN = 18     #18個通道
        self.OUT_OF_SYNC_THD = 10
        self.SBUS_NUM_CHANNELS = 18 #18個通道
        self.SBUS_SIGNAL_OK = 0     #信號正常為0
        self.SBUS_SIGNAL_LOST = 1       #信號丟失為1
        self.SBUS_SIGNAL_FAILSAFE = 2   #輸出failsafe信號時為2

        # 堆棧變量初始化
        self.isReady = True
        self.lastFrameTime = 0
        self.sbusBuff = bytearray(1)  # 用于同步的單個字節(jié)
        
        #bytearray(n) 方法返回一個長度為n的初始化數(shù)組；
        '''這里Python2與Python3存儲數(shù)據(jù)的編碼格式會不同'''
        self.sbusFrame = bytearray(25)  # 單個SBUS數(shù)據(jù)幀腥寇，25個字節(jié)
        
        # 接收到的各頻道值成翩，Python2中使用數(shù)組
        #self.sbusChannels = array.array('H', [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])  
        # 接收到的各頻道值，Python3中這里也可以使用列表
        self.sbusChannels = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
        #array.array(typecode,[initializer]) --typecode:元素類型代碼赦役；initializer:初始化器麻敌，若數(shù)組為空，則省略初始化器
        
        self.failSafeStatus = self.SBUS_SIGNAL_FAILSAFE

    def get_rx_channels(self):
        """
        用于讀取最后的SBUS通道值
        返回:由18個無符號短元素組成的數(shù)組掂摔，包含16個標(biāo)準(zhǔn)通道值+ 2個數(shù)字(ch17和18)
        """
        return self.sbusChannels

    def get_rx_channel(self, num_ch):
        """
        用于讀取最后的SBUS某一特定通道的值
        num_ch: 要讀取的某個通道的通道序號
        返回:某一通道的值
        """
        return self.sbusChannels[num_ch]

    def get_failsafe_status(self):
        """
        用于獲取最后的FAILSAFE狀態(tài)
        返回: FAILSAFE狀態(tài)值
        """
        return self.failSafeStatus

    def decode_frame(self):
        """
        對每幀數(shù)據(jù)進(jìn)行解碼术羔，每個通道的值在兩個或三個不同的字節(jié)之間，要讀取出來很麻煩
        不過futaba已經(jīng)發(fā)布了下面的解碼代碼
        """
        def toInt(_from):
            #encode() 方法以指定的編碼格式編碼字符串乙漓。
            #int() 函數(shù)用于將一個字符串或數(shù)字轉(zhuǎn)換為整型级历。
            
            #return int(codecs.encode(_from, 'hex'), 16) #Python2中要使用這句，轉(zhuǎn)換編碼格式
            return _from      #Python3中要使用這句叭披，即不用轉(zhuǎn)換編碼格式
            
        #CH1 = [data2]的低3位 + [data1]的8位（678+12345678 = 678,12345678）
        self.sbusChannels[0]  = ((toInt(self.sbusFrame[1])      |toInt(self.sbusFrame[2])<<8)                                   & 0x07FF);
        #CH2 = [data3]的低6位 + [data2]的高5位（345678+12345 = 345678,12345 ）
        self.sbusChannels[1]  = ((toInt(self.sbusFrame[2])>>3   |toInt(self.sbusFrame[3])<<5)                                   & 0x07FF);
        #CH3 = [data5]的低1位 + [data4]的8位 + [data3]的高2位（8+12345678+12 = 8,12345678,12）
        self.sbusChannels[2]  = ((toInt(self.sbusFrame[3])>>6   |toInt(self.sbusFrame[4])<<2 |toInt(self.sbusFrame[5])<<10)     & 0x07FF);
        self.sbusChannels[3]  = ((toInt(self.sbusFrame[5])>>1   |toInt(self.sbusFrame[6])<<7)                                   & 0x07FF);
        self.sbusChannels[4]  = ((toInt(self.sbusFrame[6])>>4   |toInt(self.sbusFrame[7])<<4)                                   & 0x07FF);
        self.sbusChannels[5]  = ((toInt(self.sbusFrame[7])>>7   |toInt(self.sbusFrame[8])<<1 |toInt(self.sbusFrame[9])<<9)      & 0x07FF);
        self.sbusChannels[6]  = ((toInt(self.sbusFrame[9])>>2   |toInt(self.sbusFrame[10])<<6)                                  & 0x07FF);
        self.sbusChannels[7]  = ((toInt(self.sbusFrame[10])>>5  |toInt(self.sbusFrame[11])<<3)                                  & 0x07FF);
        self.sbusChannels[8]  = ((toInt(self.sbusFrame[12])     |toInt(self.sbusFrame[13])<<8)                                  & 0x07FF);
        self.sbusChannels[9]  = ((toInt(self.sbusFrame[13])>>3  |toInt(self.sbusFrame[14])<<5)                                  & 0x07FF);
        self.sbusChannels[10] = ((toInt(self.sbusFrame[14])>>6  |toInt(self.sbusFrame[15])<<2|toInt(self.sbusFrame[16])<<10)    & 0x07FF);
        self.sbusChannels[11] = ((toInt(self.sbusFrame[16])>>1  |toInt(self.sbusFrame[17])<<7)                                  & 0x07FF);
        self.sbusChannels[12] = ((toInt(self.sbusFrame[17])>>4  |toInt(self.sbusFrame[18])<<4)                                  & 0x07FF);
        self.sbusChannels[13] = ((toInt(self.sbusFrame[18])>>7  |toInt(self.sbusFrame[19])<<1|toInt(self.sbusFrame[20])<<9)     & 0x07FF);
        self.sbusChannels[14] = ((toInt(self.sbusFrame[20])>>2  |toInt(self.sbusFrame[21])<<6)                                  & 0x07FF);
        self.sbusChannels[15] = ((toInt(self.sbusFrame[21])>>5  |toInt(self.sbusFrame[22])<<3)                                  & 0x07FF);

        #17頻道寥殖，第24字節(jié)的最低一位
        if toInt(self.sbusFrame[23])  & 0x0001 :
            self.sbusChannels[16] = 2047
        else:
            self.sbusChannels[16] = 0
        #18頻道，第24字節(jié)的低第二位涩蜘，所以要右移一位
        if (toInt(self.sbusFrame[23]) >> 1) & 0x0001 :
            self.sbusChannels[17] = 2047
        else:
            self.sbusChannels[17] = 0

        #幀丟失位為1時嚼贡，第24字節(jié)的低第三位，與0x04進(jìn)行與運算
        self.failSafeStatus = self.SBUS_SIGNAL_OK
        if toInt(self.sbusFrame[23]) & (1 << 2):
            self.failSafeStatus = self.SBUS_SIGNAL_LOST
        #故障保護(hù)激活位為1時同诫，第24字節(jié)的低第四位粤策，與0x08進(jìn)行與運算   
        if toInt(self.sbusFrame[23]) & (1 << 3):
            self.failSafeStatus = self.SBUS_SIGNAL_FAILSAFE


    def update(self):
        """
        我們需要至少2幀大小，以確保找到一個完整的幀
        所以我們?nèi)〕鏊械木彺妫ㄇ蹇账┪蠼眩x取全部數(shù)據(jù)叮盘，直到捕獲新的數(shù)據(jù)
        首先找到END BYTE并向后查找SBUS_FRAME_LEN秩贰，看看它是否是START BYTE
        """

        #我們是否有足夠的數(shù)據(jù)在緩沖區(qū)和有沒有線程在后臺?
        if self.ser.inWaiting() >= self.SBUS_FRAME_LEN*2 and self.isReady: #inWaiting()返回接收緩存中的字節(jié)數(shù)
            self.isReady = False    #表明有線程在運行，isReady = False
            
            # 讀取所有臨時幀數(shù)據(jù)
            tempFrame = self.ser.read(self.ser.inWaiting())
            # 在緩沖區(qū)幀的每個字符中熊户，我們尋找結(jié)束字節(jié)
            for end in range(0, self.SBUS_FRAME_LEN):
                #尋找結(jié)束字節(jié)萍膛，從后向前查找
                if tempFrame[len(tempFrame)-1-end] == self.END_BYTE :
                    #從最后的命中點減去SBUS_FRAME_LEN尋找起始字節(jié)
                    if tempFrame[len(tempFrame)-end-self.SBUS_FRAME_LEN] == self.START_BYTE :
                        # 如果相等，則幀數(shù)據(jù)正確嚷堡，數(shù)據(jù)以8E2包到達(dá)蝗罗，因此它已經(jīng)被校驗過
                        # 從臨時幀數(shù)據(jù)中取出剛驗證正確的一段正確幀數(shù)據(jù)
                        lastUpdate = tempFrame[len(tempFrame)-end-self.SBUS_FRAME_LEN:len(tempFrame)-1-end]
                        if not self.sbusFrame == lastUpdate: #相等即表示沒有操作，不用再次解碼
                            self.sbusFrame = lastUpdate
                            self.decode_frame() #調(diào)用解碼函數(shù)

                        self.lastFrameTime = time.time() # 跟蹤最近的更新時間
                        self.isReady = True
                        break

if __name__ == '__main__':

    sbus = SBUSReceiver('/dev/ttyAMA0')

    while True:
        time.sleep(0.005)

        # X8R的SBUS信號是間隔6ms發(fā)送一次蝌戒，一次持續(xù)發(fā)送3ms串塑；
        # 不要調(diào)用sbus.update()太快，如果sbus.ser.inWaiting()>50,且增長很多北苟，可以調(diào)用sbus.update()快點桩匪，即time.sleep()延遲短點；
        # 如果sbus.ser.inWaiting()<50,可以調(diào)用sbus.update()慢點友鼻，即time.sleep()延遲長點傻昙；
        sbus.update()

        #在您的代碼中，您可以調(diào)用sbus.get_rx_channels()來獲取所有數(shù)據(jù)彩扔，或者調(diào)用sbus.get_rx_channels()[n]來獲取第n個通道的值妆档；
        #或get_rx_channel(self, num_ch)來獲得第num_ch個通道的值；
        print(sbus.get_failsafe_status(), sbus.get_rx_channels(), str(sbus.ser.inWaiting()).zfill(4) , (time.time()-sbus.lastFrameTime))
        #str() 函數(shù)將對象轉(zhuǎn)化為適于人閱讀的形式虫碉，將指定的值轉(zhuǎn)換為字符串贾惦。
        #zfill() 方法返回指定長度的字符串，原字符串右對齊敦捧，前面填充0须板。
        #(time.time()-sbus.lastFrameTime)用于展示得到最近這次數(shù)據(jù)的延遲

第5步： 編寫超聲波測距模塊，文件名為ultrasonic.py兢卵，與樹莓派基礎(chǔ)實驗24：超聲波測距傳感器實驗中的Python程序基本相同习瑰，只是設(shè)置了類，重構(gòu)了程序秽荤。
ultrasonic.py：

#!/usr/bin/env python3
#-*- coding: utf-8 -*-
#本模塊只含Ultrasonic()一個類甜奄，用于超聲波模塊測出車與障礙物的距離
#并能返回車與障礙物的距離宪躯，單位為cm

import RPi.GPIO as GPIO
import time
class Ultrasonic():
    
    TRIG = 7  #send-pin
    ECHO = 22  #receive-pin

    def setup(self):
        GPIO.setmode(GPIO.BOARD)
        GPIO.setup(Ultrasonic.TRIG, GPIO.OUT, initial = GPIO.LOW)
        GPIO.setup(Ultrasonic.ECHO, GPIO.IN)

    def distance(self):

        GPIO.output(Ultrasonic.TRIG, 1)  #給Trig一個10US以上的高電平
        time.sleep(0.00001)
        GPIO.output(Ultrasonic.TRIG, 0)

    #等待低電平結(jié)束泪姨，然后記錄時間
        while GPIO.input(Ultrasonic.ECHO) == 0:  #捕捉 echo 端輸出上升沿
            pass
        time1 = time.time()  
    
    #等待高電平結(jié)束杂穷，然后記錄時間
        while GPIO.input(Ultrasonic.ECHO) == 1:  #捕捉 echo 端輸出下降沿
            pass
        time2 = time.time() 

        during = time2 - time1   
    #ECHO高電平時刻時間減去低電平時刻時間，所得時間為超聲波傳播時間
        return during * 340 / 2 * 100  
    #超聲波傳播速度為340m/s,最后單位米換算為厘米雁乡，所以乘以100
 
    def destroy(self):
        GPIO.cleanup()

if __name__ == "__main__":
    
    try:
        ultr = Ultrasonic()
        ultr.setup()
        while True:
            dis = ultr.distance()
            print('distance= %f cm\n' %dis)
            time.sleep(0.3)
    except KeyboardInterrupt:
        ultr.destroy()

??第6步： 編寫樹莓派智能小車的主程序，文件名為smartcar.py糜俗，將這5個Python文件放入一個文件夾后踱稍，只運行本文件就可以了曲饱。
??主程序中加入了ultra_control()超聲波控制函數(shù)，實現(xiàn)了距離小于50cm時珠月，強制性降低油門值為35（即占空比為35%）扩淀，距離小于20cm時，自動剎車啤挎，并倒車0.5秒驻谆。
smartcar.py：

#!/usr/bin/env python
#-*- coding: utf-8 -*-
#本模塊為樹莓派小車的主程序，
from motor_4w import Motor_4w
from sbus_receiver_pi import SBUSReceiver
from moving_control import MovingControl
from ultrasonic import Ultrasonic
import time

acc_value_sbus_enable = 1 #使能是否執(zhí)行SBUS油門信號庆聘，為0時使遙控器油門信號無效

sbus_receiver = SBUSReceiver('/dev/ttyAMA0') #初始化SBUS信號接收實例
smp_car = Motor_4w() #初始化電機控制實例
smp_car.setGPIO()  #初始化引腳

ENA_pwm = smp_car.pwm(smp_car.ENA) #初始化使能信號PWM胜臊，A為左邊車輪
ENB_pwm = smp_car.pwm(smp_car.ENB) #初始化使能信號PWM，B為右邊車輪
    
smartcar = MovingControl(smp_car,ENA_pwm,ENB_pwm)  #初始化車輛運動控制實例

ultr = Ultrasonic() #初始化超聲波實例
ultr.setup()        #初始化超聲波引腳

def ultra_control():
    '''超聲波傳感器控制'''
    global acc_value_sbus_enable
    dis = ultr.distance()
    print('distance= %.1f cm\n' %dis)
    if dis < 50:
    #當(dāng)障礙距離小于50cm時伙判，不執(zhí)行SBUS油門信號
        if smartcar.acc_value > 35:
        #當(dāng)障礙距離小于50cm象对，且油門大于40時，油門設(shè)定為40宴抚，為防沖撞減速
            smartcar.acc_value = 35 
            smartcar.accelerator() #調(diào)節(jié)油門
    if dis < 20:
    #如果測距小于20cm時勒魔，先停車再倒車0.5秒
        smartcar.brake()
        smartcar.reverse()
        time.sleep(0.5)

def sbus_control():
    '''無線電控制'''
    sbus_receiver.update() #獲取一個完整的幀數(shù)據(jù)
    global acc_value_sbus_enable
            
    aileron_value = sbus_receiver.get_rx_channel(0) #1通道為副翼通道，這里控制車原地轉(zhuǎn)向
    elevator_value = sbus_receiver.get_rx_channel(1)#2通道為升降舵通道菇曲，這里控制車前進(jìn)后退
    rudder_value = sbus_receiver.get_rx_channel(3)#4通道為方向舵通道冠绢，這里控制車左右偏移行進(jìn)
    
    if acc_value_sbus_enable == 1:
    #使能SBUS信號的油門控制    
        acc_value_sbus = 172  #3通道，即油門的值羊娃，最低時為172        
        if sbus_receiver.get_rx_channel(2):
            acc_value_sbus = sbus_receiver.get_rx_channel(2) #3通道為油門通道唐全，這里控制車速度
        #將172~1811的油門通道值轉(zhuǎn)換為0~100的占空比信號，
        smartcar.acc_value = int(100*(acc_value_sbus-172)/(1811-172))
        print('000.acc_value=',smartcar.acc_value) #檢測SBUS信號的油門值
    #print('subsu_acc_value_sbus_enable=',acc_value_sbus_enable) #測試數(shù)據(jù)用
    ultra_control()    
    print('1111.acc_value=',smartcar.acc_value)  #檢測超聲波函數(shù)處理后的油門值
      
    if 970 < rudder_value < 1100: #沒有左右偏移時蕊玷，中間值為992邮利，但遙控器微調(diào)時會有上下浮動
        pass                    #沒有左右偏移時
    elif rudder_value >=1100:      #向右偏移行進(jìn)時
        rate_right = (1811.0 - rudder_value)/(1811-1100) 
        #最大值一般為1811，這里使用浮點類型垃帅，所以一定要使用1811.0
        smartcar.accelerator(1,rate_right)
    elif rudder_value <=970:       #向左偏移行進(jìn)時
        rate_left = (rudder_value - 172.0)/(970-172)  
        #最小值為172延届，這里使用浮點類型，所以一定要使用172.0
        smartcar.accelerator(rate_left,1)
    print('elevator_value=%d,aileron_value=%d,rudder_value=%d'%(elevator_value,aileron_value,rudder_value))#測試數(shù)據(jù)用
            
    if aileron_value >= 1100:  #原地左轉(zhuǎn)彎
        smartcar.leftTurn()
        smartcar.accelerator()
    elif aileron_value <= 970: #原地右轉(zhuǎn)彎
        smartcar.rightTurn()
        smartcar.accelerator()
    else : 
        smartcar.brake()       #停車
                
    if elevator_value >=1100:  #前進(jìn)
        smartcar.forward()
        
    elif elevator_value <=970: #后退
        smartcar.reverse()
        #循環(huán)最后贸诚，這里不能再用停車了    

try:    
    
    while True:
        time.sleep(0.005)
        sbus_control()

except KeyboardInterrupt:  
    smp_car.destroy(ENA_pwm,ENB_pwm)
finally:
    smp_car.destroy(ENA_pwm,ENB_pwm)

超聲波避障

這里將超聲波模塊加入了進(jìn)來方庭，但只做了簡單應(yīng)用，后面將把各種傳感器加入進(jìn)來后酱固，會實現(xiàn)超聲波模塊更復(fù)雜的應(yīng)用械念。

這個項目的代碼90%是我原創(chuàng)瞎寫的，有需要參考的同學(xué)可以下載：
樹莓派智能小車項目python源代碼下載