树莓派综合项目2：智能小车（五）红外避障

#!/usr/bin/env python3
#-*- coding: utf-8 -*-
#本模块只含Infrared()一个类，用于红外避障模块测出是否有障碍物
#有障碍物时返回值0，无障碍物时返回值1

import RPi.GPIO as GPIO
import time
class Infrared():
    
    InfraredPinLeft = 29  #左侧模块的输出连接树莓派29号针脚
    InfraredPinRight = 31

    def setup(self):
        GPIO.setmode(GPIO.BOARD)
        GPIO.setup(Infrared.InfraredPinLeft, GPIO.IN, pull_up_down=GPIO.PUD_UP)
        GPIO.setup(Infrared.InfraredPinRight, GPIO.IN, pull_up_down=GPIO.PUD_UP)

    def infra_detect(self):
        infra_left_value = 1
        infra_right_value = 1
        if (0 == GPIO.input(Infrared.InfraredPinLeft)):  #当检测到障碍物时，输出低电平信号
            infra_left_value = 0
        if (0 == GPIO.input(Infrared.InfraredPinRight)):  #当检测到障碍物时，输出低电平信号
            infra_right_value = 0
        return infra_left_value,infra_right_value
 
    def destroy(self):
        GPIO.cleanup()

if __name__ == "__main__":
    
    try:
        infrared = Infrared()
        infrared.setup()
        while True:
            infra_left_value,infra_right_value = infrared.infra_detect()
            print('infra_left_value=%d infra_right_value=%d'%(infra_left_value,infra_right_value) )
            time.sleep(0.1)
    except KeyboardInterrupt:
        infrared.destroy()

#!/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
from infrared import Infrared
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()        #初始化超声波引脚

infrared = Infrared()  #初始化红外避障实例
infrared.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 > 40:
        #当障碍距离小于50cm，且油门大于40时，油门设定为40，为防冲撞减速
            smartcar.acc_value = 40 
            smartcar.accelerator() #调节油门
    if dis < 10:
    #如果测距小于10cm时，先停车再倒车0.3秒
        smartcar.brake()
        smartcar.reverse()
        time.sleep(0.3)
        
def infra_control():
    '''红外避障传感器控制'''
    infra_left_value = 1       #值默认为1时，表示无障碍物
    infra_right_value = 1
    infra_left_value,infra_right_value = infrared.infra_detect()
    if infra_left_value == 0:  #值为0时，表示左侧检测到障碍物
        #smartcar.acc_value=80
        smartcar.accelerator(1,0)  #控制小车向右偏转
        time.sleep(0.5)            #向右偏转行进0.5秒
    if infra_right_value == 0:
        #smartcar.acc_value=80
        smartcar.accelerator(0,1)
        time.sleep(0.5)
    print('infra_left_value=%d infra_right_value=%d'%(infra_left_value,infra_right_value) )

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('original.acc_value=',smartcar.acc_value) #检测SBUS信号的油门值
        
    ultra_control()  #超声波避障
    infra_control()  #红外避障
       
    print('disposed.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.01)
        sbus_control()

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