树莓派综合项目2：智能小车（四）超声波避障
2020-12-21 本文已影响0人
Maker张
一、介绍
阅读本篇文章前建议先参考前期文章：
树莓派基础实验34：L298N模块驱动直流电机实验，学习了单个电机的简单驱动。
树莓派综合项目2：智能小车（一）四轮驱动，实现了代码输入对四个电机的简单控制。
树莓派综合项目2：智能小车（二）tkinter图形界面控制，实现了本地图形界面控制小车的前进后退、转向和原地转圈。
树莓派综合项目2：智能小车（三）无线电遥控，实现了无线电遥控设备控制小车的前进后退、转向和原地转圈。
本实验中将使用HC-SR04超声波传感器实时感知小车前方障碍物的距离，当距离近于某个阈值时，小车自动减速，再低于某个阈值时自动刹车，然后倒车至安全距离。
其它基础内容可以参考文集：树莓派基础实验。
二、组件
★Raspberry Pi 3 B+全套*1
★睿思凯Frsky XM+ 迷你接收机*1
★电平反向器模块*1
★睿思凯Frsky Taranis X9D PLUS SE2019遥控器*1
★L298N扩展板模块*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超声波传感器
关于超声波传感器的基础知识请参见树莓派基础实验24：超声波测距传感器实验。
四、实验步骤
第1步： 连接电路。在树莓派综合项目2：智能小车（一）四轮驱动中的接线基础上，接入电平反向器、无线电接收机。
| 树莓派（name） | 树莓派（BOARD） | L298N小车扩展板 |
|---|---|---|
| GPIO.0 | 11 | ENA |
| GPIO.2 | 13 | IN1 |
| GPIO.3 | 15 | IN2 |
| GPIO.1 | 12 | ENB |
| GPIO.4 | 16 | IN3 |
| GPIO.5 | 18 | IN4 |
| GND | GND | 电池组供电负极 |
关于这里树莓派GND、L298N小车扩展板的电池组供电负极相连，是特殊情况下的情况，经测试发现：
如果树莓派用的是充电头供电，而L298N扩展板用的是电池组供电，这两个负极必须相连，否则马达不动。
如果树莓派用的是L298N扩展板接出来的5V供电，即两者同一个电源，则这里不用连接。
| L298N小车扩展板 | 电池组 | 树莓派 | 电压表头 | 马达 |
|---|---|---|---|---|
| 电池+（-） | 电池+（-） | |||
| 5V供电 | 电源接口 | |||
| +（-） | +（-） | |||
| T1（L后） | +（-） | |||
| T2（L前） | +（-） | |||
| T3（R前） | +（-） | |||
| T4（R后） | +（-） |
| 树莓派（name） | 树莓派（BOARD） | 电平反向器 | 无线电接收机 |
|---|---|---|---|
| A6 | SBUS_OUT | ||
| RXD | 10 | B6 | |
| 3.3V | 1 | VCC | |
| 0V | 9 | GND | |
| 5V | 2 | VCC | |
| 0V | 14 | GND |
这里也要注意，由于树莓派的GPIO只能接收3.3V的最高输入，所以电平反相器的电源也只能使用3.3V，若反向后接收的信号需要是5V，则电平反相器的电源就使用5V。
| 树莓派（name） | 树莓派（BOARD） | 超声波模块 |
|---|---|---|
| GPIO.6 | 22 | Echo |
| GPIO.7 | 7 | Trig |
| 5V | 5V | VCC |
| GND | GND | GND |
加装超声波模块小车电路图
加装超声波模块的小车
这里我将18650电池组换成了航模使用的格氏ACE锂电池（3S/11.1V/2200MAH/40C），电压更高，能给树莓派提供更稳定的电源，动力性也更好，效果非常不错。
第2步： 编写电机的驱动程序，文件名为motor_4w.py。与树莓派综合项目2：智能小车（一）四轮驱动中的程序基本相同。
该车的行进控制与履带车的行进控制类似：
前进和后退很简单，左右两边的方向都朝前或朝后，速度一致；
原地顺时针旋转时，左边轮子前进，右边轮子后退，速度一致；
原地逆时针旋转时，左边轮子后退，右边轮子前进，速度一致；
偏左前进时，左右两边的方向都朝前，左轮速度比右轮速度慢一点；
偏右前进时，左右两边的方向都朝前，左轮速度比右轮速度快一点；
偏左后退时，左右两边的方向都朝后，左轮速度比右轮速度慢一点；
偏右后退时，左右两边的方向都朝后，左轮速度比右轮速度快一点；
下面的部分程序在前面的文章中已有，这里做了部分优化，主要的超声波模块程序，和主程序在最后面。
motor_4w.py：
#!/usr/bin/env python
#-*- coding: utf-8 -*-
#本模块只含Motor_4w()一个类，用于树莓派对电机信号的控制
#通过GPIO输出信号，直接对某个电机的转动方向、速度进行控制
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) #关闭警告
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（脉宽调制），返回PWM对象'''
EN_pwm = GPIO.PWM(pwm, 500)
EN_pwm.start(0)
return EN_pwm
def changespeed(self,pwm,speed):
'''通过改变占空比改变马达转速'''
pwm.ChangeDutyCycle(speed)
#print('speed=',speed) #测试数据用
def clockwise(self,in1_pin,in2_pin):
'''马达顺时针转的信号'''
GPIO.output(in1_pin, 1)
GPIO.output(in2_pin, 0)
def counter_clockwise(self,in1_pin,in2_pin):
'''马达逆时针转的信号'''
GPIO.output(in1_pin, 0)
GPIO.output(in2_pin, 1)
def stop_car(self,in1_pin,in2_pin):
'''马达制动的信号'''
GPIO.output(in1_pin, 0)
GPIO.output(in2_pin, 0)
#使能信号为低电平，或者高电平（占空比设为100，IN1和IN2都为0或1时）马达制动
def destroy(self,A,B):
'''结束程序时清空GPIO状态'''
A.stop()
B.stop()
GPIO.cleanup() # Release resource
if __name__ == '__main__': # 本模块单独测试时运行使用
try:
motor_4w = Motor_4w() #创建树莓派小车对象
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:
'''通过输入的命令改变马达转动'''
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，马达制动
motor_4w.stop_car(motor_4w.IN1,motor_4w.IN2)
motor_4w.stop_car(motor_4w.IN3,motor_4w.IN4)
continue #跳出本次循环
# 通过输入的两位数字设置占空比，改变马达转速
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步： 小车行进控制模块，文件名为moving_control.py，设计小车的移动行为，方向控制分为：原地左转弯、原地右转弯、直线前进、直线后退、刹车，而向前左偏移开进或右偏移开进等，由左右两边不同的速度（油门）来控制。
moving_control.py:
#-*- coding: utf-8 -*-
#本模块只含MovingControl()一个类，针对遥控器的控制方式，设计小车的移动行为
#方向控制分为：原地左转弯、原地右转弯、直线前进、直线后退、刹车
#而向前左偏移开进或右偏移开进等，由左右两边不同的速度（油门）来控制
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为向左偏移行进时，降低左边轮的速度
#rate_right为向右偏移行进时，降低右边轮的速度
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):
'''原地左转弯'''
self.smpcar.counter_clockwise(self.smpcar.IN1,self.smpcar.IN2) #左边车轮后退
self.smpcar.clockwise(self.smpcar.IN3,self.smpcar.IN4) #右边车轮前进
def rightTurn(self):
'''原地右转弯'''
self.smpcar.clockwise(self.smpcar.IN1,self.smpcar.IN2)
self.smpcar.counter_clockwise(self.smpcar.IN3,self.smpcar.IN4)
def forward(self):
'''直线前进'''
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，与树莓派基础实验39：解析无线电接收机PWM、SBUS信号中的Python2程序有所不同，下面的程序在Python3中运行，并标注了两者的不同之处。
sbus_receiver_pi.py：
#!/usr/bin/env python
#-*- coding: utf-8 -*-
#本模块只含SBUSReceiver()一个类，用于获取和解析遥控器接收机的SBUS输出信号
#并能返回每个通道的数值，遥控器的failsafe信号状态，每次获得数据帧的长度和这次数据的延迟时间
#import array #array模块是python中实现的一种高效的数组存储类型
import serial #serial模块封装了对串行端口的访问
#import binascii #python2运行时需要，binascii模块包含很多在二进制和ASCII编码的二进制表示转换的方法
#import codecs #python2运行时需要，Python中专门用作编码转换的模块
import time
class SBUSReceiver():
def __init__(self, _uart_port='/dev/ttyAMA0'):
#初始化树莓派串口参数
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个数据位
timeout = 0,
)
# 常数
#这里注意：Python2 与Python3 的编码方式是不同的
#self.START_BYTE = b'\x0f' #python2运行时用这句，起始字节为0x0f
#self.END_BYTE = b'\x00' #python2运行时用这句，结束字节为0x00
self.START_BYTE = 0x0f #python3运行时用这句，起始字节为0x0f
self.END_BYTE = 0x00 #python3运行时用这句，结束字节为0x00
self.SBUS_FRAME_LEN = 25 #SBUS帧有25个字节
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) # 用于同步的单个字节
#bytearray(n) 方法返回一个长度为n的初始化数组；
'''这里Python2与Python3存储数据的编码格式会不同'''
self.sbusFrame = bytearray(25) # 单个SBUS数据帧，25个字节
# 接收到的各频道值，Python2中使用数组
#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:初始化器，若数组为空，则省略初始化器
self.failSafeStatus = self.SBUS_SIGNAL_FAILSAFE
def get_rx_channels(self):
"""
用于读取最后的SBUS通道值
返回:由18个无符号短元素组成的数组，包含16个标准通道值+ 2个数字(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状态
返回: FAILSAFE状态值
"""
return self.failSafeStatus
def decode_frame(self):
"""
对每帧数据进行解码，每个通道的值在两个或三个不同的字节之间，要读取出来很麻烦
不过futaba已经发布了下面的解码代码
"""
def toInt(_from):
#encode() 方法以指定的编码格式编码字符串。
#int() 函数用于将一个字符串或数字转换为整型。
#return int(codecs.encode(_from, 'hex'), 16) #Python2中要使用这句，转换编码格式
return _from #Python3中要使用这句，即不用转换编码格式
#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字节的最低一位
if toInt(self.sbusFrame[23]) & 0x0001 :
self.sbusChannels[16] = 2047
else:
self.sbusChannels[16] = 0
#18频道，第24字节的低第二位，所以要右移一位
if (toInt(self.sbusFrame[23]) >> 1) & 0x0001 :
self.sbusChannels[17] = 2047
else:
self.sbusChannels[17] = 0
#帧丢失位为1时，第24字节的低第三位，与0x04进行与运算
self.failSafeStatus = self.SBUS_SIGNAL_OK
if toInt(self.sbusFrame[23]) & (1 << 2):
self.failSafeStatus = self.SBUS_SIGNAL_LOST
#故障保护激活位为1时，第24字节的低第四位，与0x08进行与运算
if toInt(self.sbusFrame[23]) & (1 << 3):
self.failSafeStatus = self.SBUS_SIGNAL_FAILSAFE
def update(self):
"""
我们需要至少2帧大小，以确保找到一个完整的帧
所以我们取出所有的缓存（清空它），读取全部数据，直到捕获新的数据
首先找到END BYTE并向后查找SBUS_FRAME_LEN，看看它是否是START BYTE
"""
#我们是否有足够的数据在缓冲区和有没有线程在后台?
if self.ser.inWaiting() >= self.SBUS_FRAME_LEN*2 and self.isReady: #inWaiting()返回接收缓存中的字节数
self.isReady = False #表明有线程在运行，isReady = False
# 读取所有临时帧数据
tempFrame = self.ser.read(self.ser.inWaiting())
# 在缓冲区帧的每个字符中，我们寻找结束字节
for end in range(0, self.SBUS_FRAME_LEN):
#寻找结束字节，从后向前查找
if tempFrame[len(tempFrame)-1-end] == self.END_BYTE :
#从最后的命中点减去SBUS_FRAME_LEN寻找起始字节
if tempFrame[len(tempFrame)-end-self.SBUS_FRAME_LEN] == self.START_BYTE :
# 如果相等，则帧数据正确，数据以8E2包到达，因此它已经被校验过
# 从临时帧数据中取出刚验证正确的一段正确帧数据
lastUpdate = tempFrame[len(tempFrame)-end-self.SBUS_FRAME_LEN:len(tempFrame)-1-end]
if not self.sbusFrame == lastUpdate: #相等即表示没有操作，不用再次解码
self.sbusFrame = lastUpdate
self.decode_frame() #调用解码函数
self.lastFrameTime = time.time() # 跟踪最近的更新时间
self.isReady = True
break
if __name__ == '__main__':
sbus = SBUSReceiver('/dev/ttyAMA0')
while True:
time.sleep(0.005)
# X8R的SBUS信号是间隔6ms发送一次，一次持续发送3ms；
# 不要调用sbus.update()太快，如果sbus.ser.inWaiting()>50,且增长很多，可以调用sbus.update()快点，即time.sleep()延迟短点；
# 如果sbus.ser.inWaiting()<50,可以调用sbus.update()慢点，即time.sleep()延迟长点；
sbus.update()
#在您的代码中，您可以调用sbus.get_rx_channels()来获取所有数据，或者调用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() 函数将对象转化为适于人阅读的形式，将指定的值转换为字符串。
#zfill() 方法返回指定长度的字符串，原字符串右对齐，前面填充0。
#(time.time()-sbus.lastFrameTime)用于展示得到最近这次数据的延迟
第5步： 编写超声波测距模块，文件名为ultrasonic.py，与树莓派基础实验24：超声波测距传感器实验中的Python程序基本相同，只是设置了类，重构了程序。
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)
#等待低电平结束，然后记录时间
while GPIO.input(Ultrasonic.ECHO) == 0: #捕捉 echo 端输出上升沿
pass
time1 = time.time()
#等待高电平结束，然后记录时间
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()超声波控制函数，实现了距离小于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 #使能是否执行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:
#当障碍距离小于50cm时，不执行SBUS油门信号
if smartcar.acc_value > 35:
#当障碍距离小于50cm，且油门大于40时，油门设定为40，为防冲撞减速
smartcar.acc_value = 35
smartcar.accelerator() #调节油门
if dis < 20:
#如果测距小于20cm时，先停车再倒车0.5秒
smartcar.brake()
smartcar.reverse()
time.sleep(0.5)
def sbus_control():
'''无线电控制'''
sbus_receiver.update() #获取一个完整的帧数据
global acc_value_sbus_enable
aileron_value = sbus_receiver.get_rx_channel(0) #1通道为副翼通道，这里控制车原地转向
elevator_value = sbus_receiver.get_rx_channel(1)#2通道为升降舵通道，这里控制车前进后退
rudder_value = sbus_receiver.get_rx_channel(3)#4通道为方向舵通道，这里控制车左右偏移行进
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的油门通道值转换为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) #测试数据用
ultra_control()
print('1111.acc_value=',smartcar.acc_value) #检测超声波函数处理后的油门值
if 970 < rudder_value < 1100: #没有左右偏移时，中间值为992，但遥控器微调时会有上下浮动
pass #没有左右偏移时
elif rudder_value >=1100: #向右偏移行进时
rate_right = (1811.0 - rudder_value)/(1811-1100)
#最大值一般为1811，这里使用浮点类型，所以一定要使用1811.0
smartcar.accelerator(1,rate_right)
elif rudder_value <=970: #向左偏移行进时
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))#测试数据用
if aileron_value >= 1100: #原地左转弯
smartcar.leftTurn()
smartcar.accelerator()
elif aileron_value <= 970: #原地右转弯
smartcar.rightTurn()
smartcar.accelerator()
else :
smartcar.brake() #停车
if elevator_value >=1100: #前进
smartcar.forward()
elif elevator_value <=970: #后退
smartcar.reverse()
#循环最后，这里不能再用停车了
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)
超声波避障
这里将超声波模块加入了进来，但只做了简单应用，后面将把各种传感器加入进来后，会实现超声波模块更复杂的应用。
