智能车比赛总结（树莓派控制的智障小车）

忙忙碌碌两个星期，这次的智能车校内赛终于是落下了帷幕。不管结果如何，准备的过程才是真正的收获，尤其是对于一个第一次使用单片机（开发板）的小白来说，实实在在学到了很多东西。下面将对这次的比赛过程做一个小小的总结。

1、比赛要求

比赛要求参赛队每组设计一个自动或半自动的机器人，比赛过程中，从启动区出发进入迷宫，击倒靶标，再从出口驶出。决赛中机器人需要先获取裁判出示的随机打靶顺序，按顺序击倒靶标。

2、硬件准备

1）小车底盘加水弹枪云台。小车底盘我们使用的是麦轮的底盘，主要考虑在行动中可以自由旋转和平移。水弹枪云台使用两个自由度的云台控制（两个舵机分别控制它的平移角度和倾斜角度）

2）开发板。我们的是树莓派3B+开发板，主要是因为树莓派可以使用Python语言编写，编程小白容易上手。/偷笑

3）电机驱动板。我们使用了两块L293N电机驱动板控制四路电机。

4）舵机驱动板。舵机控制板至少需要同时控制两路舵机，满足此条件即可，我们是用的是以前学长自己画的控制板，这里就不在展示。
如果有能力自己画一块板子，把电机驱动和舵机驱动整合到一块板子上最好，这样可以节省很多空间。我没学过PCB也不会，所以只能自己东拼西凑找来一堆凑着用。/惭愧
5）水弹枪电机驱动板。水弹枪电机由于负载较大所以其功率也大，建议单独使用一块驱动板控制。我们使用的是MOS驱动模块。（主要是便宜 ）

6）超声波模块。超声波模块最好使用三个，前左右各一个，可以让小车灵活的避障和转弯。也有使用一个舵机加一个超声波判断的，个人感觉这种判断方式会占用不必要的时间。而且对于新手来说加大了编程的困难度，当然，大佬请忽略。/笑哭

7）摄像头。摄像头主要用来识别AprilTag标签，在打靶和走十字路口时用到。我们使用的是openMV，其一识别AprilTag标签的程序官方已经做的很好，可以直接使用，其二openMV也可以作为一块单片机使用，这样我们就相当于有了两块控制开发板，岂不妙哉。

8）VCC、GND面包板。有了VCC、GND面包板我们就可将电源和接地引出，一来可以扩展接口，二来方便接线。次面包板建议使用两块，一块用来走电源大于6V的电，一块走信号电，小于5V。
9）杜邦线若干。

3、物理接线

接线也是一件技术活，最考验细心和耐心。接线之前应该先把控制板的GPIO口定义完成，这样在接线的时候才能做到有条不紊，也更方便检查。
1）电机驱动
首先应该是电机线已经焊接到电机两端并且保证焊接牢靠。（哈哈哈，说的好像是废话）
四个电机分别接到驱动板的四个电机接口，正负可以不用太在意，后面可调，但是哪个电机接到哪个接口要记住，最好用笔记下来，正所谓好记性不如烂笔头，不然后面还得测，麻烦。ENA、ENB是电机的使能端，若想要使用PWM调速，则拔掉使能端与旁边引脚的跳线帽，将使能端接定义的IO口；若不用PWM调速，则插上跳线帽，使其始终高电平，因为其是高电平有效。若是其他驱动板没有始终高电平的引脚可以接到控制板的GPIO口，在程序中直接给高电平或者接到控制板的高电平输出，反正很多种办法使其在工作时保持高电平，看自己喜好。剩下的IN接控制板GPIO口，控制电机正反转。
驱动板电源接12V端口，GND接电源负极，电源大小视具体情况而定，我们接了8V电源，想接12V来着因为第一次弄总是不大胆，怕把电机给烧了（实际上已经少了一个，但是没给12V电压，具体原因至今没找到，怀疑是缠上了头发。/捂脸），其实完全可以接12V，我们小车的速度始终没上去，就是因为电压不够，后悔。
注意：驱动板和控制板一定要共地（也就是驱动板的GND要和控制板的GND接到一起），不然电机是动不起来的。
2）舵机驱动
舵机接线盒普通直流电机接线大差不差，舵机引出的是三根线，一根信号线，两根电源线。将其插到控制板的对应接口，然后控制板会有一根信号引脚将其接到控制板的GPIO口即可。电源可以使用和电机的同一个电源。
3）MOS模块
VIN,GND接电源，V+、V-接水弹枪电机，VCC、GND接控制板电源或者GPIO口自定义高低电平，SIG接GPIO口，输入的是PWM信号，控制水弹枪的射速。
4）树莓派与openMV通信物理接线
树莓派与openMV选用UART通信，树莓派的RX连接OpenMV 的TX，树莓派的TX连接OpenMV 的RX，树莓派的GND连接OpenMV 的GND。

4、软件控制

1）追踪AprilTag标签云台控制
云台使用两个舵机分别控制它的平移和倾斜角度，两个舵机的控制我们在openMV完成，一来可以使它不会与小车的动作冲突，二来openMV有类似的官方代码，修修改改就可以使用，何乐不为（其实还是因为自己菜，也在树莓派里面写过，效果不如在openMV完成）。云台使用PID控制，使其能够准确地瞄准标签中心。控制框图如下：

控制流程图：

打靶有一个要求，比赛前由裁判抽取打靶顺序，机器人应按照裁判抽取的顺序进行打靶。这里我们使用数组存入顺序然后按顺序抽取数组中的元素给小车的击打信号。
程序：
（云台控制加与树莓派通信，未包含射击部分）

import sensor, image, time

from pid import PID
from pyb import Servo
from pyb import UART

uart = UART(3,115200)
pan_servo=Servo(1)
tilt_servo=Servo(2)
pan_angle = 0
tilt_angle = 0


#pan_pid = PID(p=0.07, i=0, imax=90) #脱机运行或者禁用图像传输，使用这个PID
#tilt_pid = PID(p=0.05, i=0, imax=90) #脱机运行或者禁用图像传输，使用这个PID
pan_pid = PID(p=0.2, i=1.5, imax=90)#在线调试使用这个PID
tilt_pid = PID(p=0.22, i=0, imax=90)#在线调试使用这个PID

sensor.reset() # Initialize the camera sensor.
sensor.set_pixformat(sensor.RGB565) # use RGB565.
sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed.
sensor.skip_frames(10) # Let new settings take affect.
sensor.set_auto_whitebal(False) # turn this off.
clock = time.clock() # Tracks FPS.

def find_max(blobs):
    max_size=0
    for blob in blobs:
        if blob[2]*blob[3] > max_size:
            max_blob=blob
            max_size = blob[2]*blob[3]
    return max_blob


while(True):
    if uart.any():
        a = uart.readline().decode()
        b = int(a)
    clock.tick() # Track elapsed milliseconds between snapshots().
    img = sensor.snapshot() # Take a picture and return the image.

    blobs = img.find_apriltags()
    if blobs:
        max_blob = find_max(blobs)
        pan_error = max_blob.cx()-img.width()/2
        tilt_error = max_blob.cy()-img.height()/2

        #print("pan_error: ", pan_error)
        #print("tilt_error:", tilt_error)

        img.draw_rectangle(max_blob.rect()) # rect
        img.draw_cross(max_blob.cx(), max_blob.cy()) # cx, cy

        pan_output=pan_pid.get_pid(pan_error,1)/4
        tilt_output=tilt_pid.get_pid(tilt_error,1)
        print("pan_output",pan_output,"tilt_output",tilt_output)

        #pan_angle = pan_angle + pan_output
        #tilt_angle = tilt_angle - tilt_output
        pan_servo.angle(-pan_output)
        tilt_servo.angle(-tilt_output)
        #print("pan_angle",pan_angle,"tilt_angle",tilt_angle)
        output_str="%d,%d,%d" % (0,max_blob.id(),max_blob.id())
        # UART 3, and baudrate.
        print('you send:',output_str)
        uart.write(output_str)

    else:
        #pan_servo.angle(0)
        #tilt_servo.angle(0)
        output_str = '[%d]' % 0
        print('you send:',output_str)
        uart.write(output_str)

2）小车避障控制
在小车的前左右分别装上超声波，根据超声波测得的距离实现避障功能。控制流程图如下图所示。

超声波测距程序：

import RPi.GPIO as GPIO
import time

#超声波引脚
Trig_F = 16
Echo_F = 18

Trig_L = 13
Echo_L = 15

Trig_R = 11
Echo_R = 12

#设置引脚模式
GPIO.setmode(GPIO.BOARD)
GPIO.setwarnings(False)

GPIO.setup(Trig_F, GPIO.OUT)
GPIO.setup(Echo_F, GPIO.IN)

GPIO.setup(Trig_L, GPIO.OUT)
GPIO.setup(Echo_L, GPIO.IN)

GPIO.setup(Trig_R, GPIO.OUT)
GPIO.setup(Echo_R, GPIO.IN)

#前方距离
def Distance_Front():
    GPIO.output(Trig_F, 0)
    time.sleep(0.000002)
    GPIO.output(Trig_F, 1)
    time.sleep(0.00001)
    GPIO.output(Trig_F, 0)
    while GPIO.input(Echo_F) == 0:
        pass
    emitTime = time.time()
    while GPIO.input(Echo_F) == 1:
        pass
    acceptTime = time.time()
    totalTime = acceptTime - emitTime
    distance = totalTime * 340 / 2 * 100
    print('front:',distance)
    time.sleep(0.06)
    return  distance

#左侧距离
def Distance_Left():
    GPIO.output(Trig_L, 0)
    time.sleep(0.000002)
    GPIO.output(Trig_L, 1)
    time.sleep(0.00001)
    GPIO.output(Trig_L, 0)
    while GPIO.input(Echo_L) == 0:
        pass
    emitTime = time.time()
    while GPIO.input(Echo_L) == 1:
        pass
    acceptTime = time.time()
    totalTime = acceptTime - emitTime
    distance = totalTime * 340 / 2 * 100
    print('left:',distance)
    time.sleep(0.06)
    return  distance

#右侧距离
def Distance_Right():
    GPIO.output(Trig_R, 0)
    time.sleep(0.000002)
    GPIO.output(Trig_R, 1)
    time.sleep(0.00001)
    GPIO.output(Trig_R, 0)
    while GPIO.input(Echo_R) == 0:
        pass
    emitTime = time.time()
    while GPIO.input(Echo_R) == 1:
        pass
    acceptTime = time.time()
    totalTime = acceptTime - emitTime
    distance = totalTime * 340 / 2 * 100
    time.sleep(0.06)
    print('right:',distance)
    return  distance

小车避障程序：

import RPi.GPIO as GPIO
import Ultrasonic as u
import time

#左前轮引脚设置
FrontLeft_0 = 35
FrontLeft_1 = 37
FrontLeft_en = 40
#右前轮引脚设置
FrontRight_0 = 31
FrontRight_1 = 33
FrontRight_en = 38
#左后轮引脚设置
BackLeft_0 = 26
BackLeft_1 = 29
BackLeft_en =36
#右后轮引脚设置
BackRight_0 = 24
BackRight_1 = 22
BackRight_en = 32


GPIO.setmode(GPIO.BOARD)
GPIO.setwarnings(False)

GPIO.setup(FrontLeft_0,GPIO.OUT)
GPIO.setup(FrontLeft_1,GPIO.OUT)
GPIO.setup(FrontLeft_en,GPIO.OUT)
GPIO.setup(FrontRight_0,GPIO.OUT)
GPIO.setup(FrontRight_1,GPIO.OUT)
GPIO.setup(FrontRight_en,GPIO.OUT)
GPIO.setup(BackLeft_0,GPIO.OUT)
GPIO.setup(BackLeft_1,GPIO.OUT)
GPIO.setup(BackLeft_en,GPIO.OUT)
GPIO.setup(BackRight_0,GPIO.OUT)
GPIO.setup(BackRight_1,GPIO.OUT)
GPIO.setup(BackRight_en,GPIO.OUT)
    
#PWM设置
FrontLeft_pwm = GPIO.PWM(FrontLeft_en, 100)
FrontRight_pwm = GPIO.PWM(FrontRight_en, 100)
BackLeft_pwm = GPIO.PWM(BackLeft_en, 100)
BackRight_pwm = GPIO.PWM(BackRight_en, 100)

#设置初始速度
FrontLeft_pwm.start(0)
FrontRight_pwm.start(0)
BackLeft_pwm.start(0)
BackRight_pwm.start(0)

# 左前轮向前转
def front_left_forward(speed):
    FrontLeft_pwm.ChangeDutyCycle(speed)
    GPIO.output(FrontLeft_0,GPIO.HIGH)
    GPIO.output(FrontLeft_1,GPIO.LOW)
    #time.sleep(time)
    
# 左后轮向前转
def rear_left_forward(speed):
    BackLeft_pwm.ChangeDutyCycle(speed)
    GPIO.output(BackLeft_0,GPIO.HIGH)
    GPIO.output(BackLeft_1,GPIO.LOW)

# 右前轮向前转
def front_right_forward(speed):
    FrontRight_pwm.ChangeDutyCycle(speed)
    GPIO.output(FrontRight_0,GPIO.HIGH)
    GPIO.output(FrontRight_1,GPIO.LOW)

# 右后轮向前转
def rear_right_forward(speed):
    BackRight_pwm.ChangeDutyCycle(speed)
    GPIO.output(BackRight_0,GPIO.HIGH)
    GPIO.output(BackRight_1,GPIO.LOW)

# 左前轮向后转
def front_left_back(speed):
    FrontLeft_pwm.ChangeDutyCycle(speed)
    GPIO.output(FrontLeft_0,GPIO.LOW)
    GPIO.output(FrontLeft_1,GPIO.HIGH)
    
# 左后轮向后转
def rear_left_back(speed):
    BackLeft_pwm.ChangeDutyCycle(speed)
    GPIO.output(BackLeft_0,GPIO.LOW)
    GPIO.output(BackLeft_1,GPIO.HIGH)

# 右前轮向后转
def front_right_back(speed):
    FrontRight_pwm.ChangeDutyCycle(speed)
    GPIO.output(FrontRight_0,GPIO.LOW)
    GPIO.output(FrontRight_1,GPIO.HIGH)

# 右后轮向后转
def rear_right_back(speed):
    BackRight_pwm.ChangeDutyCycle(speed)
    GPIO.output(BackRight_0,GPIO.LOW)
    GPIO.output(BackRight_1,GPIO.HIGH)

# 前进，4个轮子全部向前转
def forward(speed_fl,speed_fr,speed_bl,speed_br):
    front_left_forward(speed_fl)
    front_right_forward(speed_fr)
    rear_left_forward(speed_bl)
    rear_right_forward(speed_br)
    
# 后退，4个轮子全部向后转
def backward(speed_fl,speed_fr,speed_bl,speed_br):
    front_left_back(speed_fl)
    front_right_back(speed_fr)
    rear_left_back(speed_bl)
    rear_right_back(speed_br)

# 左转
def turnleft(speed_fl,speed_fr,speed_bl,speed_br):
    front_left_back(speed_fl)
    rear_left_back(speed_bl)
    front_right_forward(speed_fr)
    rear_right_forward(speed_br)
    
# 右转
def turnright(speed_fl,speed_fr,speed_bl,speed_br):
    front_right_back(speed_fr)
    rear_right_back(speed_br)
    front_left_forward(speed_fl)
    rear_left_forward(speed_bl)
    
# 左平移
def left_translation(speed_fl,speed_fr,speed_bl,speed_br):
    front_left_back(speed_fl)
    rear_left_forward(speed_bl)
    front_right_forward(speed_fr)
    rear_right_back(speed_br)

# 右平移
def right_translation(speed_fl,speed_fr,speed_bl,speed_br):
    front_left_forward(speed_fl)
    rear_left_back(speed_bl)
    front_right_back(speed_fr)
    rear_right_forward(speed_br)    
                
    
def pause():
    FrontLeft_pwm.ChangeDutyCycle(0)
    GPIO.output(FrontLeft_0,GPIO.LOW)
    GPIO.output(FrontLeft_1,GPIO.LOW)
    #GPIO.output(ForntLeft_en,GPIO.LOW)
    FrontRight_pwm.ChangeDutyCycle(0)
    GPIO.output(FrontRight_0,GPIO.LOW)
    GPIO.output(FrontRight_1,GPIO.LOW)
    #GPIO.output(FrontRight_en,GPIO.LOW)
    BackLeft_pwm.ChangeDutyCycle(0)
    GPIO.output(BackLeft_0,GPIO.LOW)
    GPIO.output(BackLeft_1,GPIO.LOW)
    #GPIO.output(BackLeft_en,GPIO.LOW)
    BackRight_pwm.ChangeDutyCycle(0)
    GPIO.output(BackRight_0,GPIO.LOW)
    GPIO.output(BackRight_1,GPIO.LOW)
    #GPIO.output(BackRight_en,GPIO.LOW)

def stop():
    pause()
    GPIO.cleanup()
    
def run():
    distance_front = u.Distance_Front()
    distance_left = u.Distance_Left()
    distance_right = u.Distance_Right()
    #自动运行，避障
    if distance_front < 50:
        #backward(100,100,100,100)
        if distance_left > 60:
            turnleft(100,100100,100)
        elif distance_right > 60:
            turnright(100,100,100,100)
        else:
            backward(100,100,100,100)
    elif distance_left < 18:
        right_translation(100,100,100,100)
    elif distance_right < 18:
        left_translation(100,100,100,100)
    else:    
        forward(100,100,100,100)    

关于如何判断小车是否到达射击区域，比赛场地是有AprilTag标签提示的，发现标签停止，执行射击程序，此组射击完成执行运动程序。
3）树莓派与openMV通信
按照上述完成接线后进行通信程序编写
树莓派端

# -*- coding: utf-8 -*
import serial
import time

ser = serial.Serial("/dev/ttyAMA0",115200)

if not ser.isOpen():
    print("open failed")
else:
    print("open success: ")
    print(ser)

try:
    while True:
        count = ser.inWaiting()
        if count > 0:
            recv = ser.read(count)
            print("recv: " + recv)
            ser.write(recv)
        time.sleep(0.05) 
except KeyboardInterrupt:
    if ser != None:
        ser.close()

openMV端

# UART Control
#
# This example shows how to use the serial port on your OpenMV Cam. Attach pin
# P4 to the serial input of a serial LCD screen to see "Hello World!" printed
# on the serial LCD display.

import time
from pyb import UART

# Always pass UART 3 for the UART number for your OpenMV Cam.
# The second argument is the UART baud rate. For a more advanced UART control
# example see the BLE-Shield driver.
uart = UART(3, 115200)

while(True):
    uart.write("Hello World!\r")
    time.sleep(1000)

这里没想清楚通信程序怎么在子程序中完成，所以把通信写在了主程序中，若有好的可以写在子程序的想法，欢迎联系。
上述只是通信测试的程序。