机器人制作开源方案 | 智能循迹排爆小车

作者：陈豪、高鹏、宋晨博、王伟嘉、许广豪

单位：唐山学院

指导老师：袁娜

1. 引言

1.1 研究背景和发展现状

? ? ? 随着电子技术、计算机技术、智能控制技术的飞速发展，产品的智能化和小型化越来越成为人们关注的热点，智能小车也被称之为轮式机器人。我们知道机器人技术的发展是一个国家高科技水平和工业自动化程度的重要标志和体现。机器人由于具有高度的灵活性、可以帮助人们提高生产率、改进产品质量和改善劳动条件等优点，在世界各地的生产生活领域得到了广泛的应用。

? ? ? 智能小车正是模仿机器人的一种尝试。它是一种以汽车电子为背景，涵盖控制，模式识别，电子、电气、机械等多学科的科技创新性设计，一般主要由路径识别、速度采集、角度控制以及车速控制等模块组成。这种智能小车能够自动搜寻前进路线，还能爬坡；通过IIC颜色识别模块，能够有效的识别颜色进行针对性的排爆；加入相关声光讯号后，更能体现出智能化和人性化的一面。智能小车作为一种智能化的交通工具，综合了车辆工程、测控工程、人工智能等多个学科领域的理论技术，具有良好的民事和军用应用前景，尤其是固定场地搬运车和自动料车等技术领域。

1.2 研究目的及意义

? ? ? 其研究意义涵盖了工业、生活、勘探以及人类关注的探月工程。本项目旨在设计出一款可以自主按照人类预设的轨迹行走（或者完全自主行走）并完成指定任务的小车，例如智能运输系统。公共交通是城市发展的必然产物，也是城市赖以生存的重要基础设施之一，它作为城市动态大系统中一个重要组成部分，是城市整体发展中不可缺少的物质条件和基础产业，也是联系社会生产、流通和人民生活的纽带。具有运载量大、运送效率高、能源消耗低、相对污染少、运输成本低等优点。

1.3 研究内容

? ? ? 本作品是一种基于探索者BASRA主控板和Bigfish扩展板控制的自动循迹排爆小车系统，从设计的功能要求出发，包括小车机械构成设计和控制系统的软硬件设计。为了适应复杂地形，采用稳定性比较高的六轮构架式，利用灰度传感器对路面黑色轨迹进行检测，并确定小车当前的位置状态，再将路面检测信号反馈给主控板。主控板对采集到的信号予以分析判断，及时控制驱动电机以调整小车的定位和姿态，从而使小车能够沿着黑色轨迹自动行驶，实现小车自动循迹，进而实现排爆的目的。

2. 系统总体设计

2.1 总体设计与功能

? ? ? 从智能循迹排爆小车（如下图所示）的设计要求出发，在小车身上加上轨迹识别、舵机装置、颜色识别模块和马达驱动装置来完成整个功能。

? ? ? 智能小车采用：六轮驱动，循迹灰度传感器分别装在车头下的左中右。当车身下左边的传感器检测到黑线时，小车右转；当车身下右边传感器检测到黑线时，小车左转，直到小车完全回到黑线。如果三个灰度传感器同时检测到黑线时，小车直行；当小车第三次是三个灰度传感器同时检测到黑线时，小车停止，并识别色卡，之后对爆破物体进行颜色比对，实现排爆。信号流程图如下图所示：

2.2 执行机构

? ? ? 小车由探索者平台上的直流电机、轮子、双足脚、机械手指、双足连杆、输出支架、螺丝、轴套、57孔平板、6-42A舵机、711孔平板和23折弯、IIC颜色识别模块、灰度传感器等组成。

? ? ? 主控板部分：将主控板固定在711孔平板上。

? ? ? 驱动部分：采用原有的直流驱动电机，由六个直流电机驱动小车，实现六驱，其力矩完全可以达到模拟效果，使其越障性能更加出色，同时可加入履带片来增加驱动轮的抓地力，减小轮子空转所引起的误差。
? ? ? 电池部分：将锂电池放置在车体的上面，提高了底盘的灵活性和稳定性。

? ? ? 舵机部分：将6-42A舵机外套上马达支架，固定在711孔平板的右侧。

? ? ? 传感器部分：将三个灰度传感器由双足支杆通过轴套悬挂到前两个轮子的内侧，IIC颜色识别模块通过马达支架固定在舵机外侧，方便检测爆破物体颜色信息。

2.3 接线设计

? ? ? ① 灰度传感器和电机接线：

② 舵机接线：

?车头舵机:3

?气球舵机:8

?车尾舵机:12

③ 颜色传感器接线：

?TCS3473x ?Arduino_Uno

?SDA ?----- ?A4

?SCL ?----- ?A5

?VIN ?----- ?5V

?GND ?----- ?GND

3. 硬件设计

3.1 执行机构硬件

? ? ? 对于小车的机械系统稳定、灵活、简单的考虑，选用六轮式，考虑到六轮车具有更稳定灵活的特点，使设计更贴近大环境需求。

? ? ? 本项目（如图1）是在月球车底盘（如图2）的基础上，在四个前后轮上缠绕了履带片以增大轮径，提高越障的性能。另外在车身上增加了一个舵机驱动的摆杆，摆杆的顶部可以安装针甚至排针，用来刺破气球。

3.2 BASRA主控板和Bigfish扩展板控制系统硬件

? ? ? 本项目是一种基于探索者BASRA主控板和Bigfish扩展板控制的自动循迹排爆小车系统。BigFish能将大部分传感器轻松的和Arduino控制板连接。通过BigFish扩展板连接的电路可靠稳定，上面还扩展了伺服电机接口、8*8Led点阵、直流电机驱动以及一个通用扩展接口，可以说是Arduino控制板的必备配件。
? ? ? ① BigFish特点
? ? ? ??·完全兼容Arduino控制板标准接口
? ? ? ??·彩色分组插针，一目了然
? ? ? ??·全部铜制插针，用料考究，电器性能稳定
? ? ? ??·优秀PCB设计，美观大方
? ? ? ??·多种特殊接口设计，兼容所有探索者电子模块，使用方便
? ? ? ??·所有3P、4P接口采用防反插设计，避免电子模块间连线造成的误操作
? ? ? ??·板载舵机接口、直流电机驱动芯片、MAX7219LED驱动芯片，可直接驱动舵机、直流电机、数码管等机器人常规执行部件，无需外围电路
? ? ? ??·具有5V、3.3V及vin 3种电源接口，便于为各类扩展模块供电
? ? ? ② BigFish参数
? ? ? ??·4针防反插接口供电5V
? ? ? ??·舵机接口使用3A的稳压芯片LM1085ADJ，为舵机提供6v额定电压
? ? ? ??·8*8LED点阵模块采用MAX7219驱动芯片
? ? ? ??·板载两片直流电机驱动芯片L9170，支持3V~15V的vin电压，可驱动两个直流电机

? ? ? ??·2个2*5的杜邦座扩展坞，方便无线模块、OLED、蓝牙等扩展模块直插连接，无需额外接线

? ? ? ③ BigFish注意事项
? ? ? D11\D12舵机端口与LED点阵复用，请注意避免同时使用。
? ? ? 背面两侧的跳线分别作用于两侧的红色接口（通常采用5V，接传感器）或白色接口（通常采用6V，接舵机），使用前请检查背面跳线设置是否与器件电压相符。

? ? ? ④ 实物图片

3.3 驱动电机的选择

? ? ? 直流电机和步进电机都可以用于小车驱动，故有两种方案。

? ? ? 方案一：使用直流电机（如下图所示），加上适当减速比的减速器。直流电机具有良好的调速性能，控制起来也比较简单。直流电机只要通上直流电源就可连续不断的转动，调节电压的大小就可以改变电机的速度。直流电机的驱动电路实际上就是一个功率放大器。常用的驱动方式是PWM方式，即脉冲宽度调制方式。此方法性能较好，电路和控制都比较简单。

? ? ? 方案二：使用步进电机。步进电机具有良好的控制性能。当给步进电机输入一个电脉冲信号时，步进电机的输出轴就转动一个角度，因此可以实现精确的位置控制。与直流电机不同，要使步进电机连续的转动，需要连续不断的输入点脉冲信号，转速的大小由外加的脉冲频率决定。去而且其转动不受电压波动和负载变化的影响，也不受温度、气压等环境因素的影响，仅与控制脉冲有关。但步进电机的驱动相对较复杂，要由控制器和功率放大器组成，具体差别见下表1。

? ? ? 由上表可以看出步进电机和直流电机都有各自的优点。步进电机能进行精确的位置控制，但驱动电路麻烦，本设计直流电机即可满足小车要求的精度。且直流电机易于控制，驱动电路十分简单。

3.4 循迹模块

? ? ? 光电传感器和灰度传感器都可以用于小车循迹，故有两种方案。

? ? ? 方案一：采用简易光电传感器结合外围电路探测，但实际效果并不理想，对行驶过程中的稳定性要求很高，且误测几率较大、易受光线环境和路面介质影响。在使用过程极易出现问题，而且容易因为该部件造成整个系统的不稳定。故最终未采用该方案。

? ? ? 方案二：采用灰度传感器（如下图所示）进行循迹，灰度传感器有一只发光二极管和一只光敏电阻，安装在同一面上。灰度传感器利用不同颜色的检测面，对光的反射程度不同，光敏电阻利用不同检测面返回的光不同、其阻值也不同的原理来进行颜色深浅检测。在有效的检测距离内，发光二极管发出白光，照射在检测面上，检测面反射部分光线，光敏电阻检测此光线的强度并将其转换为机器人可以识别的信号。灰度既可以当作数字量传感器使用，也可以当作模拟量传感器使用，用三只灰度传感器，平均置于小车车头前端，根据其接受到黑线的情况来控制小车转向来调整车向，测试表明，只要合理安装好三只灰度传感器的位置就可以很好的实现循迹的功能。通过比较，我选取第二种方案来实现循迹。

? ? ? 灰度传感器工作原理：

? ? ? 灰度检测的有效距离在0.7cm~3cm之间，当传感器检测到有深色标记时，将会触发传感器，使其输出口有低电平输出；当传感器检测到浅色标记时，传感器将不会被触发，所以其输出口是高电平输出。原理图如下图所示：

3.5 IIC颜色识别模块

? ? ? IIC颜色识别模块（如下图所示）是基于TCS34725彩色光数字转换器为芯片设计的颜色识别传感器，传感器提供红色,绿色,蓝色(RGB)和清晰光感应值的数字输出,具有抗光干扰，可检测不发光物体颜色，防反插接口设计等优点。集成红外阻挡滤光片，可最大限度地减少入射光的红外光谱成分,并可精确地进行颜色测量。具有高灵敏度、宽动态范围，且配置红外阻隔滤波器。最小化IR和UV光谱分量效应，以产生准确的颜色测量。带有环境光强检测和可屏蔽中断，通过l2C接口通信。
? ? ? 本模块增加可调M12镜头，有效增加检测距离。并设置有4个高亮LED灯进行补光，可通过红外检测头自主控制LED亮灭。内置一个定位器，可通过定位器旋钮调节检测距离 (最大为常量，最小为常闭)。

3.6 舵机

? ? ? 舵机（如下图所示）是制作机器人模型的常见部件，一般用来制作机械臂、多足仿生等模型。舵机是一种简化版的伺服电机，最早用于航模的制作，用于船模、机模的尾舵，所以称为“舵”机。舵机一般由直流电机、控制电路板、电位器、减速器（齿轮组）、外壳、紧固件等组成。“模拟舵机”是相对于“数字舵机”来讲的，二者的区别在于控制电路，一个采用模拟电路，一个采用数字电路。

? ? ? 舵机的控制一般需要一个20ms左右的时基脉冲，其中有2.5ms是控制舵机角度的，在这2.5毫秒内分别对应了舵机的不同脉宽（表现的现象就是电压的大小）。该脉冲的高电平部分一般为0.5ms-2.5ms范围内的角度控制脉冲部分，总间隔为2ms。舵机的基准信号一般都是周期为20ms，宽度为1.5ms的脉冲信号，这个基准信号定义的位置为舵机摆动行程的中间位置。如果舵机的行程为0°~180°，该信号定义的位置为90°。

? ? ? 当舵机接收到的脉冲宽度小于1.5ms时，输出轴会以中间位置为基准，逆时针旋转一定角度；当舵机接收到的脉冲宽度大于1.5ms时，输出轴会以中间位置为基准，顺时针旋转一定角度。

4. 系统软件设计

? ? ? 软件设计主要包括了主程序、颜色识别模块、驱动电机控制模块、舵机控制模块四部分。

4.1主程序

? ? ? 本程序主要对直流电机速度及引脚进行定义和初始化，对舵机初始角度，排爆角度进行定义及对引脚进行初始化，对传感器进行定义及小车模式的定义等。主程序主要有如下：

void setup() {

  delay(1500);

  Serial.begin(115200); //打开串口，并设置波特率为115200

  Motor_Sensor_Init();   //电机、灰度传感器引脚初始化

  Servo_Init();   //舵机引脚初始化

  Color_Init();   //颜色传感器引脚初始化

Zha_Qi_Qiu(3);//测试扎气球程序

}

void loop() {

  //小车从出发区域开始到小车走出第二个管道程序部分

  Car_Road_Obstacle();

  //小车从第二个U型管道出来后，进入扎气球路段

  while(1){

      Car_Road_Balloon(); //小车进入扎气球路段

  }

}

4.2 颜色识别模块

? ? ? 将IIC颜色识别模块的Arduino函数库拷贝到Arduino 软件的libraries文件夹中，共包含“Adafruit_GFX”、“Adafruit_NeoPixel”、“MH_TCS34725”三个文件夹。启动ArduinoIDE，可以发现，在Example中增加了一个名为MH_TCS34725 的例程库，包含4个例程文件。在颜色识别程序中把串口打印打开，并在主程序中进行测试，主要代码如下：

Serial.println(color_data);// 在颜色识别程序中，色彩串口打印

while(1){return_color_ballon();};//在主程序中，死循环

4.3 驱动电机控制模块

? ? ? 此模块包含了基于三个灰度传感器的直流电机的速度及方向的控制，具体控制如下真值表所示。

? ? ? ? ? ? ? A2 ? ?A3 ? ?A0

? ? ? ? ? ? 0 ? ?0 ? ?0 ? ?0x00 ?表示三个传感器都没有触发，直行

? ? ? ? ? ? ?0 ? ?0 ? ?1 ? ?0x01 ?表示小车右边一个传感器触发，右转

? ? ? ? ? ? ?0 ? ?1 ? ?0 ? ?0x02 ?表示小车中间传感器触发，直行

? ? ? ? ? ? ?0 ? ?1 ? ?1 ? ?0x03 ?表示小车右边两个传感器都触发，右转

? ? ? ? ? ? ?1 ? ?0 ? ?0 ? ?0x04 ?表示小车左边一个传感器触发，左转

? ? ? ? ? ? ?1 ? ?0 ? ?1 ? ?0x05 ?表示小车左边和右边两个传感器触发，直行

? ? ? ? ? ? ?1 ? ?1 ? ?0 ? ?0x06 ?表示小车左边两个传感器都触发，左转

? ? ? ? ? ? ?1 ? ?1 ? ?1 ? ?0x07 ?表示小车三个传感器都触发，直行或停止

? ? ? 灰度传感器A0、A3、A2，用来小车巡线时，采集模拟量，并返回传感器数值，并设置了当小车遇到第三个黑色横线的时候令其停止，此时小车会停止，并识别色卡。

4.4 舵机控制模块

? ? ? 舵机控制主要基于爆破物颜色与色卡比对成功来进行摆动，初始角度定义为90，排爆角度为179，当比对成功后，舵机摆动三次。舵机排爆的动作，摆动几次程序如下：

void Zha_Qi_Qiu(int Numbers)

{

  for(int i=0;i<Numbers;i++)

  {

    Servo_Move_Single(1,Balloon_Servo_Init,Balloon_Servo_Down,8);  

    Servo_Move_Single(1,Balloon_Servo_Down,Balloon_Servo_Init,7);

  }

}

5. 功能总结

5.1 小车创新点

? ? ? 小车采用三个灰度传感器来循迹，灰度传感器具有体积小、重量轻、功能强大、方便安装的优势，可以广泛应用在机器人寻迹、灰度识别或者和processing等作出各种色彩互动产品。灰度传感器体积小，用在机器人身上，能够通过推断擂台的颜色得出机器人距离擂台边缘的距离，固定在机器人底部，碰到反光对手不会误判擂台边缘。

? ? ? 小车在基于月球车底盘的基础上增加了一个舵机驱动的摆杆，摆杆的顶部安装了排针，用来刺破气球。两侧摇臂用零件固定在了一起，单侧摇臂不能单独摆动，只能同时摆动，损失了一些灵活性，但是增加了行驶中的稳定性，尤其是增加了攀爬越障时的稳定性，车身不容易跑偏。

5.2 小车设计难点及解决方案

? ? ? 智能循迹排爆小车难点在于精准循迹，并对直流电机精准的速度和方向的控制以及排爆时对舵机的控制。解决方案是用三个灰度传感器来精准循迹，即在小车车头下方的左中右三个位置分别放置三个灰度传感器，根据传感器是否触发或是传感器返回的数值大小来判断车体的三种位置状态：中正、偏左、偏右。电机和舵机的各项控制需要在后续的程序及调试中实现。

6. 示例程序

① Car.ino

/*

*=====================================================================================================*

*实验接线:                                                                                            |

*=====================================================================================================*

*                       车头

*   灰度传感器：     A2   A3   A0

*                *----------------*

*                |                |

*                |                |

*                |                |

*                |                | 右侧

*         motor   |                | 车轮

*          9,10   |                | 5，6

*                |                |

*                |                |

*                |                |

*                |                |

*                |                |

*                *----------------*

*                       车尾

* 舵机接线：

*         车头舵机:3

*         气球舵机:8

*         车尾舵机:12

*

*

* 颜色传感器接线：

*            TCS3473x   Arduino_Uno

*              SDA   -----   A4

*              SCL   -----   A5

*              VIN   -----   5V

*              GND   -----   GND

****************************************************************************************************/

//------------颜色传感器库函数--------------------------------------

#include <Wire.h>        //调用IIC库函数

#include "MH_TCS34725.h" //调用颜色识别传感器库函数

#include <Adafruit_GFX.h>

#ifdef __AVR__

  #include <avr/power.h>

#endif

//颜色传感器不同通道值设置

MH_TCS34725 tcs = MH_TCS34725(TCS34725_INTEGRATIONTIME_50MS, TCS34725_GAIN_4X); //设置颜色传感器采样周期50毫秒

//------------舵机库函数--------------------------------------

#include<ServoTimer2.h>    //调用舵机库函数/

ServoTimer2 myServo[3];    //声明舵机数组/

//#include<Servo.h>    //调用舵机库函数

//Servo myServo[3];    //声明舵机数组

//-------------定义程序需要用到的其他变量--------------------------------------

#define TrackingSensorNum 3 //小车寻迹时使用的灰度传感器数量

#define Forward_Left_Speed 100   //小车前进时左轮速度

#define Forward_Right_Speed 90 //小车前进时右轮速度

#define Back_Left_Speed 110     //小车后退时左轮速度

#define Back_Right_Speed 90    //小车后退时右轮速度

#define Left_Left_Speed 235     //小车左转时左轮速度

#define Left_Right_Speed 240    //小车左转时右轮速度

#define Right_Left_Speed 235    //小车右转时左轮速度

#define Right_Right_Speed 240   //小车右转时右轮速度

#define Car_speed_stop 255

#define CAR_STOP_DELAY 500

#define Head_Servo_Init 90     //车头舵机初始角度

#define Head_Servo_Down 60     //车头舵机向下降角度

#define Head_Servo_Up 150      //车头舵机向上抬起角度

#define Balloon_Servo_Init 87 //气球舵机初始角度

#define Balloon_Servo_Down 179 //气球舵机扎气球角度

#define Tail_Servo_Init 90     //车尾舵机初始角度

#define Tail_Servo_Down 60     //车尾舵机向下降角度

#define Tail_Servo_Up 150      //车尾舵机向上抬起角度

#define Grey_Sensor_AnalogRead 288   //定义灰度传感器触发时的数值

int Gray_Three = 0; //小车开始扎气球路线后，记录小车经过场地中的特殊横线的次数

int Start_Car = 0;   //小车从开始到走出第二个U型管道之间的路线，记录小车经过场地中的特殊横线的次数

int tracking_die = 0;

int color_detection_card = 0;     

int color_detection_ballon = 0;

bool finish=true;   //小车识别完一个动作的标志位

bool finish_all = true; //小车识别完色卡和气球匹配是否成功的标志位

int First_Road = 1; //小车第一段路线运行是否成功的标志位

int Second_Road = 1;   //小车第二段路线运行是否成功的标志位

int Gray_SensorPin[3]={A0,A3,A2};//寻迹、检测路口传感器

int Car_Head_Gray_SensorPin_Num = sizeof(Gray_SensorPin)/sizeof(Gray_SensorPin[0]);//定义gray数量

int Car_DC_Motor_Pin[4] = {9,10,5,6};//直流电机引脚

int motor_num = sizeof(Car_DC_Motor_Pin) / sizeof(Car_DC_Motor_Pin[0]);//定义电机数量

int servo_port[3] = {3,8,12};//定义舵机引脚

int servo_num = sizeof(servo_port) / sizeof(servo_port[0]);//定义舵机数量

float value_init[3] = {Head_Servo_Init, Balloon_Servo_Init, Tail_Servo_Init};//定义舵机初始角度

int f = 30; //定义舵机每个状态间转动的次数，以此来确定每个舵机每次转动的角度

enum{Forward=1,Back,Left,Right,Stop,ForwardSpeedDown,BackSpeedDown,ForwardRoad};//小车各种模式状态

void setup() {

  delay(1500);  

 

  Serial.begin(115200); //打开串口，并设置波特率为115200

  Motor_Sensor_Init();   //电机、灰度传感器引脚初始化

  Servo_Init();   //舵机引脚初始化

  Color_Init();   //颜色传感器引脚初始化

//Zha_Qi_Qiu(3);//测试扎气球程序

//while(1){return_color_ballon();};//死循环

}

void loop() {

  //小车从出发区域开始到小车走出第二个管道程序部分

  Car_Road_Obstacle();

  //小车从第二个U型管道出来后，进入扎气球路段

  while(1){

      Car_Road_Balloon(); //小车进入扎气球路段

  }

}

② Color_detection.ino

//颜色传感器相关内容

void Color_Init()

{

  Serial.println("Color View Test!"); //串口打印：Color View Test!

  //检测是否有颜色传感器模块

  if (tcs.begin()) {                 //如果检测到颜色传感器模块

    Serial.println("Found sensor");   //串口打印 Found sensor

  } else {                           //如果没有检测到颜色传感器模块

    Serial.println("No TCS34725 found ... check your connections");//串口打印：没有找到颜色识别传感器模块

    while (1); // halt! //程序陷入死循环

  }

}

//气球颜色检测

void return_color_ballon()

{

  String color_data = "";

  uint16_t clear, red, green, blue; //分别定义用于存储红、绿、蓝三色值变量

  tcs.getRGBC(&red, &green, &blue, &clear); //将原始R/G/B值转换为色温（以度为单位）

  tcs.lock();   //禁用中断（可省略）

  uint32_t sum = clear;           //===========

  float r, g, b;                  // 计算红

  r = red; r /= sum;              // 绿、蓝

  g = green; g /= sum;            // 三色数

  b = blue; b /= sum;             // 值

  r *= 256; g *= 256; b *= 256;   //===========

  /***********************************************************

  Serial.print("\t");

  Serial.print((int)r); Serial.print("\t"); // 在串口中分别打印

  Serial.print((int)g); Serial.print("\t"); // 红、绿、蓝三色

  Serial.print((int)b);                     // 值

  Serial.println();

  ***********************************************************/

  if( (r > 0) && (g>0) && (b>0) && (r>g) && (r>b) )   //如果检测到红色，

  {

    color_data = "red";//记录一下颜色，方便色卡和气球颜色对比

    color_detection_ballon = 1;

  }

  else if( (r > 0) && (g>0) && (b>0) && (r<g) && (b<g) ) //如果检测到绿色

  {

    color_data = "green";

    color_detection_ballon = 3;

  }

 

  else if( (r > 0) && (g>0) && (b>0) && (r<b) && (g<b) ) //如果检测到蓝色

  {

    color_data = "blue";

    color_detection_ballon = 2;

  }

  else{                        //否则，小车停止

    color_data = "none";

  }

//Serial.println(color_data);//色彩串口打印

}

//色卡颜色检测

void return_color_card()

{

  String color_data = "";

  uint16_t clear, red, green, blue; //分别定义用于存储红、绿、蓝三色值变量

  tcs.getRGBC(&red, &green, &blue, &clear); //将原始R/G/B值转换为色温（以度为单位）

  tcs.lock();   //禁用中断（可省略）

  uint32_t sum = clear;           //===========

  float r, g, b;                  // 计算红

  r = red; r /= sum;              // 绿、蓝

  g = green; g /= sum;            // 三色数

  b = blue; b /= sum;             // 值

  r *= 256; g *= 256; b *= 256;   //===========

  /***********************************************************

  Serial.print("\t");

  Serial.print((int)r); Serial.print("\t"); // 在串口中分别打印

  Serial.print((int)g); Serial.print("\t"); // 红、绿、蓝三色

  Serial.print((int)b);                     // 值

  Serial.println();

  ***********************************************************/

  if( (r > 0) && (g>0) && (b>0) && (r>g) && (r>b) )   //如果检测到红色

  {

    color_data = "red";

    color_detection_card = 1;

  }

  else if( (r > 0) && (g>0) && (b>0) && (r<g) && (b<g) ) //如果检测到绿色

  {

    color_data = "green";

    color_detection_card = 3;

  }

 

  else if( (r > 0) && (g>0) && (b>0) && (r<b) && (g<b) ) //如果检测到蓝色

  {

    color_data = "blue";

    color_detection_card = 2;

  }

  else{                        //否则，小车停止，oled屏幕显示none

    color_data = "none";

  }

//Serial.println(color_data);//色彩串口打印

}

//颜色传感器检测颜色测试程序

void color()

{

  String color_data = "";

  uint16_t clear, red, green, blue; //分别定义用于存储红、绿、蓝三色值变量

  tcs.getRGBC(&red, &green, &blue, &clear); //将原始R/G/B值转换为色温（以度为单位）

  tcs.lock();   //禁用中断（可省略）

 

  uint32_t sum = clear;           //===========

  float r, g, b;                  // 计算红

  r = red; r /= sum;              // 绿、蓝

  g = green; g /= sum;            // 三色数

  b = blue; b /= sum;             // 值

  r *= 256; g *= 256; b *= 256;   //===========

  if( (r > 0) && (g>0) && (b>0) && (r>g) && (r>b) )   //如果检测到红色

  {

    color_data = "red";

  }

  else if( (r > 0) && (g>0) && (b>0) && (r<g) && (b<g) ) //如果检测到绿色

  {

    color_data = "green";

  }

 

  else if( (r > 0) && (g>0) && (b>0) && (r<b) && (g<b) ) //如果检测到蓝色

  {

    color_data = "blue";

  }

  else{                        //否则，小车停止，oled屏幕显示none

    color_data = "none";

  }

  delay(20);

// Serial.println("color_data:");

}

③ Motor_Gray_Control.ino

void Motor_Sensor_Init()

{

  for(int i=0;i<Car_Head_Gray_SensorPin_Num;i++) {//初始化灰度传感器

     pinMode(Gray_SensorPin[i],INPUT);

     delay(20);

  }

  for(int i=0;i<motor_num;i++) {//初始化 电机

     pinMode(Car_DC_Motor_Pin[i],OUTPUT);

     delay(20);

  }

}

void Serialprint_gray_sensor_data_analogRead()

{

   int data_sensor[3]={0,0,0};

   for(int i=0;i<3;i++)

   {

     Serial.print(Gray_SensorPin[i]);Serial.print(": ");

     Serial.print(analogRead(Gray_SensorPin[i]));

     Serial.print(" | ");

   }

   Serial.println();  

}

void Serialprint_gray_sensor_data()

{

   int data_sensor[3]={0,0,0};

   for(int i=0;i<3;i++)

   {

     Serial.print(Gray_SensorPin[i]);Serial.print(": ");

     Serial.print(digitalRead(Gray_SensorPin[i]));

     Serial.print(" | ");

   }

   Serial.println();  

}

/*-----------------------------------------------------------------------

  A2         A3         A0

  ----------------------------------

  0          0          0      0x00   表示三个传感器都没有触发

  0          0          1      0x01   表示小车右边一个传感器触发

  0          1          0      0x02   表示小车中间传感器触发

  0          1          1      0x03   表示小车右边两个传感器都触发

  1          0          0      0x04   表示小车左边一个传感器触发

  1          0          1      0x05  

  1          1          0      0x06   表示小车左边两个传感器都触发

  1          1          1      0x07   表示小车三个传感器都触发  

------------------------------------------------------------------------*/

int Detection_tracking() //灰度传感器A0,A3,A2,用来小车巡线时，返回传感器数值；

{

  int num = 0;

  for(int i=0;i<Car_Head_Gray_SensorPin_Num;i++)

  {

    num |= ( (!digitalRead(Gray_SensorPin[i]) ) << i);

  }

  //Serial.println(num);

  return num;

}

void Tracking_Automatic_Tracking(unsigned long delay_tracking_time)

{

  unsigned long now_times = millis();

  while(millis()-now_times<delay_tracking_time)

  {

    Automatic_Tracking();

  }

}

void Automatic_Tracking() //小车前方两个灰度传感器用来寻迹（即小车巡线）。

{

  int Get_Data = 0;

  Get_Data = Detection_tracking_analogRead();

  switch(Get_Data)

  {   

    case 0x00: Car_Move(Forward,Forward_Left_Speed,Forward_Right_Speed);break;//forward

    case 0x01: Car_Move(Right,   Right_Left_Speed,   Right_Right_Speed   );break;//RIGHT   

    case 0x02: Car_Move(Forward,Forward_Left_Speed,Forward_Right_Speed);break;//forward

    case 0x03: Car_Move(Right,   Right_Left_Speed,   Right_Right_Speed   );break;//right

    case 0x04: Car_Move(Left,   Left_Left_Speed,   Left_Right_Speed   );break;//left

    case 0x06: Car_Move(Left,   Left_Left_Speed,   Left_Right_Speed   );break;//LEFT

    case 0x07: Car_Move(Forward,Forward_Left_Speed,Forward_Right_Speed);break;//forward

    default: break;     

  }

}

int Detection_tracking_analogRead() //灰度传感器A0,A3,A2,用来小车巡线时，返回传感器数值；

{

  int num = 0;

  int analogRead_data[3] = {0,0,0};

  for(int i=0;i<Car_Head_Gray_SensorPin_Num;i++)

  {

    analogRead_data[i] = analogRead(Gray_SensorPin[i]);

    if( analogRead_data[i] <=Grey_Sensor_AnalogRead ){analogRead_data[i] = 1;} else{analogRead_data[i] = 0;}

    num |= ( (analogRead_data[i]) << i);

  }

  //Serial.println(num);

  return num;

}

void Automatic_Tracking_analogRead() //小车前方3个灰度传感器用来寻迹（即小车巡线）。

{

  int Get_Data = 0;

  Get_Data = Detection_tracking_analogRead();

  switch(Get_Data)

  {   

    case 0x00: Car_Move(Forward,Forward_Left_Speed,Forward_Right_Speed);break;//forward

    case 0x01: Car_Move(Right,   Right_Left_Speed,   Right_Right_Speed   );break;//RIGHT   

    case 0x02: Car_Move(Forward,Forward_Left_Speed,Forward_Right_Speed);break;//forward

    case 0x03: Car_Move(Right,   Right_Left_Speed,   Right_Right_Speed   );break;//right

    case 0x04: Car_Move(Left,   Left_Left_Speed,   Left_Right_Speed   );break;//left

    case 0x06: Car_Move(Left,   Left_Left_Speed,   Left_Right_Speed   );break;//LEFT

    case 0x07: Gray_Three ++; Car_Move(Forward,Forward_Left_Speed,Forward_Right_Speed);break;//forward

    default: break;     

  }

}

void Automatic_Tracking_analogRead_Start() //小车前方3个灰度传感器用来寻迹（即小车巡线）。

{

  int Get_Data = 0;

  Get_Data = Detection_tracking_analogRead();

  switch(Get_Data)

  {   

    case 0x00: Car_Move(Forward,Forward_Left_Speed,Forward_Right_Speed);break;//forward

    case 0x01: Car_Move(Right,   Right_Left_Speed,   Right_Right_Speed   );break;//RIGHT   

    case 0x02: Car_Move(Forward,Forward_Left_Speed,Forward_Right_Speed);break;//forward

    case 0x03: Car_Move(Right,   Right_Left_Speed,   Right_Right_Speed   );break;//right

    case 0x04: Car_Move(Left,   Left_Left_Speed,   Left_Right_Speed   );break;//left

    case 0x06: Car_Move(Left,   Left_Left_Speed,   Left_Right_Speed   );break;//LEFT

    case 0x07: Start_Car ++; Car_Move(Forward,Forward_Left_Speed,Forward_Right_Speed);break;//forward

    default: break;     

  }

}

void Car_Move(int Mode,int LeftSpeed,int RightSpeed)                                                                           

{

  switch(Mode)

  {

    case Forward:

                analogWrite(Car_DC_Motor_Pin[0],LeftSpeed);

                analogWrite(Car_DC_Motor_Pin[1],0);

                analogWrite(Car_DC_Motor_Pin[2],RightSpeed);

                analogWrite(Car_DC_Motor_Pin[3],0);

                break;

    case Back:

                analogWrite(Car_DC_Motor_Pin[0],0);

                analogWrite(Car_DC_Motor_Pin[1],LeftSpeed);

                analogWrite(Car_DC_Motor_Pin[2],0);

                analogWrite(Car_DC_Motor_Pin[3],RightSpeed);

                break;  

    case Left:

                analogWrite(Car_DC_Motor_Pin[0],0);

                analogWrite(Car_DC_Motor_Pin[1],LeftSpeed);

                analogWrite(Car_DC_Motor_Pin[2],RightSpeed);

                analogWrite(Car_DC_Motor_Pin[3],0);

                break;

    case Right:

                analogWrite(Car_DC_Motor_Pin[0],LeftSpeed);

                analogWrite(Car_DC_Motor_Pin[1],0);

                analogWrite(Car_DC_Motor_Pin[2],0);

                analogWrite(Car_DC_Motor_Pin[3],RightSpeed);

                break;  

    case Stop:

                analogWrite(Car_DC_Motor_Pin[0],LeftSpeed);

                analogWrite(Car_DC_Motor_Pin[1],LeftSpeed);

                analogWrite(Car_DC_Motor_Pin[2],RightSpeed);

                analogWrite(Car_DC_Motor_Pin[3],RightSpeed);

                break;                                             

  }

}

void Car_Move_Test()

{

  Car_Move(Forward,Forward_Left_Speed,Forward_Right_Speed);delay(1500);Car_Move(Stop,Car_speed_stop,Car_speed_stop);delay(1500);

  Car_Move(Back,Forward_Left_Speed,Forward_Right_Speed);delay(1500);Car_Move(Stop,Car_speed_stop,Car_speed_stop);delay(1500);

  Car_Move(Left,Left_Left_Speed,   Left_Right_Speed);delay(1500);Car_Move(Stop,Car_speed_stop,Car_speed_stop);delay(1500);

  Car_Move(Right,Right_Left_Speed,   Right_Right_Speed);delay(1500);Car_Move(Stop,Car_speed_stop,Car_speed_stop);delay(1500);

}

④ Road_Balloon.ino

//小车从第二个U型管道出来后，进入Road_Balloon：气球路段

void Car_Road_Balloon() //小车进入扎气球路段

{

  Automatic_Tracking_analogRead();

  if(Gray_Three == 1)

  {Car_Move(Stop,Car_speed_stop,Car_speed_stop);delay(3000);

    //Tracking_Automatic_Tracking(2000);

   // Car_Move(Stop,Car_speed_stop,Car_speed_stop);delay(3000);

    return_color_card(); //小车走到色卡开始识别色卡

    Tracking_Automatic_Tracking(1390);

    Car_Move(Stop,Car_speed_stop,Car_speed_stop);delay(3000);

    return_color_ballon();

    if(color_detection_card == color_detection_ballon){

      Zha_Qi_Qiu(3);

      finish = false;

      finish_all = false;

    }//小车走到第一个气球区域，并判断气球颜色。

    if(finish_all){

        Tracking_Automatic_Tracking(1200);Car_Move(Stop,Car_speed_stop,Car_speed_stop);delay(3000);

        return_color_ballon();

        if( (color_detection_card == color_detection_ballon) && finish ){

          Zha_Qi_Qiu(3);

          finish = false;

          finish_all = false;

      }//小车走到第二个气球区域，并判断气球颜色。

    }

    if(finish_all){

        Tracking_Automatic_Tracking(1200);Car_Move(Stop,Car_speed_stop,Car_speed_stop);delay(3000);

        return_color_ballon();

        if( (color_detection_card == color_detection_ballon) && finish ){

          Zha_Qi_Qiu(3);

          finish = false;

          finish_all = false;

      }//小车走到第三个气球区域，并判断气球颜色。

    }

    //Car_Move(Stop,Car_speed_stop,Car_speed_stop);delay(3000);

  }  

   if(Gray_Three == 2)

   {

    Car_Move(Stop,Car_speed_stop,Car_speed_stop);delay(3000);

    }

}

⑤ Road_Obstacle.ino

//小车从出发区域开始到小车走出第二个管道程序部分

#define Head_Servo_Init 90

#define Head_Servo_Down 60

#define Head_Servo_Up 150

void Car_Road_Obstacle() //小车从出发区域开始到小车走出第二个管道程序部分

{

  while( First_Road )   //小车从出发区域到经过第一个U型管道的程序

  {

      Automatic_Tracking_analogRead_Start(); //小车前方3个灰度传感器用来寻迹（即小车巡线）

      if( Start_Car == 1 ) //如果小车经过场地中第一个横线

      {

        Tracking_Automatic_Tracking(5000); //小车向前巡线6500毫秒   ，这里小车巡线的时间需要让小车能通过第一个 寨桥 的一个大致时间，注意，尽可能的让小车能通过第一个 寨桥 的时间，需要自己更改

       

        //Car_Move(Stop,Car_speed_stop,Car_speed_stop);delay(1000); //小车停止3秒，这里的时间可自行更改，这一行程序可要可不要，主要是方便调试，比赛中可删除该行

       

       // Tracking_Automatic_Tracking(2000); //小车向前巡线7800毫秒   ，这里小车巡线的时间需要让小车能通过第一个 U型管道 的一个大致时间，注意，尽可能的让小车能通过第一个 U型管道 的时间，

                                           //具体时间需要自己根据小车通过U型管道花费的时间计算

        First_Road = 0;

       // Car_Move(Stop,Car_speed_stop,Car_speed_stop);delay(3000); //小车停止3秒，这里的时间可自行更改，这一行程序可要可不要，主要是方便调试，比赛中可删除该行

      }

  }

  while( Second_Road )     //小车从第一个U型管道到走出第二个U型管道的程序

  {

      Automatic_Tracking_analogRead_Start(); //小车前方3个灰度传感器用来寻迹（即小车巡线）

      if( Start_Car == 2 ) //如果小车经过场地中第二个横线

      {

       

        Tracking_Automatic_Tracking(5000); //小车向前巡线6500毫秒   ，这里小车巡线的时间需要让小车能通过 T型栈桥 的一个大致时间，注意，尽可能的让小车能通过 T型栈桥 的时间，需要自己更改

       

       // Car_Move(Stop,Car_speed_stop,Car_speed_stop);delay(1000); //小车停止3秒，这里的时间可自行更改，这一行程序可要可不要，主要是方便调试，比赛中可删除该行

     

        //Tracking_Automatic_Tracking(3000); //小车向前巡线7800毫秒   ，这里小车巡线的时间需要让小车能通过第二个 U型管道 的一个大致时间，

                                           //注意，尽可能的让小车能通过第二个 U型管道 的时间，具体时间需要自己根据小车通过U型管道花费的时间计算

         

        Second_Road = 0;

        //Car_Move(Stop,Car_speed_stop,Car_speed_stop);delay(3000); //小车停止3秒，这里的时间可自行更改，这一行程序可要可不要，主要是方便调试，比赛中可删除该行

      }

  }

  Serial.println("hahaha, I get to here");

}

⑥ Servo_Move_Control.ino

//===================舵机动作========================舵机动作=======================舵机动作=======================舵机动作====

//扎气球的动作，小车扎几次气球

void Zha_Qi_Qiu(int Numbers)

{

  for(int i=0;i<Numbers;i++)

  {

    Servo_Move_Single(1,Balloon_Servo_Init,Balloon_Servo_Down,8);  

    Servo_Move_Single(1,Balloon_Servo_Down,Balloon_Servo_Init,7);

  }

}

//===================舵机底层========================舵机底层=======================舵机底层=======================舵机底层====

void Servo_Init()

{

  for(int i=0;i<servo_num;i++)

  {

    myServo[i].attach(servo_port[i]);

    myServo[i].write(map(value_init[i],0,180,500,2500));

    delay(20);

  }

}

void ServoStop(int which){//释放舵机

  myServo[which].detach();

  digitalWrite(servo_port[which],LOW);

}

void ServoGo(int which , float where){//打开并给舵机写入相关角度

  if(where!=200){

    if(where==201) ServoStop(which);

    else{

      myServo[which].write(map(where,0,180,500,2500));

      //myServo[which].write(where);

    }

  }

}

void Servo_Move_Single(int which,int Start_angle,int End_angle,unsigned long Servo_move_time)

{

  int servo_flags = 0;

  int delta_servo_angle = abs(Start_angle-End_angle);

  if( (Start_angle - End_angle)<0 )

  {

    servo_flags = 1;

  }

  else{ servo_flags = -1; }

  for(int i=0;i<delta_servo_angle;i++)

  {

    myServo[which].write(map( Start_angle+(servo_flags*i) ,0,180,500,2500));

    //myServo[which].write( Start_angle+(servo_flags*i) );

    delay(Servo_move_time);

  }

}

void servo_move(float value0, float value1, float value2,int delaytimes){ //舵机动作函数

  float value_arguments[] = {value0, value1, value2};

  float value_delta[servo_num];  

  for(int i=0;i<servo_num;i++){

    value_delta[i] = (value_arguments[i] - value_init[i]) / f;

  }  

  for(int i=0;i<f;i++){

    for(int k=0;k<servo_num;k++){

      value_init[k] = value_delta[k] == 0 ? value_arguments[k] : value_init[k] + value_delta[k];

    }

    for(int j=0;j<servo_num;j++){

      ServoGo(j,value_init[j]);

    }

    delay(delaytimes/f);

  }

}

更多详情请见：【S063】智能循迹排爆小车