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

作者：刘元青、邹海峰、付志伟、秦怀远、牛文进

单位：哈尔滨信息工程学院

指导老师：姚清元

智能小车是移动式机器人的重要组成部分，而移动机器人不仅能够在经济、国防、教育、文化和生活中起到越来越大的作用，也是研究复杂智能行为的产生、探索人类思维模式的有效工具与实验平台。

本文设计的是一款基于Arduino的智能小车，它利用Arduino作为主控系统。它用蓝牙模块进行无线数据传输，实现无线控制。它利用红外线反射等原理，实现自动循迹效果。同时它利用超声波测距模块来进行测距，将测得的距离数据传给Arduino，经过Arduino处理给出反馈，驱动电机转动，实现自动避障的功能。

1. 系统设计与实现

1.1 系统设计

1.1.1 系统架构

Roboduino智能小车车身由Arduino开发板和舵机等设备组成，其功能实现则由Arduino IDE编写程序以及Helloblock的调试来完成。

1.1.2 功能架构

基于Arduino的Roboduino智能小车通过烧录WiFi遥控程序，利用超声波测距原理实现自动避障，利用红外线反射原理实现自动循迹，此外还有其他功能模式。

1.1.3 技术架构

通过在Arduino IDE平台编写C代码来实现功能模块，将WIFI无线连接程序烧录至Arduino开发板，内含相关功能模块代码，利用超声波测距技术和红外线传感器分别实现自动避障和自动循迹功能。

1.2 系统实现

1.2.1 超声波测距

实验原理：超声波模块是利用超声波特性检测距离的传感器。其带有两个超声波探头，分别用作发射和接收超声波。本次实验我们所使用的模块，其测量的范围是 0-3m。

超声波时序图：以下时序图表明你只需要提供一个10uS 以上脉冲触发信号，该模块内部将发出8个40kHz周期电平并检测回波。一旦检测到有回波信号则输出回响信号。回响信号的脉冲宽度与所测的距离成正比。由此通过发射信号到收到的回响信号时间间隔可以计算得到距离。公式: uS/58=厘米或者uS/148=英寸;或是:距离=高电平时间*声速（340M/S）/2;建议测量周期为60ms 以上，以防止发射信号对回响信号的影响。

超声波模块引脚：根据硬件接口速查手册可知，超声波模块的超声波功能由Uno板的 Pin12引脚直接驱动。

实验程序

超声波测距（RGBUltrasonic_Ranging.ino）

/*

* @par Copyright (C): 2010-2019, Shenzhen Yahboom Tech

* @file         Ultrasonic_Ranging.c

* @author       Cindy

* @version      V1.0

* @date         2018.10.18

* @brief        Ultrasonic_Ranging

* @details

* @par History   

*

*/

#include <Wire.h>

#include "Adafruit_PWMServoDriver.h"

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);

const int SingPin = 12;

float distance;

/**

* Function       setup

* @author        liusen

* @date          2017.07.25

* @brief         Initial configuration

* @param[in]     void

* @retval        void

* @par History   no

*/

void setup()

{

   pwm.begin();

   pwm.setPWMFreq(60);                 //Analog servos run at ~60 Hz updates

   LOGO_breathing_light(255, 40, 5);   //Gradually light the blue light of the Yhaboom_LOGO

   Serial.begin(9600);

   Serial.println("Ultrasonic sensor:");

}

/**

* Function       LOGO_light(brightness,time,increament)

* @author        wusicaijuan

* @date          2019.06.26

* @brief         LOGO_light

* @param[in1]    brightness

* @param[in2]    time

* @param[in3]    increament

* @param[out]    void

* @retval        void

* @par History   no

*/

void LOGO_breathing_light(int brightness, int time, int increament)

{

  if (brightness < 0)

  {

    brightness = 0;

  }

  if (brightness > 255)

  {

    brightness = 255;

  }

  for (int b = 0; b < brightness; b += increament)

  {

    int newb = map(b, 0, 255, 0, 4095);

    pwm.setPWM(7, 0, newb);

    delay(time);

  }

}

/**

* Function       loop

* @author        Cindy

* @date          2019.07.30

* @brief         main fuction

* @param[in]     void

* @retval        void

* @par History   no

*/

void loop()

{

  pinMode(SingPin,OUTPUT);

  digitalWrite(SingPin, LOW);

  delayMicroseconds(2);

  digitalWrite(SingPin, HIGH);

  delayMicroseconds(10);

  digitalWrite(SingPin, LOW);

 

  pinMode(SingPin, INPUT);

  delayMicroseconds(50);

  distance = pulseIn(SingPin, HIGH) / 58.00;

  Serial.print("distance is :");

  Serial.print(distance);

  Serial.print("cm");

  Serial.println();

  delay(1000);

}

操作流程

我们需要通用 Arduino IDE 软件打开RGBUltrasonic_Ranging.ino文件，然后点击菜单栏中的“√”编译程序，并且等待左下角出现“编译成功”的字样。

在 Arduino IDE 的菜单栏中，我们需要选择【工具】---【端口】---选择设备管理器中刚刚显示端口号。

选择完成后，点击菜单栏下的“→”将代码上传到 UNO 板。当左下角出现“上传完成”字样时，表示程序已成功上传到 UNO 板。

程序下载完成之后，我们需要打开 Arduino IDE 界面右上角的“串口监视器”。

在串口监视器中选择和程序中相同的波特率。

接下来，我们就可以在串口监视器上面看到超声波模块当前所测量的距离被打印出来了。

1.2.2 巡线测试

实验原理：红外传感器巡线的基本原理是利用物体的反射性质，我们本次实验是巡黑线行驶，当红外线发射到黑线上时会被黑线吸收掉，发射到其他的颜色的材料上会有反射到红外的接手管上。

巡线模块引脚：根据硬件接口速查手册可知，三路巡线模块分别用Uno板的A0、A1、A2引脚驱动。

实验程序

① Tracking_PID_v1.ino

/*

* @par Copyright (C): 2010-2019, Shenzhen Yahboom Tech

* @file         Tracking_test.c

* @author       wusicaijuan

* @version      V1.0

* @date         2019.08.05

* @brief        Tracking_test

* @details

* @par History   

*

*/

#include "Adafruit_PWMServoDriver.h"

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);

float max = 3.85;

float s = 100;

float Kp = 37, Ki = 4, Kd = 60;  

float error = 0, P = 0, I = 0, D = 0, PID_value = 0;

float previous_error = 0, previous_I = 0;

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

int initial_motor_speed = 40;

const int key = 7;

void read_sensor_values(void);

void calculate_pid(void);

void motor_control(void);

void keyscan(void);

void Clear_All_PWM(void);

/**

* Function       setup

* @author        wusicaijuan

* @date          2019.08.05

* @brief         Initial configuration

* @param[in]     void

* @retval        void

* @par History   no

*/

void setup()

{

// put your setup code here, to run once:

pwm.begin();

pwm.setPWMFreq(60); // Analog servos run at ~60 Hz updates

Clear_All_PWM();

pinMode(A0, INPUT);

pinMode(A1, INPUT);

pinMode(A2, INPUT);

pinMode(key, INPUT);

keysacn();

}

/**

* Function       loop

* @author        wusicaijuan

* @date          2019.07.30

* @brief         main fuction

* @param[in]     void

* @retval        void

* @par History   no

*/

void loop()

{

read_sensor_values();

calculate_pid();

motor_control();

}

/**

* Function       read_sensor_values

* @author        wusicaijuan

* @date          2019.07.30

* @brief         read sensor value to change car movement

* @param[in]     void

* @retval        void

* @par History   no

*/

void read_sensor_values()

{

sensor[0] = analogRead(A0);

sensor[1] = analogRead(A1);

sensor[2] = analogRead(A2);

if (sensor[0] > 100)

{

sensor[0] = 1;

}

else

{

sensor[0] = 0;

}

if (sensor[1] > 100)

{

sensor[1] = 1;

}

else

{

sensor[1] = 0;

}

if (sensor[2] > 100)

{

sensor[2] = 1;

}

else

{

sensor[2] = 0;

}

if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1))

{

error = 2;

}

else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1))

{

error = 1;

}

else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 0))

{

error = 0;

}

else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 0))

{

error = -1;

}

else if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0))

{

error = -2;

}

else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0))

{

if (error > 0)

{

//spin left

error = max;

}

else

{

//spin right

error = -max;

}

}

}

/**

* Function       calculate_pid

* @author        wusicaijuan

* @date          2019.06.25

* @brief         calculate_pid

* @param[out]   

* @retval       

* @par History   no

*/

void calculate_pid()

{

P = error;

I = I + previous_I;

D = error - previous_error;

PID_value = (Kp * P) + (Ki * I) + (Kd * D);

// Serial.println(PID_value);

previous_I = I;

previous_error = error;

}

void motor_control()

{

// Calculating the effective motor speed:

int left_motor_speed = initial_motor_speed - PID_value;

int right_motor_speed = initial_motor_speed + PID_value;

// The motor speed should not exceed the max PWM value

// left_motor_speed = constrain(left_motor_speed, -255, 255);

// right_motor_speed = constrain(right_motor_speed, -255, 255);

left_motor_speed = constrain(left_motor_speed, -s, s);

right_motor_speed = constrain(right_motor_speed, -s, s);

run(left_motor_speed, right_motor_speed);

}

/**

* Function       run

* @author        wusicaijuan

* @date          2019.06.25

* @brief         run

* @param[in]     Speed

* @param[out]    void

* @retval        void

* @par History   no

*/

void run(float Speed1, float Speed2)

{

Speed1 = map(Speed1, -255, 255, -4095, 4095);

Speed2 = map(Speed2, -255, 255, -4095, 4095);

if (Speed2 > 0)

{

pwm.setPWM(10, 0, Speed2); //Right front wheel Forward

pwm.setPWM(11, 0, 0);

pwm.setPWM(8, 0, Speed2); //Right rear wheel Forward

pwm.setPWM(9, 0, 0);

}

else

{

pwm.setPWM(10, 0, 0);

pwm.setPWM(11, 0, abs(Speed2));

pwm.setPWM(8, 0, 0);

pwm.setPWM(9, 0, abs(Speed2));

}

if (Speed1 > 0)

{

pwm.setPWM(13, 0, Speed1); //Left front wheel Forward

pwm.setPWM(12, 0, 0);

pwm.setPWM(15, 0, Speed1); //Left front wheel Forward

pwm.setPWM(14, 0, 0);

}

else

{

pwm.setPWM(13, 0, 0);

pwm.setPWM(12, 0, abs(Speed1));

pwm.setPWM(15, 0, 0);

pwm.setPWM(14, 0, abs(Speed1));

}

}

/**

* Function       sleft

* @author        wusicaijuan

* @date          2019.06.25

* @brief         turn left(Left wheel stop，Right wheel advance)

* @param[in]     Speed

* @param[out]    void

* @retval        void

* @par History   no

*/

void sleft(float Speed)

{

pwm.setPWM(10, 0, Speed); //Right front wheel Forword

pwm.setPWM(11, 0, 0);

pwm.setPWM(8, 0, Speed);   //Right rear wheel Forword

pwm.setPWM(9, 0, 0);

pwm.setPWM(13, 0, 0);     //Left front wheel Back

pwm.setPWM(12, 0, Speed);

pwm.setPWM(15, 0, 0);     //Left rear wheel Back

pwm.setPWM(14, 0, Speed);

}

/**

* Function       sright

* @author        wusicaijuan

* @date          2019.06.25

* @brief         spin_right(Left wheel advance，Right wheel back)

* @param[in]     Speed

* @param[out]    void

* @retval        void

* @par History   no

*/

void sright(float Speed)

{

pwm.setPWM(10, 0, 0);

pwm.setPWM(11, 0, Speed);   //Right front wheel Back

pwm.setPWM(8, 0, 0);

pwm.setPWM(9, 0, Speed);   //Right rear wheel Back

pwm.setPWM(13, 0, Speed);   //Left front wheel Forword

pwm.setPWM(12, 0, 0);

pwm.setPWM(15, 0, Speed);   //Left rear wheel Forword

pwm.setPWM(14, 0, 0);

}

/**

* Function       keysacn

* @author        wusicaijuan

* @date          2019.06.04

* @brief         keysacn

* @param[in1]    void

* @retval        void

* @par History   no

*/

void keysacn()

{

int val;

val = digitalRead(key); //Read the level value of   digital 7 port assigned to val

while (val == HIGH)   //Cycles when the button is not pressed

{

val = digitalRead(key);

}

while (val == LOW)     //When button is not pressed

{

delay(1);

val = digitalRead(key); //Read the level value of   digital 7 port assigned to val

while (val == HIGH)   //Determine if the button is released

{

break;

}

}

}

/*

* Function       Clear_All_PWM

* @author        wusicaijuan

* @date          2019.07.04

* @brief         Turn off PWM

* @param[in]     void

* @param[out]    void

* @retval        void

* @par History   no

*/

void Clear_All_PWM()

{

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

{

pwm.setPWM(i, 0, 0);

}

}

② Tracking_test.ino

/*

* @par Copyright (C): 2010-2019, Shenzhen Yahboom Tech

* @file         Tracking_test.c

* @author       wusicaijuan

* @version      V1.0

* @date         2019.08.05

* @brief        Tracking_test

* @details

* @par History   

*

*/

#include "Adafruit_PWMServoDriver.h"

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);

int sensor[3];

const int key = 7; //Define key pin

void keyscan(void);

void Clear_All_PWM(void);

#define   L_Value   analogRead(A2)

#define   M_Value   analogRead(A1)

#define   R_Value   analogRead(A0)

/**

* Function       setup

* @author        liusen

* @date          2017.07.25

* @brief         Initial configuration

* @param[in]     void

* @retval        void

* @par History   no

*/

void setup()

{

  // put your setup code here, to run once:

  pwm.begin();

  pwm.setPWMFreq(60); // Analog servos run at ~60 Hz updates

  Clear_All_PWM();

  pinMode(A0, INPUT);

  pinMode(A1, INPUT);

  pinMode(A2, INPUT);

  pinMode(key, INPUT);

  keysacn();

}

/**

* Function       loop

* @author        wusicaijuan

* @date          2019.07.30

* @brief         main fuction

* @param[in]     void

* @retval        void

* @par History   no

*/

void loop()

{

  read_sensor_values();

}

/**

* Function       read_sensor_values

* @author        wusicaijuan

* @date          2019.07.30

* @brief         read sensor value to change car movement

* @param[in]     void

* @retval        void

* @par History   no

*/

/*450，350，400分别是中间探头左侧探头右侧探头黑白线之间的临界值,

  * 请用户一定要根据实际情况打印观察这三个数据，并进行修改。

  */

void read_sensor_values()

{

    if(M_Value>340)  

    {

      run(65);

    }

    else if(L_Value > 350)

    {

      sleft(75);

      while(L_Value>350);

    }

    else if(R_Value > 400)   

    {

      sright(75);

      while(R_Value > 400);   

    }

}

/**

* Function       run

* @author        wusicaijuan

* @date          2019.06.25

* @brief         run

* @param[in]     Speed

* @param[out]    void

* @retval        void

* @par History   no

*/

void run(float Speed)

{

  Speed = map(Speed, 0, 255, 0, 4095);

  pwm.setPWM(10, 0, Speed); //Right front wheel Forward

  pwm.setPWM(11, 0, 0);

  pwm.setPWM(8, 0, Speed); //Right rear wheel Forward

  pwm.setPWM(9, 0, 0);

  pwm.setPWM(13, 0, Speed); //Left front wheel Forward

  pwm.setPWM(12, 0, 0);

  pwm.setPWM(15, 0, Speed); //Left front wheel Forward

  pwm.setPWM(14, 0, 0);

}

/**

* Function       left

* @author        wusicaijuan

* @date          2019.06.26

* @brief         turn left(Left wheel stop，Right wheel advance)

* @param[in]     Speed

* @param[out]    void

* @retval        void

* @par History   no

*/

void left(float Speed)

{

  Speed = map(Speed, 0, 255, 0, 4095);

  pwm.setPWM(10, 0, Speed);   //Right front wheel Reverse

  pwm.setPWM(11, 0, 0);

  pwm.setPWM(8, 0, Speed);   //Right rear wheel Reverse

  pwm.setPWM(9, 0, 0);

  pwm.setPWM(13, 0, 0);     //Left front wheel Stop

  pwm.setPWM(12, 0, 0);

  pwm.setPWM(15, 0, 0);

  pwm.setPWM(14, 0, 0);

}

/**

* Function       right

* @author        wusicaijuan

* @date          2019.06.26

* @brief         turn right (Left wheel advance，Right wheel stop)

* @param[in]     Speed

* @param[out]    void

* @retval        void

* @par History   no

*/

void right(float Speed)

{

  Speed = map(Speed, 0, 255, 0, 4095);

  pwm.setPWM(10, 0, 0);   //Right front wheel Stop

  pwm.setPWM(11, 0, 0);

  pwm.setPWM(8, 0, 0);

  pwm.setPWM(9, 0, 0);

  pwm.setPWM(13, 0, Speed);   //Left front wheel Reverse

  pwm.setPWM(12, 0, 0);

  pwm.setPWM(15, 0, Speed); //Left rear wheel Reverse

  pwm.setPWM(14, 0, 0);

}

/**

* Function       sleft

* @author        wusicaijuan

* @date          2019.06.25

* @brief         spin_left(Left wheel back，Right wheel advance)

* @param[in]     Speed

* @param[out]    void

* @retval        void

* @par History   no

*/

void sleft(float Speed)

{

  Speed = map(Speed, 0, 255, 0, 4095);

  pwm.setPWM(10, 0, Speed);   //Right front wheel Forword

  pwm.setPWM(11, 0, 0);

  pwm.setPWM(8, 0, Speed);   //Right rear wheel Forword

  pwm.setPWM(9, 0, 0);

  pwm.setPWM(13, 0, 0);      //Left front wheel Back

  pwm.setPWM(12, 0, Speed);

  pwm.setPWM(15, 0, 0);

  pwm.setPWM(14, 0, Speed);   //Left rear wheel Back

}

/**

* Function       sright

* @author        wusicaijuan

* @date          2019.06.25

* @brief         spin_right(Left wheel advance，Right wheel back)

* @param[in]     Speed

* @param[out]    void

* @retval        void

* @par History   no

*/

void sright(float Speed)

{

  Speed = map(Speed, 0, 255, 0, 4095);

  pwm.setPWM(10, 0, 0);

  pwm.setPWM(11, 0, Speed);   //Right front wheel Back

  pwm.setPWM(8, 0, 0);

  pwm.setPWM(9, 0, Speed);   //Right rear wheel Back

  pwm.setPWM(13, 0, Speed);   //Left front wheel Forword

  pwm.setPWM(12, 0, 0);

  pwm.setPWM(15, 0, Speed);   //Left rear wheel Forword

  pwm.setPWM(14, 0, 0);

}

/**

* Function       keysacn

* @author        wusicaijuan

* @date          2019.06.04

* @brief         keysacn

* @param[in1]    void

* @retval        void

* @par History   no

*/

void keysacn()

{

  int val;

  val = digitalRead(key); //Read the level value of   digital 7 port assigned to val

  while (val == HIGH)     //Cycles when the button is not pressed

  {

    val = digitalRead(key);

  }

  while (val == LOW)      //When button is not pressed

  {

    delay(10);       

    val = digitalRead(key); //Read the level value of   digital 7 port assigned to val

    while (val == HIGH)     //Determine if the button is released

    {

      break;

    }

  }

}

/*

* Function       Clear_All_PWM

* @author        wusicaijuan

* @date          2019.07.04

* @brief         Turn off PWM

* @param[in]     void

* @param[out]    void

* @retval        void

* @par History   no

*/

void Clear_All_PWM()

{

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

  {

    pwm.setPWM(i, 0, 0);

  }

}

③ TrackingSensorTest.ino

//30    27    29

void setup()

{

Serial.begin(115200);

pinMode(A0, INPUT);

pinMode(A1, INPUT);

pinMode(A2, INPUT);

}

void loop()

{

delay(50);

Serial.print(analogRead(A0));   

Serial.print(",");

Serial.print(analogRead(A1));

Serial.print(",");

Serial.println(analogRead(A2));

}

实验现象

对于程序Tracking_test.ino，首先打开在TrackingSensorTest文件夹中 TrackingSensorTest.ino。并通过数据线将小车（或者是已经插入扩展板的 Uno板）与电脑连接。然后将三路巡线模块处于白色底部上（必须是你的小车将要行驶的巡线赛道的白色底部），如下图所示将三路巡线模块处于黑色赛道上（必须是你的小车将要行驶的巡线黑色赛道）。

在Arduino IDE界面的右上角打开串口监视器，需要选择与程序中所设置的相同的波特率。

当巡线传感器的三个探头检测到白色时，检测到黑色时，打印出当前输出的模拟值。

如果数据显示一点波动，这是正常的。

打开 Tracking_test 文件夹中的 Tracking_test.ino 文件，并且根据上一步打印出来的数值，取一个最佳的临界值，修改程序中的数据。

修改完成之后，保存程序并将程序下载小车上。

对于程序：Tracking_PID.ino首先，打开在TrackingSensorTest 文件夹中 TrackingSensorTest.ino。并通过数据线将小车（或者是已经插入扩展板的 Uno 板）与电脑连接。然后，将三路巡线模块处于白色底部上（必须是你的小车将要行驶的巡线赛道的白色底部）。

在 Arduino IDE 界面的右上角打开串口监视器，需要选择与程序中所设置的相同的波特率。

当巡线传感器的三个探头检测到白色时，打印出当前输出的模拟值。

如果数据显示一点波动，这是正常的，可以取五个数据的平均值。

打开Tracking_PID 文件夹中的 Tracking_PID.ino 文件，并且修改程序中的数据。

1.2.3 WIFI摄像头控制

实验原理：根据硬件接口速查手册可知，WIFI 摄像头模块是通过串口进行通讯的。

实验程序

WIFI模块（WIFI_control_car_15.ino）

/**

* @par Copyright (C): 2010-2019, Shenzhen Yahboom Tech

* @file         Wifi control car.c

* @author       Cindy

* @version      V1.0

* @date         2019.09.11

* @brief       

* @details

* @par History  

*

*/

#include <Arduino.h>

#include "Adafruit_PWMServoDriver.h"

#include "Adafruit_NeoPixel.h"       

#include "RGBLed.h"

#include "avr/pgmspace.h"

#define RGB_GREEN    0xFF0000    //定义RGB灯的颜色

#define RGB_RED   0x00FF00

#define RGB_BLUE    0x0000FF

#define RGB_YELLOW   0xFFFF00

#define RGB_PURPLE   0x00FFFF

#define RGB_WHITE   0xFFFFFF

#define RGB_CYAN   0xFF00FF

#define RGB_OFF   0x000000

const int RgbPin = 11;    //定义七彩超声波模块上面RGB灯的引脚

RGBLed mRgb(RgbPin,2);

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);

#define SERVOMIN 150     

#define SERVOMAX 600     

#define PIN 6   //定义RGB的引脚

#define MAX_LED 1 //扩展板上仅有一个同类型的板载RGB灯

Adafruit_NeoPixel strip = Adafruit_NeoPixel(MAX_LED, PIN, NEO_RGB + NEO_KHZ800);

const char enServo[] = {0, 1, 2, 3};

const unsigned char music_max[5] = {42,39,36,70,25};   //音乐歌曲的最大长度

#define run_car '1'       

#define back_car '2'     

#define left_car '3'     

#define right_car '4'   

#define spin_left_car '5'  

#define spin_right_car '6'

#define stop_car '7'     

#define H_servoL 'L'       

#define H_servoR 'R'     

#define H_servoS 'S'   

#define V_servoU 'U'

#define V_servoD 'D'

#define V_servoS 'S'

int Servo_LR = 90;

int Servo_UD = 90;

const int flag_time = 2000; //时间标记点间隔2s

int newtime = 0;            //记录系统当前的时间

int lasttime = 0;           //记录系统上一次的时间点

int g_num = 0;

int buzzer = 10;            //定义蜂鸣器的引脚

int CarSpeedControl = 80;   //set speed of car

int SingPin = 12;         

int distance = 0;

int red, green, blue;

int initial_motor_speed = 100;

int sensor[3];

int time = 40000;

int count = 10;

/*小车运动状态枚举*/

const typedef enum {

  enRUN = 1,

  enBACK,

  enLEFT,

  enRIGHT,

  enSPINLEFT,

  enSPINRIGHT,

  enSTOP

} enCarState;

/*舵机状态枚举*/

const typedef enum {

  enHServoL = 1,

  enHServoR,

  enHServoS

} enHServoState;

int IncomingByte = 0;            //接收到的数据字节

String InputString = "";         //用来存储接收到的数据

boolean NewLineReceived = false; //上一次数据结束的标志

boolean StartBit   = false;       //协议开始标志

String ReturnTemp = "";          //用来存储返回值

static int g_CarState = enSTOP;         //1前进 2后退 3左转 4 右转 0停止

static int g_HServoState = enHServoS; //1:左转 2:右转 3:停止

int g_modeSelect = 0;   //0:默认的状态

int g_modeMusic = 0; //0:默认的状态

int g_musicSelect = 1;

int g_modeCar = 0;

boolean g_motor = false;

//Music

enum enMusic

  {

    enStar=1,

    Bingo=2,

    MerryChristmas=3,

    Ode=4,

    Birthday=5

  };

#define G5 392

#define A6 440

#define B7 494

#define c1 525

#define d2 587

#define e3 659

#define f4 698

#define g5 784

#define a6 880

#define b7 988

#define C1 1047

#define D2 1175

#define E3 1319

#define F4 1397

#define GG5 1568

#define AA6 1769

#define g4 392

#define c5 523

#define a4 440

#define d5 587

#define e5 659

#define b4 494

#define c6 1047

#define d6 1175

#define b5 988

#define a5 880

#define g5 784

#define e6 1319

#define f6 1397

#define a5 880

#define f5 698

const PROGMEM   int ODe[70][2]   //歌曲--欢乐颂

{

  {e3,2},{e3,2},{f4,2},{g5,2},{g5,2},{f4,2},{e3,2},{d2,2},{c1,2},{c1,2},{d2,2},{e3,2},{e3,3},{d2,1},{d2,4},

  {e3,2},{e3,2},{f4,2},{g5,2},{g5,2},{f4,2},{e3,2},{d2,2},{c1,2},{c1,2},{d2,2},{e3,2},{d2,3},{c1,1},{c1,4},

  {d2,2},{d2,2},{e3,2},{c1,2},{d2,2},{e3,1},{f4,1},{e3,2},{c1,2},

  {d2,2},{e3,1},{f4,1},{e3,2},{d2,2},{c1,2},{d2,2},{G5,2},

  {e3,2},{e3,2},{f4,2},{g5,2},{g5,2},{f4,2},{e3,2},{d2,2},{c1,2},{c1,2},{d2,2},{e3,2},{d2,3},{c1,1},{c1,4},

};

const PROGMEM   int BIrthday[25][2] //歌曲--生日快乐

{

  {G5,2},{A6,2},{G5,2},{c1,2},{B7,4},

  {G5,2},{A6,2},{G5,2},{d2,2},{c1,4},

  {G5,2},{g5,2},{e3,2},{c1,2},{B7,2},{A6,2},

  {f4,2},{e3,2},{c1,2},{d2,2},{c1,2}

};

const PROGMEM   int STar[42][2] //歌曲--小星星

{

  {c1,2},{c1,2},{g5,2},{g5,2},{a6,2},{a6,2},{g5,4},

  {f4,2},{f4,2},{e3,2},{e3,2},{d2,2},{d2,2},{c1,4},

  {g5,2},{g5,2},{f4,2},{f4,2},{e3,2},{e3,2},{d2,4},

  {g5,2},{g5,2},{f4,2},{f4,2},{e3,2},{e3,2},{d2,4},

  {c1,2},{c1,2},{g5,2},{g5,2},{a6,2},{a6,2},{g5,4},

  {f4,2},{f4,2},{e3,2},{e3,2},{d2,2},{d2,2},{c1,4},

};

const PROGMEM   int MErryChristmas[36][2]   //歌曲--圣诞快乐

{

  {g5,1},{g5,1},{c6,2},{c6,1},{d6,1},{c6,1},{b5,1},{a5,2},{a5,2},{0,2},

  {a5,1},{a5,1},{d6,2},{d6,1},{e6,1},{d6,1},{c6,1},{b5,2},{g5,2},{0,2},

  {g5,1},{g5,1},{e6,2},{e6,1},{f6,1},{e6,1},{d6,1},{c6,2},{a5,2},{0,2},

  {g5,1},{g5,1},{a5,1},{d6,1},{b5,1},{c6,2}

};

const PROGMEM   int BIngo[39][2]   //歌曲--宾果

{

  {g4,1},{c5,1},{c5,1},{c5,1},{g4,1},{a4,1},{a4,1},{g4,1},{g4,1},

  {c5,1},{c5,1},{d5,1},{d5,1},{e5,2},{c5,1},{0,1},

  {e5,2},{e5,2},{f5,1},{f5,1},{f5,2},{d5,2},{d5,2},{e5,1},{e5,1},{e5,2},

  {c5,2},{c5,2},{d5,1},{d5,1},{d5,1},{c5,1},{b4,1},{g4,1},{a4,1},{b4,1},{c5,2},{c5,1},{c5,1}

};

int serial_putc( char c, struct __file * )

{

  Serial.write( c );

  return c;

}

void printf_begin(void)

{

  fdevopen( &serial_putc, 0 );

}

/*

* Function       Clear_All_PWM

* @author        wusicaijuan

* @date          2019.07.04

* @brief         关闭PCA9685的所有PWM

* @param[in]     void

* @param[out]    void

* @retval        void

* @par History   No

*/

void Clear_All_PWM()

{

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

  {

    pwm.setPWM(i, 0, 0);

  }

}

/**

* Function       setup

* @author        Cindy

* @date          2019.09.11

* @brief         初始化设置

* @param[in]     void

* @retval        void

* @par History   no

*/

void setup()

{

  Serial.begin(9600);

  printf_begin();

  strip.begin();

  strip.show();

  pwm.begin();

  pwm.setPWMFreq(60);

  Clear_All_PWM();

  Servo180(1,90); //将三个舵机都初始化至90度

  Servo180(2,90);

  Servo180(3,90);

  breathing_light(255, 40, 5);

  pinMode(buzzer, OUTPUT);

  digitalWrite(buzzer, HIGH);

}

/**

* Function       setBuzzer_Tone

* @author        Cindy

* @date          2019.09.02

* @brief         设置蜂鸣器的音调

* @param[in]     void

* @param[out]    void

* @retval        void

* @par History   no

*/

void setBuzzer_Tone(uint16_t frequency, uint32_t duration)

{

  int period = 1000000L / frequency;//1000000L

    int pulse = period / 2;

 

    for (long i = 0; i < duration * 200000L; i += period)

    {

    digitalWrite(buzzer, 1);

      delayMicroseconds(pulse);

      digitalWrite(buzzer, 0);

      delayMicroseconds(pulse);

    }

    if(frequency==0)

    delay(230*duration);  

  delay(20);

}

/**

* Function       5-music

* @author        Cindy

* @date          2019.09.02

* @brief         五首歌曲

* @param[in]     void

* @param[out]    void

* @retval        void

* @par History   no

*/

void birthday(int j)    //生日快乐

{

    setBuzzer_Tone(pgm_read_word_near(&BIrthday[j][0]), pgm_read_word_near(&BIrthday[j][1]));

}

void ode(int j)     //欢乐颂

{

    setBuzzer_Tone(pgm_read_word_near(&ODe[j][0]), pgm_read_word_near(&ODe[j][1]));

}

void star(int j)   //小星星

{

    setBuzzer_Tone(pgm_read_word_near(&STar[j][0]), pgm_read_word_near(&STar[j][1]));

}

void merryChristmas(int j)   //圣诞快乐

{

    setBuzzer_Tone(pgm_read_word_near(&MErryChristmas[j][0]), pgm_read_word_near(&MErryChristmas[j][1]));

}

void bingo(int j)   //宾果

{

    setBuzzer_Tone(pgm_read_word_near(&BIngo[j][0]), pgm_read_word_near(&BIngo[j][1]));

}

/**

* Function       music_Play

* @author        Cindy

* @date          2019.09.11

* @brief         播放音乐

* @param[in]     void

* @param[out]    void

* @retval        void

* @par History   no

*/

void music_Play(uint8_t v_song, uint8_t index)

{

  switch(v_song)

  {

    case enStar:

    {

      star(index);

      break;

    }

    case Bingo:

    {

      bingo(index);   

      break;

    }

    case MerryChristmas:

    {

      merryChristmas(index);     

      break;

    }

    case Ode:

    {

      ode(index);

      break;

    }

    case Birthday:

    {

      birthday(index);  

      break;

    }

  }

}

/*

* Function      Servo180(num, degree)

* @author       wusicaijuan

* @date         2019.06.25

* @bried        控制180度舵机旋转

* @param[in1]   index

                    1: s1

                    2: s2

                    3: s3

                    4: s4

* @param[in2]   degree (0 <= degree <= 180)

* @retval       void

*/

void Servo180(int num, int degree)

{

  long us = (degree * 1800 / 180 + 600); // 0.6 ~ 2.4

  long pwmvalue = us * 4096 / 20000;   // 50hz: 20,000 us

  pwm.setPWM(enServo[num - 1], 0, pwmvalue);

}

/**

* Function       Distance_test

* @author        Cindy

* @date          2019.09.11

* @brief         超声波测距

* @param[in]     void

* @param[out]    void

* @retval        void

* @par History   no

*/

void Distance_test()

{

  pinMode(SingPin,OUTPUT);

  digitalWrite(SingPin, LOW);

  delayMicroseconds(2);

  digitalWrite(SingPin, HIGH);

  delayMicroseconds(15);

  digitalWrite(SingPin, LOW);

 

  pinMode(SingPin, INPUT);

  delayMicroseconds(50);

  distance = pulseIn(SingPin, HIGH) / 58;

  //Serial.print("distance is :");

  //Serial.print(distance);

  //Serial.print("cm");

  //Serial.println();

  //delay(1000);

}

/**

* Function       PCB_RGB(R,G,B)

* @author        wusicaijuan

* @date          2019.06.26

* @brief         控制板载的RGB

* @param[in1]    R

* @param[in2]    G

* @param[in3]    B

* @param[out]    void

* @retval        void

* @par History   

*

*/

void PCB_RGB(int R, int G, int B)

{

  uint8_t i = 0;

  R = map(R, 0, 255, 0, 200);  

  G = map(G, 0, 255, 0, 200);

  B = map(B, 0, 255, 0, 200);

  uint32_t color = strip.Color(G, R, B);

  strip.setPixelColor(i, color);

  strip.show();

}

/**

* Function       Ultrasonic_RGB(R,G,B)

* @author        wusicaijuan

* @date          2019.06.26

* @brief         控制超声波模块上面的七彩灯

* @param[in1]    R

* @param[in2]    G

* @param[in3]    B

* @param[out]    void

* @retval        void

* @par History   no

*/

void   Ultrasonic_RGB(int R, int G, int B)

{

  mRgb.setColor(0,G,R,B);

  mRgb.show();

}

/**

* Function       advance

* @author        wusicaijuan

* @date          2019.06.25

* @param[in]     小车前进

* @param[out]    void

* @retval        void

* @par History   no

*/

void advance(int Speed)

{

  Speed = map(Speed, 0, 255, 0, 4095);

  pwm.setPWM(10, 0, Speed); //右前方车轮前进

pwm.setPWM(11, 0, 0);

  pwm.setPWM(8, 0, Speed); //右后方车轮前进

  pwm.setPWM(9, 0, 0);

  pwm.setPWM(13, 0, Speed); //左前方车轮前进

  pwm.setPWM(12, 0, 0);

  pwm.setPWM(15, 0, Speed); //左后方车轮前进

  pwm.setPWM(14, 0, 0);

}

/**

* Function       brake

* @author        wusicaijuan

* @date          2019.06.25

* @brief         小车停止

* @param[in]     void

* @param[out]    void

* @retval        void

* @par History   no

*/

void brake()

{

pwm.setPWM(8, 0, 0);

  pwm.setPWM(9, 0, 0);

pwm.setPWM(11, 0, 0);

  pwm.setPWM(10, 0, 0);

  pwm.setPWM(12, 0, 0);

  pwm.setPWM(13, 0, 0);

  pwm.setPWM(14, 0, 0);

  pwm.setPWM(15, 0, 0);

}

/**

* Function       left

* @author        wusicaijuan

* @date          2019.06.26

* @brief         小车左转

* @param[in]     Speed

* @param[out]    void

* @retval        void

* @par History   no

*/

void left(int Speed)

{

  Speed = map(Speed, 0, 255, 0, 4095);

  pwm.setPWM(10, 0, Speed);     //右前方车轮前进

  pwm.setPWM(11, 0, 0);

  pwm.setPWM(8, 0, Speed);      //右后方车轮前进

  pwm.setPWM(9, 0, 0);     

  pwm.setPWM(13, 0, 0);         //左侧车轮停止

  pwm.setPWM(12, 0, 0);

  pwm.setPWM(15, 0, 0);

  pwm.setPWM(14, 0, 0);

}

/**

* Function       right

* @author        wusicaijuan

* @date          2019.06.26

* @brief         小车右转

* @param[in]     Speed

* @param[out]    void

* @retval        void

* @par History   no

*/

void right(int Speed)

{

  Speed = map(Speed, 0, 255, 0, 4095);

  pwm.setPWM(10, 0, 0);        //右侧车轮停止

  pwm.setPWM(11, 0, 0);

  pwm.setPWM(8, 0, 0);

  pwm.setPWM(9, 0, 0);

  pwm.setPWM(13, 0, Speed);    //左前方车轮前进

  pwm.setPWM(12, 0, 0);

  pwm.setPWM(15, 0, Speed);    //左后方车轮前进

  pwm.setPWM(14, 0, 0);

}

/**

* Function       spin_left

* @author        wusicaijuan

* @date          2019.06.25

* @brief         左旋(左轮后退，右轮前进)

* @param[in]     Speed

* @param[out]    void

* @retval        void

* @par History   no

*/

void spin_left(int Speed)

{

  Speed = map(Speed, 0, 255, 0, 4095);

  pwm.setPWM(10, 0, Speed);      //右前方车轮前进

  pwm.setPWM(11, 0, 0);

  pwm.setPWM(8, 0, Speed);       //右前方车轮前进

  pwm.setPWM(9, 0, 0);

  pwm.setPWM(13, 0, 0);

  pwm.setPWM(12, 0, Speed);      //左前方车轮后退

  pwm.setPWM(15, 0, 0);

  pwm.setPWM(14, 0, Speed);      //左后方车轮后退

}

/**

* Function       spin_right

* @author        wusicaijuan

* @date          2019.06.25

* @brief         右旋(左轮前进，右轮后退)

* @param[in]     Speed

* @param[out]    void

* @retval        void

* @par History   no

*/

void spin_right(int Speed)

{

  Speed = map(Speed, 0, 255, 0, 4095);

  pwm.setPWM(10, 0, 0);

  pwm.setPWM(11, 0, Speed);    //右前方车轮后退

  pwm.setPWM(8, 0, 0);

  pwm.setPWM(9, 0, Speed);     //右后方车轮后退

  pwm.setPWM(13, 0, Speed);    //左前方车轮前进

  pwm.setPWM(12, 0, 0);

  pwm.setPWM(15, 0, Speed);    //左后方车轮前进

  pwm.setPWM(14, 0, 0);

}

/**

* Function       back

* @author        wusicaijuan

* @date          2019.06.25

* @brief         小车后退

* @param[in]     Speed

* @param[out]    void

* @retval        void

* @par History   No

*/

void back(int Speed)

{

  Speed = map(Speed, 0, 255, 0, 4095);

  pwm.setPWM(10, 0, 0);

  pwm.setPWM(11, 0, Speed); //右前方车轮后退

  pwm.setPWM(8, 0, 0);

  pwm.setPWM(9, 0, Speed); //右后方车轮后退

  pwm.setPWM(13, 0, 0);

  pwm.setPWM(12, 0, Speed);   //左前方车轮后退

  pwm.setPWM(15, 0, 0);

  pwm.setPWM(14, 0, Speed); //左后方车轮后退

}

/**

* Function       whistle

* @author        Cindy

* @date          2019.09.11

* @brief         鸣笛

* @param[in]     void

* @param[out]    void

* @retval        void

* @par History   no

*/

void whistle()

{

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

  {

    digitalWrite(buzzer, HIGH); //发出声音

    delay(3);         

    digitalWrite(buzzer, LOW);   //不发出声音

    delay(1);         

  }

}

/**

* Function       breathing_light(brightness,time,increament)

* @author        wusicaijuan

* @date          2019.06.26

* @brief         logo内嵌的蓝色RGB灯

* @param[in1]    brightness

* @param[in2]    time

* @param[in3]    increament

* @param[out]    void

* @retval        void

* @par History   no

*/

void breathing_light(int brightness, int time, int increament)

{

  if (brightness < 0)

  {

    brightness = 0;

  }

  if (brightness > 255)

  {

    brightness = 255;

  }

  for (int b = 0; b < brightness; b += increament)

  {

    int newb = map(b, 0, 255, 0, 4095);

    pwm.setPWM(7, 0, newb);

    delay(time);

  }

}

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

/*模式3-巡线模式*/

/**

* Function       Tracking_Mode

* @author        Cindy

* @date          2019.09.11

* @brief         巡线

* @param[in1]    void

* @param[out]    void

* @retval        void

* @par History   no

*/

//可根据实际情况修改程序中的参数

void Tracking_Mode()

{

  sensor[0] = analogRead(A0);

  sensor[1] = analogRead(A1);

  sensor[2] = analogRead(A2);

  if(sensor[0]>100)   //三个参数100，100，100大家可根据实际情况进行调节

  {

    sensor[0] = 1;

  }

  else

  {

    sensor[0] = 0;

  }

  if(sensor[1]>100)

  {

    sensor[1] = 1;

  }

  else{

    sensor[1] = 0;

  }

  if(sensor[2]>100)

  {

    sensor[2] = 1;

  }

  else

  {

    sensor[2] = 0;

  }

 

  if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1))

  {

    spin_left(80);

  }

  else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1))

  {

    left(100);

  }

  else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 0))

  {

    advance(70);

  }

  else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 0))

  {

    right(100);

  }

  else if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0))

  {

    spin_right(80);

  }

  else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0))

  {

    //Clear_All_PWM();//全部检测到白色,保持上一个状态

  }

  else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1))

  {

    //Clear_All_PWM();//全部检测到黑色,保持上一个状态

  }

}

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

/*模式2: 超声波避障模式*/

/**

* Function       bubble

* @author        Cindy

* @date          2019.09.11

* @brief         采用冒泡排序法。

* @param[in1]    a:超声波数组的首地址

* @param[in2]    n:超声波数组的大小

* @param[out]    void

* @retval        void

* @par History   no

*/

void bubble(unsigned long *a, int n)

{

  int i, j, temp;

  for (i = 0; i < n - 1; i++)

  {

    for (j = i + 1; j < n; j++)

    {

      if (a[i] > a[j])

      {

        temp = a[i];

        a[i] = a[j];

        a[j] = temp;

      }

    }

  }

  return;

}

/**

* Function       Distance

* @author        Cindy

* @date          2019.09.11

* @brief         去掉最大值和最小值，取中间三个数据的平均值

* @param[in]     void

* @param[out]    void

* @retval        void

* @par History   No

*/

void Distance()

{

  unsigned long ultrasonic[5] = {0};

  int num = 0;

  while (num < 5)

  {

    Distance_test();

    ultrasonic[num] = distance;

    //printf("L%d:%d\r\n", num, (int)distance);

    num++;

    delay(10);

  }

  num = 0;

  bubble(ultrasonic, 5);

  distance = (ultrasonic[1] + ultrasonic[2] + ultrasonic[3]) / 3;

  return;

}

enum {

  LEFT_DIRECTION,

  RIGHT_DIRECTION,

  FRONT_DIRECTION,

  ALL_CHECKED_START_ACTION

};

/**

* Function       ult_check_distance_and_action

* @author        Cindy

* @date          2019.09.11

* @brief         舵机转动检测距离

* @param[in]     void

* @param[out]    void

* @retval        void

* @par History   no

*/

int ult_check_distance_and_action(uint8_t p_direction)

{

  static int LeftDistance = 0;    //LeftDistance

  static int RightDistance = 0;   //RightDistance

  static int FrontDistance = 0;   //FrontDistance

  static int cnt = 0;

  int ret = 0;

  if (0 == g_modeSelect)

  {

    cnt = 0;

    brake();

    LeftDistance = 0;

    RightDistance = 0;

    FrontDistance = 0;

    ret = -1;

    return ret;

  }

  mRgb.setColor(0,RGB_GREEN);  

  mRgb.show();

  if (LEFT_DIRECTION == p_direction)

  {

    Servo180(1, 180);     

  } else if (RIGHT_DIRECTION == p_direction)

  {

    Servo180(1, 0);     

  } else if (FRONT_DIRECTION == p_direction)

  {

    Servo180(1, 90);     

  }

  else if (ALL_CHECKED_START_ACTION == p_direction)   //舵机转动左,右,前三个方向完成测距

  {

    if (0 == cnt)

    {

      brake();

      delay(50);

    }

    cnt++;   

    if (LeftDistance < 25 && RightDistance < 25 && FrontDistance < 25)   //比较三个方向的距离

    {

      mRgb.setColor(0,RGB_PURPLE);  

      mRgb.show();

      spin_right(80);     

      delay(19);

    } else if (LeftDistance >= RightDistance)

{

      mRgb.setColor(0,RGB_BLUE);  

      mRgb.show();

      spin_left(80);     

      delay(13);

    } else if (LeftDistance < RightDistance)

    {

      mRgb.setColor(0,RGB_YELLOW);  

      mRgb.show();

      spin_right(80);   

      delay(13);

    }

    if (cnt > 50)    //此处是控制小车左旋、右旋的时间19*50/13*50

    {

      brake();

      LeftDistance = 0;

      RightDistance = 0;

      FrontDistance = 0;

      cnt = 0;

      ret = 1;

      return ret;

    }

    else

    {

      return ret;

    }

  }

    delay(20);

    cnt++;

    if (cnt > 20)   //这里是用来控制,让舵机每次转动之后延迟20ms

    {

      cnt = 0;

      Distance();

      if (LEFT_DIRECTION == p_direction)

      {

        LeftDistance = distance;

      } else if (RIGHT_DIRECTION == p_direction)

      {

        RightDistance = distance;

      } else if (FRONT_DIRECTION == p_direction)

      {

        FrontDistance = distance;

      }

      ret = 1;

    }

  return ret;

}

/**

* Function       UltrasonicAvoidServoMode

* @author        Cindy

* @date          2019.09.11

* @brief         超声波避障模式

* @param[in]     void

* @param[out]    void

* @retval        void

* @par History   no

*/

void UltrasonicAvoidServoMode()

{

  static int cnt_1 = 0;

  static int distance_smaller_25 = 0;

  static int bak_distance = 0;

  int ret = 0;

  if (0 == distance_smaller_25)

  {

    Distance_test();

    bak_distance = distance;

  }

  if (bak_distance < 25)    //如果检测到的距离小于25，小车开始避障

  {

    if (0 == distance_smaller_25)

    {

      cnt_1 = 0;

      distance_smaller_25 = 1;

      brake();

      delay(50);

    }

    ret = ult_check_distance_and_action(cnt_1);

    if (-1 == ret)

    {

      distance_smaller_25 = 0;

      bak_distance = 0;

      cnt_1 = 0;

      return;

    }

    else if (1 == ret)

   {

      cnt_1 ++;

    }

    if (4 == cnt_1)

    {

      distance_smaller_25 = 0;

      bak_distance = 0;

      cnt_1 = 0;

    }

  }

  else if(bak_distance >= 25)     //如果距离大于25，小车保持前进

  {

    if (1 == distance_smaller_25)

    {

      distance_smaller_25 = 0;

    }

    bak_distance = 0;

    advance(95);

  }

}

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

/*模式1---七彩灯模式*/

/**

* Function       color_light

* @author        Cindy

* @date          2019.09.11

* @brief         通过舵机转动到不同的角度改变RGB的颜色

* @param[in]     pos:舵机转动的角度

* @param[out]    void

* @retval        void

* @par History   no

*/

void color_light(int pos)

{

  //当舵机转动到150-180度之间，点亮白色

  if (pos > 150)

  {

    mRgb.setColor(0,RGB_WHITE);   

    mRgb.show();

  }

  //当舵机转动到125-150度之间，点亮红色

  else if (pos > 125)

  {

    mRgb.setColor(0,RGB_RED);  

    mRgb.show();

  }

  //当舵机转动到100-125度之间，点亮绿色

  else if (pos > 100)

  {

    mRgb.setColor(0,RGB_GREEN);  

    mRgb.show();

  }

  //当舵机转动到75-100度之间，点亮蓝色

  else if (pos > 75)

mRgb.setColor(0,RGB_BLUE);

    mRgb.show();

  }

  //当舵机转动到50-75度之间，点亮黄色

  else if (pos > 50)

  {

    mRgb.setColor(0,RGB_YELLOW);  

    mRgb.show();

  }

  //当舵机转动到25-50度之间，点亮紫色

  else if (pos > 25)

  {

    mRgb.setColor(0,RGB_PURPLE);  

    mRgb.show();

  }

  //当舵机转动到0-25度之间，点亮青色

  else if (pos > 0)

  {

    mRgb.setColor(0,RGB_CYAN);  

    mRgb.show();

  }

  else

  {

    mRgb.setColor(0,RGB_OFF);  

    mRgb.show();

  }

}

/**

* Function       ServoColorRGBMode

* @author        wusicaijuan

* @date          2019.06.26

* @brief         七彩灯模式

* @param[in1]    brightness

* @param[in2]    time

* @param[in3]    increament

* @param[out]    void

* @retval        void

* @par History   no

*/

static int pos = 0;

static int is_max_pos = 0;

void   ServoColorRGBMode()

{

  if (0 == is_max_pos)

  {

    pos += 25;

    if (pos >= 180)

    {

        is_max_pos = 1;  

    }

  } else {

    pos -= 25;

    if (pos <= 0)

    {

        is_max_pos = 0;

    }

  }

  Servo180(1, pos);

  color_light(pos);

  delay(200);

}

/**

* Function       BeepOnOffMode

* @author        Cindy

* @date          2019.09.11

* @brief         切换模式鸣笛

* @param[in]     void

* @param[out]    void

* @retval        void

* @par History   no

*/

void BeepOnOffMode()

{

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

  {

    digitalWrite(buzzer, HIGH); //发出声音

    delay(3);         

    digitalWrite(buzzer, LOW);   //不发出声音

    delay(1);         

  }

}

/**

* Function       serial_data_parse

* @author        Cindy

* @date          2019.09.11

* @brief         Serial port data parsing and specify the corresponding action

* @param[in]     void

* @param[out]    void

* @retval        void

* @par History   no

*/

void serial_data_parse()

{

    //Serial.println(InputString);

    //解析串口发来指令并控制舵机云台的转动

    //首先，判断是否进入了模式选择

    if (InputString.indexOf("Mode") > 0 && (InputString.length() == 9) )

    {

      if (InputString[6] == '0'&& InputString[7] == '0') //stop

      {

        Clear_All_PWM();

        g_CarState = enSTOP;

        g_modeSelect = 0;

        BeepOnOffMode();

        Servo180(1,90);

        Ultrasonic_RGB(0,0,0);

      }

      else if(InputString[6] != '0' && InputString[7] == '1')

      {

        switch (InputString[6])

        {

          //case '0': g_modeSelect = 0; Clear_All_PWM(); break;

          case '1': g_modeSelect = 1; BeepOnOffMode(); break;

          case '2': g_modeSelect = 2; BeepOnOffMode(); break;

          case '3': g_modeSelect = 3; BeepOnOffMode(); break;

          default: g_modeSelect = 0;   break;

        }

        //delay(1000);

        //BeepOnOffMode();

      }

        InputString = "";                     

        NewLineReceived = false;

        return;

    }

    if (g_modeSelect != 0)

    {

      InputString = "";                   

      NewLineReceived = false;

      return;

    }

   

    if (InputString.indexOf("Music") > 0 && (InputString.length() == 10))

    {

      //Serial.println(InputString);

      g_modeMusic = 1;   //开启音乐模式

      g_musicSelect = (InputString[8] - 0x30)*10 + (InputString[7] - 0x30);   //将字符串转化成数字

      g_num = 0;

      InputString = "";                     

      NewLineReceived = false;

      return;

    }

    //解析上位机发来的舵机云台的控制指令并执行舵机旋转

   if (InputString.indexOf("Servo") > 0 )

    {

    //$Servo,UD180# servo rotate 180

    if (InputString.indexOf("UD") > 0)   //控制垂直方向的舵机（摄像头上下转动）

    {

      int i = InputString.indexOf("UD"); //寻找以PTZ开头,#结束中间的字符

      int ii = InputString.indexOf("#", i);

      if (ii > i)

      {

        String m_skp = InputString.substring(i + 2, ii);

        int m_kp = m_skp.toInt(); //将找到的字符串变成整型

        Servo180(3, 180 - m_kp);   //让舵机转动到指定角度m_kp

      }

      InputString = ""; //清空串口数据

      NewLineReceived = false;

      return; //跳出循环

    }

    //$Servo,LRS#   舵机停止

    //$Servo,LRL#   舵机左转

    //$Servo,LRR#   舵机右转

      if (InputString.indexOf("LR") > 0)

      {

        switch (InputString[9])

        {

        case H_servoL:

          g_HServoState = enHServoL;

          break;

        case H_servoR:

          g_HServoState = enHServoR;

          break;

        case H_servoS:

          g_HServoState = enHServoS;

          break;

        }

         InputString = ""; //清除串口数据

         NewLineReceived = false;

         return;

      }

    }

  //解析上位机发来的通用协议指令,并执行相应的动作

  //如:$1,0,0,0#    小车前进

  //将数据的长度作为判断的依据

  if ((InputString.indexOf("Mode") == -1) && (InputString.indexOf("Servo") == -1) && (InputString.length() == 9))

  {

    //Light up RGB

    if (InputString[7] == '1')     

    {

       int r = random (0, 255);  

       int g = random (0, 255);  

       int b = random (0, 255);

       PCB_RGB(r,g,b);

       Ultrasonic_RGB(r,g,b);

       InputString = ""; //清除串口数据

      NewLineReceived = false;

      return;

    }

     //Make whistle

    if (InputString[5] == '1')     

    {

      whistle();   //鸣笛

    }

    //调节小车的速度

    if (InputString[3] == '1') //加速

    {

      CarSpeedControl += 20;

      if (CarSpeedControl > 150)

      {

        CarSpeedControl = 150;

      }

      InputString = ""; //清除串口数据

      NewLineReceived = false;

      return;

    }

    if (InputString[3] == '2') //减速

    {

      CarSpeedControl -= 20;

      if (CarSpeedControl < 50)

      {

        CarSpeedControl = 50;

      }

      InputString = ""; //清除串口数据

      NewLineReceived = false;

      return;

    }

    switch (InputString[1])

    {

    case run_car:

      g_CarState = enRUN;

      break;

    case back_car:

      g_CarState = enBACK;

      break;

    case left_car:

      g_CarState = enLEFT;

      break;

    case right_car:

      g_CarState = enRIGHT;

      break;

    case spin_left_car:

      g_CarState = enSPINLEFT;

      break;

    case spin_right_car:

      g_CarState = enSPINRIGHT;

      break;

    case stop_car:

      g_CarState = enSTOP;

      break;

    default:

      g_CarState = enSTOP;

      break;

    }

     InputString = "";         //清除串口数据

    NewLineReceived = false;

   

    //控制小车的运动状态

    switch (g_CarState)

    {

    case enSTOP:

      brake();

      break;

    case enRUN:

      advance(CarSpeedControl);

      break;

    case enLEFT:

      left(CarSpeedControl);

      break;

    case enRIGHT:

      right(CarSpeedControl);

      break;

    case enBACK:

      back(CarSpeedControl);

      break;

    case enSPINLEFT:

      spin_left(CarSpeedControl);

      break;

    case enSPINRIGHT:

      spin_right(CarSpeedControl);

      break;

    default:

      brake();

      break;

    }

  }

  InputString = ""; //清除串口数据

  NewLineReceived = false;

  return;

}

/**

* Function       serial_data_postback

* @author        Cindy

* @date          2019.09.11

* @brief         将采集的传感器数据通过串口传输给上位机显示

* @param[in]     void

* @retval        void

* @par History   NO

*/

void serial_data_postback()

{

  /*将超声波的数据传送到APK并且显示出来 */

  //$CSB,000#

  //Ultrasonic Data

  Distance_test();

  ReturnTemp = "$CSB," ;

  ReturnTemp.concat(distance);

  ReturnTemp += "#";

  Serial.print(ReturnTemp);

  return;

}

/*

* Function       HServo_State

* @author        wusicaijuan

* @date          2019.07.04

* @brief         Control state of servo

* @param[in]     void

* @param[out]    void

* @retval        void

* @par History   no

*/

void HServo_State()   //控制摄像头水平方向转动（左---右）

{

  if (g_HServoState != enHServoS)

  {

    if (g_HServoState == enHServoL)

    {

      Servo_LR++;

      if (Servo_LR > 180)

      {

        Servo_LR = 180;

      }

      Servo180(2, Servo_LR);

      delay(5);

    }

    if (g_HServoState == enHServoR)

    {

      Servo_LR--;

      if (Servo_LR < 0)

      {

        Servo_LR = 0;

      }

      Servo180(2, Servo_LR);

      delay(5);

     }

  return;

  }

}

/**

* Function       serialEvent

* @author        Cindy

* @date          2019.09.11

* @brief         

* @param[in]     void

* @param[out]    void

* @retval        void

* @par History   

*/

void serialEvent()   //该函数在Arduino内部被自动调用

{

  while (Serial.available())

  {

    //一个字节一个字节地读,下一句是读到的放入字符串数组中组成一个完成的数据包

    IncomingByte = Serial.read();

    if (IncomingByte == '$')

    {

      StartBit = true;

    }

    if (StartBit == true)

    {

      InputString += (char) IncomingByte;

    }

    if (StartBit == true && IncomingByte == '#')

    {

      NewLineReceived = true;

      StartBit = false;

    }

  }

}

/**

* Function       loop

* @author        Cindy

* @date          2019.09.11

* @brief         主函数

* @param[in]     void

* @retval        void

* @par History   

*/

void loop()

{

  if (NewLineReceived)

  {

    serial_data_parse();  

  }

  newtime = millis();

  if (newtime - lasttime > flag_time)

  {

    lasttime = newtime;

    InputString = "";   //清除串口数据

  }

  switch (g_modeSelect)

  {

    case 1:

      ServoColorRGBMode();   //七彩灯模式

      break;

    case 2:

      UltrasonicAvoidServoMode();//超声波避障模式

      break;

    case 3:

      Tracking_Mode();   //巡线模式

      break;

    case 0:

    default:

      break;

  }

  if( g_modeMusic == 1 )

  {

      switch (g_musicSelect)

      {

        case 11:

          music_Play(1, g_num);

          break;

        case 12:

          music_Play(2, g_num);

          break;

        case 13:

          music_Play(3, g_num);

          break;

        case 14:

          music_Play(4, g_num);

          break;

        case 15:

          music_Play(5, g_num);

          break;  

        case 0:

        default:

          g_modeMusic = 0;

          break;

      }

      g_num++;

      if(g_musicSelect != 0 && g_num >= music_max[g_musicSelect % 10 - 1])

      {

        g_num = 0;

        g_modeMusic = 2; //stop music

      }

  }

  // if (g_modeSelect == 0 && g_modeMusic == 0 && g_motor == false)   //上报超声波距离

  // {

  //   time--;

  //   if (time == 0)

  //   {

  //     count--;

  //     time = 40000;

  //     if (count == 0)

  //     {

  //       serial_data_postback();

  //       time = 40000;

  //       count = 10;

  //     }

  //   }

  // }

  HServo_State();  

}

实验现象

程序下载完成之后，使用APK控制小车。手机连接上WIFI摄像头模块的WIFI热点：Yahboom_WIFI，无需密码，可以直接连接。具体步骤如下：程序下载完成之后，将WIFI 摄像头模块的接线插入扩展板上相应位置。打开小车的电源开关，可以看到WIFI摄像头模块上面红色的指示灯处于闪烁状态。下载APK：安卓用户请用浏览器扫描二维码，下载并安装APK；苹果用户请用相机扫码二维码或者进入APP Store搜索“YahboomRobot”，下载并安装该APK。

模式选择：当模式切换成功之后，我们可以听到蜂鸣器鸣笛。如果你点击了一下选项，没有听到蜂鸣器鸣笛的话，就证明没有进入模式，请APK界面的按钮，或者再次长按APK 界面的按钮再松手，即可成功进入相应的模式。

2. 结论

相较于其他系统,Arduino表现出编程简易、成本低、平台跨越等一系列优势,将其应用于智能小车设计具有十分重要的现实意义.文章通过阐述整体系统设计方案,对Arduino单片机智能小车硬件设计、软件设计及示例功能的实现展开探讨,旨在为促进智能小车设计的健康稳定发展提供必要借鉴。

小车以Arduino为控制核心，用单片机产生PWM波，控制小车速度。利用灰度传感器对路面黑色轨迹进行检测，并将路面检测信号反馈给单片机。单片机对采集到的信号予以分析判断，及时控制驱动电机以调整小车转向，从而使小车能够沿着黑色轨迹自动行驶，实现小车自动循迹的目的。

以Arduino单片机为控制核心，基于超声波测距的原理,利用超声波传感器，检测小车前方障碍物的距离，然后把数据传送给单片机。当超声波检测到距离小车前方15CM有障碍物时单片机就发出指令让小车左转一定角度，然后停止行进继续探测。如果前方15CM没有障碍物则直行，否则继续左转一定角度。如此通过超声波不断的循环检测周边环境的情况进行自动避障。

总体来说，基于Arduino的Roboduino智能小车还是有市场前景的，这是人工智能必要的产物，紧跟时代潮流。

更多详情请见：【S054】智能循迹避障小车