树莓派综合项目2：智能小车（六）黑线循迹

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

import RPi.GPIO as GPIO
import time
class Tracking():
    
    trackingPinLeft = 33  #左侧模块的输出连接树莓派33号针脚
    trackingPinRight = 35

    def setup(self):
        GPIO.setmode(GPIO.BOARD)
        GPIO.setup(Tracking.trackingPinLeft, GPIO.IN, pull_up_down=GPIO.PUD_UP)
        GPIO.setup(Tracking.trackingPinRight, GPIO.IN, pull_up_down=GPIO.PUD_UP)

    def tracking_detect(self):
        tracking_left_value = 1
        tracking_right_value = 1
        if (0 == GPIO.input(Tracking.trackingPinLeft)):  # 当检测到红外反射，即白色地面时，输出低电平信号
            tracking_left_value = 0
        if (0 == GPIO.input(Tracking.trackingPinRight)):  #当检测到红外反射时，即白色地面时，输出低电平信号
            tracking_right_value = 0
        return tracking_left_value,tracking_right_value
 
    def destroy(self):
        GPIO.cleanup()

if __name__ == "__main__":
    
    try:
        tracking = Tracking()
        tracking.setup()
        while True:
            tracking_left_value,tracking_right_value = tracking.tracking_detect()
            print('tracking_left_value=%d tracking_right_value=%d'%(tracking_left_value,tracking_right_value) )
            #time.sleep(0.1)
    except KeyboardInterrupt:
        tracking.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
from tracking import Tracking
import time
import threading

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()       # 初始化红外避障引脚

tracking = Tracking()
tracking.setup()


def ultra_control():
    """超声波传感器控制"""
    # 如果测距小于10cm时，先停车再倒车0.3秒
    smartcar.brake()
    smartcar.reverse()
    time.sleep(0.3)

        
def infra_control(infra_left_value,infra_right_value):
    """红外避障传感器控制"""
    if infra_left_value == 0:  # 值为0时，表示左侧检测到障碍物
        # smartcar.acc_value=80
        smartcar.accelerator(1, 0.5)  # 控制小车向右偏转
        time.sleep(0.5)             # 向右偏转行进0.5秒
    if infra_right_value == 0:
        # smartcar.acc_value=80
        smartcar.accelerator(0.5, 1)
        time.sleep(0.5)
    print('infra_left_value=%d infra_right_value=%d'%(infra_left_value,infra_right_value) )


exitFlag_tracking = 1    # 是否退出循迹while循环的标志，0为不退出
def tracking_control():
    """红外循迹传感器，控制小车沿黑线自动行进"""
    while True:
        time.sleep(0.5)   # 为节省计算资源，不让循环过于太快
        while not exitFlag_tracking:
            smartcar.acc_value = 40
            tracking_left_value,tracking_right_value = tracking.tracking_detect()
            if tracking_left_value == 0 and tracking_right_value == 0:  # 值都为0时，表示都检测到红外反射，直行
                smartcar.accelerator()  # 让油门左右一致
                smartcar.forward()      # 控制小车直线前进

            if tracking_left_value == 1 and tracking_right_value == 0:  # 左侧检测不到反射信号，即遇到黑线，车向左偏转
                smartcar.accelerator(0, 1)
                smartcar.forward()

            if tracking_right_value == 1 and tracking_left_value == 0:  # 右侧检测不到反射信号，即遇到黑线，车向右偏转
                smartcar.accelerator(1, 0)
                smartcar.forward()

            if tracking_left_value == 1 and tracking_right_value == 1:  # 值都为1时，表示都没有检测到红外反射，停车
                smartcar.brake()  # 控制小车停车

def sbus_control():
    """无线电控制"""
    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通道为方向舵通道，这里控制车左右偏移行进

    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信号的油门值
    smartcar.accelerator()
    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()       # 停车，这里用了else，后面不需要再停车了
                
    if elevator_value >= 1100:   # 前进
        smartcar.forward()
    elif elevator_value <= 970:  # 后退
        smartcar.reverse()
        # 循环最后，这里不能再用停车了


try:
    # 创建循迹控制的线程，让tracking_control()函数始终处于运行状态
    myThread_tracking = threading.Thread(target=tracking_control)
    myThread_tracking.start()  # 启动线程
    while True:
        sbus_receiver.update()  # 获取一个完整的帧数据
        tracking_value = sbus_receiver.get_rx_channel(4)  # 5通道为是否循迹的开关，打开后则按循迹自动行进
        # print('tracking_value=',tracking_value)
        if tracking_value > 1800:  # 当SA开关打到下档位置时，get_rx_channel(4)的值为1811
            exitFlag_tracking = 0  # 让循迹函数线程中的while循环开始，则由循迹函数线程控制小车行进
            
        else:
            exitFlag_tracking = 1  # 让循迹函数线程中的while循环结束，开始由遥控器和其它传感器控制小车行进
            dis = ultr.distance()
            # print('distance= %.1f cm\n' % dis)
            if dis < 10:
                ultra_control()  # 超声波避障
        
            infra_left_value,infra_right_value = infrared.infra_detect()    
            if infra_left_value == 0 or infra_right_value == 0:
                infra_control(infra_left_value,infra_right_value)

            sbus_control()  # 遥控器控制小车行进

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