python控制树莓派小车 树莓派控制小车代码

#coding=utf-8
import RPi.GPIO as GPIO
import time
from Tkinter import *
from PIL import Image,ImageTk
import tkFileDialog as filedialog
import cv2
import threading

#定义电机模块的GPIO口
PWMA=18 #调速端A(左)
IN1=11 #左前
IN2=12 #左后
PWMB=16 #调速端B(右)
IN3=13 #右前
IN4=15 #右后
#定义超声波模块的GPIO口
Trig=35 #发射端
Echo=37 #接收端
#定义红外循迹传感器GPIO口
LSenso=29
RSenso=31
#定义红外避障传感器GPIO口
L_Senso=40
R_Senso=36

def init():
    #设置接触警告
    GPIO.setwarnings(False)
    #设置引脚模式为物理模式
    GPIO.setmode(GPIO.BOARD)
    #对四个电机进行初始化,将引脚设置为输出
    GPIO.setup(IN1,GPIO.OUT)
    GPIO.setup(IN2,GPIO.OUT)
    GPIO.setup(IN3,GPIO.OUT)
    GPIO.setup(IN4,GPIO.OUT)
    #调速端的初始化
    GPIO.setup(PWMA,GPIO.OUT)
    GPIO.setup(PWMB,GPIO.OUT)
    #超声波传感器引脚初始化
    GPIO.setup(Trig,GPIO.OUT) #将发射端引脚设置为输出
    GPIO.setup(Echo,GPIO.IN) #将接收端引脚设置为输入
    #红外循迹传感器引脚初始化,设置为输入，接受红外信号
    GPIO.setup(LSenso,GPIO.IN)
    GPIO.setup(RSenso,GPIO.IN)
    #红外避障传感器引脚初始化，设置为输入，接受红外信号
    GPIO.setup(L_Senso,GPIO.IN)
    GPIO.setup(R_Senso,GPIO.IN)

#让小车前进
def turn_up(speed,t_time):
    L_Motor.ChangeDutyCycle(speed)
    GPIO.output(IN1,GPIO.HIGH)
    GPIO.output(IN2,GPIO.LOW)

    R_Motor.ChangeDutyCycle(speed)
    GPIO.output(IN3,GPIO.HIGH)
    GPIO.output(IN4,GPIO.LOW)
    time.sleep(t_time)
    
#让小车后退
def turn_back(speed,t_time):
    L_Motor.ChangeDutyCycle(speed)
    GPIO.output(IN1,GPIO.LOW)
    GPIO.output(IN2,GPIO.HIGH)

    R_Motor.ChangeDutyCycle(speed)
    GPIO.output(IN3,GPIO.LOW)
    GPIO.output(IN4,GPIO.HIGH)
    time.sleep(t_time)

#让小车向左转
def turn_left(speed,t_time):
    L_Motor.ChangeDutyCycle(speed)
    GPIO.output(IN1,False)
    GPIO.output(IN2,False)

    R_Motor.ChangeDutyCycle(speed)
    GPIO.output(IN3,GPIO.HIGH)
    GPIO.output(IN4,GPIO.LOW)
    time.sleep(t_time)

#让小车向右转
def turn_right(speed,t_time):
    L_Motor.ChangeDutyCycle(speed)
    GPIO.output(IN1,GPIO.HIGH)
    GPIO.output(IN2,GPIO.LOW)

    R_Motor.ChangeDutyCycle(speed)
    GPIO.output(IN3,False)
    GPIO.output(IN4,False)
    time.sleep(t_time)

#让小车停止
def car_stop():
    L_Motor.ChangeDutyCycle(0)
    GPIO.output(IN1,False)
    GPIO.output(IN2,False)

    L_Motor.ChangeDutyCycle(0)
    GPIO.output(IN3,False)
    GPIO.output(IN4,False)

#超声波测距函数
def get_distance():
    GPIO.output(Trig,GPIO.HIGH) #给Trig发送高电平，发出触发信号
    time.sleep(0.00015) #需要至少10us的高电平信号，触发Trig测距
    GPIO.output(Trig,GPIO.LOW)
    while GPIO.input(Echo)!=GPIO.HIGH: #等待接收高电平
        pass
    t1=time.time() #记录信号发出的时间
    while GPIO.input(Echo)==GPIO.HIGH: #接收端还没接收到信号变成低电平就循环等待（等高电平结束）
        pass
    t2=time.time() #记录接收到反馈信号的时间
    distance=(t2-t1)*340*100/2 #计算距离，单位换成cm
    return distance


        
#避障功能函数（超声波避障结合红外避障）
def bizhang():
    safe_dis=40 #设置一个安全距离
    while True:
        barrier_dis=get_distance() #获取当前障碍物的距离
        #当测得前方障碍物距离小于安全距离时，先让小车停止
        if (barrier_dis < safe_dis) == True:
            while (barrier_dis < safe_dis) == True:
                L_S=GPIO.input(L_Senso)
                R_S=GPIO.input(R_Senso) #接受红外避障传感器的信号
                #如果红外传感器检测到左边有障碍物，右边没有，小车向右转
                if L_S == False and R_S == True:
                    print "左有障碍物先后退再右转"
                    turn_back(18,0.5)
                    turn_right(18,0.2)
                #如果红外传感器检测到右边有障碍物，左边没有，小车向左转
                if L_S == True and R_S == False:
                    print "右有障碍物先后退再左转"
                    turn_back(18,0.5)
                    turn_left(18,0.2)
                #如果红外传感器检测到左右两边都有障碍物，小车后退
                if L_S ==False and R_S == False:
                    print "两边都有障碍物后退"
                    turn_back(18,0.5)
                    #再次接收红外信号（说明刚才的路线已经不能再走，退到一定程度，小车就得左转或者右转，提前寻找新的路线）
                    L_S=GPIO.input(L_Senso)
                    R_S=GPIO.input(R_Senso)
                    #退到左前和右前都没有障碍物的位置
                    if L_S == True and R_S == True:
                        print "右转"
                        turn_right(18,0.2)
                    #退到了右前没有障碍物的位置
                    if L_S == False and R_S == True:
                        print "右转"
                        turn_right(18,0.2)
                    #退到了左前没有障碍物的位置
                    if L_S == True and R_S == False:
                        print "左转"
                        turn_left(18,0.2)
                    #还没有退出刚才的死路，继续后退

                    if L_S == False and R_S == False:
                        print "后退"
                        turn_back(18,0.5)
                #如果红外传感器检测到两边都没有障碍物，此时让小车后退一点，然后向右转
                if L_S == True and R_S == True:

                    print "两边都没有障碍物，后退再右转"
                    turn_back(18,0.5)
                    turn_right(18,0.2)
                print barrier_dis,'cm'
                print ''
                barrier_dis=get_distance()
        else:
            #小车在安全区里内，但是由于超声波传感器无法检测到除了正前方其他方向的障碍物，所以在此接收红外信号，通过左前和右前有没有障碍物，来判断小车该怎样运行
            L_S=GPIO.input(L_Senso)
            R_S=GPIO.input(R_Senso) #接受红外避障传感器的信号
            #在安全距离内同时红外传感器检测到左前和有前方没有障碍物，小车前进
            if L_S == True and R_S == True:
                print "前方40cm内没有障碍物，且左前和右前方没有障碍物，前进"
                turn_up(18,0)
            #在安全距离内，但左前有障碍物，让小车后退，再右转
            if L_S == False and R_S == True:
                print "前方40cm内没有障碍物，左前有障碍物，右前方没有障碍物，后退右转"
                turn_back(18,0.5)
                turn_right(18,0.2)
            #在安全距离内，但右前有障碍物，让小车后退，再左转
            if L_S == True and R_S == False:
                print "前方40cm内没有障碍物，右前有障碍物，左前方没有障碍物，后退右转"
                turn_back(18,0.5)
                turn_left(18,0.2)
            #在安全距离内，但左前，右前都有障碍物，让小车后退
            if L_S == False and R_S == False:
                print "前方40cm内没有障碍物，左前,右前方都有障碍物，后退"
                turn_back(18,0.5)
                #再次接收红外信号（说明刚才的路线已经不能再走，退到一定程度，小车就得左转或者右转，提前寻找新的路线）
                L_S=GPIO.input(L_Senso)
                R_S=GPIO.input(R_Senso)
                #退到左前和右前都没有障碍物的位置
                if L_S == True and R_S == True:
                    print "右转"
                    turn_right(18,0.2)
                #退到了右前没有障碍物的位置
                if L_S == False and R_S == True:
                    print "右转"
                    turn_right(18,0.2)
                #退到了左前没有障碍物的位置

                if L_S == True and R_S == False:
                    print "左转"
                    turn_left(18,0.2)
                #还没有退出刚才的死路，继续后退
                if L_S == False and R_S == False:
                    print "后退"
                    turn_back(18,0.5)
        print barrier_dis,'cm'
        print ''


#红外循迹函数
def track():
    while True:
        #接收两个红外传感器的信号
        LS=GPIO.input(LSenso)
        RS=GPIO.input(RSenso)
        #左右两个传感器都检测到黑色，小车在赛道上，前进
        if LS==True and RS==True:
            print "前进"
            turn_up(16,0.1)
        #左边的传感器没检测到黑色，说明小车车身偏离赛道靠左，右转将小车车身向右调整
        elif LS==False and RS==True:
            print "右转"
            turn_right(18,0.1)
        #右边的传感器没检测到黑色，说明小车车身偏离赛道靠右，左转将小车车身向左调整
        elif LS==True and RS==False:
            print "左转"
            turn_left(18,0.1)
        #两个传感器都没有检测到黑色，说明小车完全偏离赛道，停止
        else:
            print "停止"
            car_stop()

#清除GPIO占用
def gpio_clean():
    print "清除GPIO占用"
    GPIO.cleanup()
            

#创建线程执行避障函数
def Thread_bizhang():
    thread_bizhang=threading.Thread(target=bizhang)
    thread_bizhang.start()

#创建线程执行循迹函数
def Thread_track():
    thread_track=threading.Thread(target=track)
    thread_track.start()

#创建线程执行清理GPIO函数
def Thread_gpio_clean():
    Thread_gpio_clean=threading.Thread(target=gpio_clean)
    Thread_gpio_clean.start()
    
#控制界面函数
def ConInterface():
    #播放本地视频的函数
    def playLocalVideo():
        moviePath=filedialog.askopenfilename() #文件对话框，用来打开文件夹，选择文件
        print(moviePath)
        playBtn.place_forget() #隐藏播放按钮
        movie=cv2.VideoCapture(moviePath)
        waitTime= 1000/movie.get(5) #获取每帧需要的时间
        print(waitTime)
        movieTime = (movie.get(7) / movie.get(5))/60 #用视频文件的总帧数除以视频文件的帧速率得到视频的总时长，并换算成分钟
        print(movieTime)
        playSc.config(to=movieTime)
        while movie.isOpened():
            ret,readyFrame=movie.read() #read()返回两个值，ret 表示帧是否为空，readyFrame表示当前帧
            if ret==True:
                movieFrame=cv2.cvtColor(readyFrame,cv2.COLOR_BGR2RGBA) #校正颜色
                newImage=Image.fromarray(movieFrame).resize((675,360)) #调整大小
                newCover=ImageTk.PhotoImage(image=newImage)
                video.configure(image=newCover)  #将图像加入到video标签中
                video.image=newCover
                video.update()
                cv2.waitKey(int(waitTime)) #播放完每帧等待的时间
            else:
                break

    #使用opencv调用树莓派摄像头
    def playVideo():
        playBtn.place_forget()  #点击播放按钮之后开始播放视频，隐藏按钮
        movie=cv2.VideoCapture(0)  #打开摄像头
        movie_fps=movie.get(5)  #得到画面的帧速率
        print(movie_fps)
        waitTime=int(1000/movie_fps)
        print(waitTime)
        while movie.isOpened():
            #读取帧，返回布尔值和当前帧，ret 表示帧是否为空，readyFrame表示当前帧
            ret,readyFrame=movie.read()
            if ret==True:
                #校正图像颜色
                movieFrame=cv2.cvtColor(readyFrame,cv2.COLOR_BGR2RGBA)
                #设置图像大小
                newImage=Image.fromarray(movieFrame).resize((675,360))
                #将照片设置成和tkinter兼容的图像
                newCover=ImageTk.PhotoImage(image=newImage)
                #将图像放入Label中
                video.configure(image=newCover)
                video.image=newCover
                video.update()
                cv2.waitKey(20)
            else:
                break
        movie.release()   #释放资源

    #设置线程执行播放视频程序
    def Thread_playVideo():
        thread_playVideo=threading.Thread(target=playVideo)
        thread_playVideo.start()
                
    
    root=Tk() #初始化tk
    root.title("控制界面")     #设置标题
    root.geometry("1080x720")  #设置窗口大小
    root['bg']="#333333"       #设置窗口背景颜色
    #root.iconbitmap('/home/pi/ZRcode/go.ico')
    root.resizable(width=False,height=False)    #设置窗口长和宽是否可以变化


    #分别设置三个Frame 模块用于存放Label,Button
    videof=Frame(root,height=360,width=675,cursor="cross")
    videof.place(x=199,y=0)

    f1=Frame(root,height=270,width=540,cursor="circle",bg="#333333")
    f1.place(x=269,y=359)

    f2=Frame(root,height=180,width=540,cursor="plus",bg="#333333")
    f2.place(x=269,y=629)
    
    #视频窗口
    movieImage=Image.open('/home/pi/ZRcode/raspberry.jpg').resize((675,360))
    cover=ImageTk.PhotoImage(image=movieImage)
    
    video=Label(videof,width=675,height=360,bd=0,bg="pink",image=cover)
    video.place(x=0,y=0)

    #播放按钮的布局
    iconImag=Image.open('/home/pi/ZRcode/play.ico').resize((32,32))
    icoBtn=ImageTk.PhotoImage(image=iconImag)
    playBtn=Button(videof,image=icoBtn,cursor="hand2",command=Thread_playVideo,relief="groove")
    playBtn.place(x=319,y=159)
    #playSc=Scale(videof,from_=0,to=90,length=675,orient=HORIZONTAL,resolution=0.2,showvalue=0,bd=0,cursor="hand2",troughcolor="white")
    #playSc.place(x=0,y=310)
    
    
    #控制按钮
    up=Button(f1,text="前进",command=lambda:turn_up(20,1),activeforeground="green",activebackground="yellow",height=1,width=4)
    up.place(x=267,y=39)
    left=Button(f1,text="左转",command=lambda:turn_left(20,1),activeforeground="green",activebackground="yellow",height=1,width=4)
    left.place(x=132,y=134)
    right=Button(f1,text="右转",command=lambda:turn_right(20,1),activeforeground="green",activebackground="yellow",height=1,width=4)
    right.place(x=412,y=134)
    back=Button(f1,text="后退",command=lambda:turn_back(20,1),activeforeground="green",activebackground="yellow",height=1,width=4)
    back.place(x=267,y=230)
    stop=Button(f1,text="停止",command=car_stop,activeforeground="green",activebackground="yellow",height=1,width=4)
    stop.place(x=267,y=134)
    xunji=Button(f2,text="循迹",command=Thread_track,activeforeground="green",activebackground="yellow",height=1,width=4)
    xunji.place(x=68,y=44)
    bz=Button(f2,text="避障",command=Thread_bizhang,activeforeground="green",activebackground="yellow",height=1,width=4)
    bz.place(x=461,y=44)
    over=Button(f2,text="OVER",command=Thread_gpio_clean,activeforeground="green",activebackground="yellow",height=1,width=6)
    over.place(x=263,y=44)

    root.mainloop()

#主函数
if __name__=="__main__":
    init()
    L_Motor=GPIO.PWM(PWMA,100)
    L_Motor.start(0)
    R_Motor=GPIO.PWM(PWMB,100)
    R_Motor.start(0)
    try:
        ConInterface()
    except KeyboardInterrupt:
        GPIO.cleanup()      #清除GPIO占用