基于arduino的循迹小车（含有PID算法）

最新推荐文章于 2024-09-07 08:35:58 发布

Sumjess

最新推荐文章于 2024-09-07 08:35:58 发布

阅读量8.5w

收藏 1.5k

点赞数 312

分类专栏：智能小车文章标签：循迹小车循迹PID PID

本文链接：https://blog.csdn.net/qq_38351824/article/details/81275262

版权

智能小车专栏收录该内容

10 篇文章 33 订阅

订阅专栏

循迹小车一般分为两方面：一方面是简单的闭环赛道只有直道和弯道，另一方面是毕设类型的包括一些元素：90度弯道、十字道路、S形弯道等。

1、CSDN下载：

含有PID：https://download.csdn.net/download/qq_38351824/11107048

没有PID：https://download.csdn.net/download/qq_38351824/11107175

2、可以关注点赞并在下方评论，我给你邮箱发过去。

3、关注微信公众号下载：

① 关注微信公众号：Tech云

②

本篇博客试根据下图来进行书写的，如果大家有什么新的元素，也可以在下方评论，我进行更新。

作者：sumjess

注意本篇博客循迹模块使用了5个

一、简单的闭环赛道

随意画了一个

（1）逻辑部分：

所以程序的写法也很简单，就是检测到哪种情况对应着哪种反应。这一过程可以用switch也可以用if来实现这一过程。下文用if来演示。

（2）各程序片段

总的循环：

void loop()
{
     read_sensor_values();  //获取5个循迹模块的数值情况
     calc_pid();            //pid计算出转向的pwm值
     motor_control();       //电机转动
}

第一部分：检测部分程序片段

void read_sensor_values()
{
  sensor[0] = digitalRead(leftA_track_PIN);
  sensor[1] = digitalRead(leftB_track_PIN);
  sensor[2] = digitalRead(middle_track_PIN);
  sensor[3] = digitalRead(righA_track_PIN);
  sensor[4] = digitalRead(righB_track_PIN);
  
  if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 1)) {
      error = 2;//          0 0 0 0 1
    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 1) && (sensor[4] == 0)) {
      error = 1;//          0 0 0 1 0
    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0)) {
      error = 0;//          0 0 1 0 0
    } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {
      error = -1;//         0 1 0 0 0
    } else if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {
      error = -2;//         1 0 0 0 0
    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {
      if (error == -2) {//  0 0 0 0 0
        error = -3;
      }else{
        error = 3;
      }
    }
  }

解释：

<1> 将读取到的五个循迹模块的数据存入数组sensor：

sensor[0] = digitalRead(leftA_track_PIN); 左边第一个循迹模块
sensor[1] = digitalRead(leftB_track_PIN); 左边第二个循迹模块
sensor[2] = digitalRead(middle_track_PIN); 中间的循迹模块
sensor[3] = digitalRead(righA_track_PIN); 右边第二个循迹模块
sensor[4] = digitalRead(righB_track_PIN); 右边第一个循迹模块

<2> 检测到哪种情况对应着哪种反应：

if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 1)) {
error = 2;// 0 0 0 0 1
} else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 1) && (sensor[4] == 0)) {
error = 1;// 0 0 0 1 0
} else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0)) {
error = 0;// 0 0 1 0 0
} else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {
error = -1;// 0 1 0 0 0
} else if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {
error = -2;// 1 0 0 0 0
} else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {
if (error == -2) {// 0 0 0 0 0
error = -3;
}else{
error = 3;
}
}

前面几个就不解释了完全按照前面excel写的，最后一个是所有传感器均没有检测到黑线的情况，下面具体解释一下：

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

检测到为0000的情况
if (error == -2) {// //如果上一次 error == -2

意味着上一次是1000，也就是意味着这次可能车在左面第一个传感器和第二个传感器之间或者是在左边第一个传感器的左边，无论是上面的哪种情况，车都需要大左转
error = -3;
}else{

否则就是相反的情况，都需要大右转
error = 3;
}
}

第二部分：计算PID（计算转向大小）

void calc_pid()
{
  P = error;
  I = I + error;
  D = error - previous_error;
 
  PID_value = (Kp * P) + (Ki * I) + (Kd * D);
 
  previous_error = error;
}

利用上一部分得到的error计算车的偏离情况，车偏离赛道的情况从而来调整下一次给出的PWM进而快速转正车身。

这一部分的kp、ki、kd需要不断地调试，从而得出最佳解。

第三部分：电机转向

//速度设定范围(-255,255)
void motorsWrite(int speedL, int speedR)
{
  if (speedR > 0) {
    analogWrite(leftA_PIN, speedR);
    analogWrite(leftB_PIN, 0);
  } else {
    analogWrite(leftA_PIN, 0);
    analogWrite(leftB_PIN, -speedR);
  }
 
  if (speedL > 0) {
    analogWrite(righA_PIN, speedL);
    analogWrite(righB_PIN, 0);
  } else {
    analogWrite(righA_PIN, 0);
    analogWrite(righB_PIN, -speedL);
  }
}
//速度设定范围(-100，100)
void motorsWritePct(int speedLpct, int speedRpct) {
  //speedLpct, speedRpct ranges from -100 to 100
  motorsWrite(speedLpct * 2.55, speedRpct * 2.55);
}
void motor_control()
{
  int left_motor_speed = initial_motor_speed - PID_value;
  int right_motor_speed = initial_motor_speed + PID_value;
  
  if(left_motor_speed < -255){
    left_motor_speed = -255;
  }
  if(left_motor_speed > 255){
    left_motor_speed = 255;
  }
  motorsWrite(left_motor_speed,right_motor_speed);
}

第一个函数是在检查更改正负值，来保证PWM都是正的，即轮子不会倒转。

第二个函数是在利用一次函数，将输入范围变成0-100

第三个函数是在控制范围，ardunio的PWM范围是在正负255，此函数是在做一个限制·

二、在上面的基础上添加元素

（1）逻辑部分：

（2）各程序片段

除了read_sensor_values() 各程序片段与之前都一样，read_sensor_values() 只是添加了一部分，情况如下：

void read_sensor_values()
{
  sensor[0] = digitalRead(leftA_track_PIN);
  sensor[1] = digitalRead(leftB_track_PIN);
  sensor[2] = digitalRead(middle_track_PIN);
  sensor[3] = digitalRead(righA_track_PIN);
  sensor[4] = digitalRead(righB_track_PIN);
  
      if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 1)) {
      decide = 1;//十字路口 1 1 1 1 1   直行
    } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 0)) {
      decide = 1;//十字路口 0 1 1 1 0   直行
    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 1)) {
      decide = 2;//90度路口 0 0 1 1 1    右转90度
    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 0)) {
      decide = 2;//90度路口 0 0 1 1 0    右转90度 
    } else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0)) {
      decide = 3;//90度路口 1 1 1 0 0    左转90度 
    } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0)) {
      decide = 3;//90度路口 0 1 1 0 0    左转90度 
    } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0)) {
      decide = 3;//向上锐角 0 1 1 0 0    向上锐角 
    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 1)) {
      error = 2;//          0 0 0 0 1
    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 1) && (sensor[4] == 0)) {
      error = 1;//          0 0 0 1 0
    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0)) {
      error = 0;//          0 0 1 0 0
    } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {
      error = -1;//         0 1 0 0 0
    } else if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {
      error = -2;//         1 0 0 0 0
    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {
      if (error == -2) {//  0 0 0 0 0
        error = -3;
      }else{
        error = 3;
      }
    }
 
}

各个条件都是按照前面excel写的

附全套程序

//电机使用：5-6-9-10
//循迹模块使用：2-3-7-8-11

//-------------------------------------------------------------------//
//*******************************************************************//
///
//小车部分/
///

//           电机设置             //
#define leftA_PIN 5
#define leftB_PIN 6
#define righA_PIN 9
#define righB_PIN 10

float Kp = 10, Ki = 0.5, Kd = 0;                    //pid弯道参数参数 
float error = 0, P = 0, I = 0, D = 0, PID_value = 0;//pid直道参数 
float decide = 0;                                   //元素判断
float previous_error = 0, previous_I = 0;           //误差值 
int sensor[5] = {0, 0, 0, 0, 0};                    //
5个传感器数值的数组 
static int initial_motor_speed = 60;                //初始速度 
 
void read_sensor_values(void);                      //读取初值 
void calc_pid(void);                                //计算pid 
void motor_control(void);                           //电机控制 

void motor_pinint( );     //引脚初始化
void _stop();             //停车
///
//*******************************************************************//
//-------------------------------------------------------------------//

//-------------------------------------------------------------------//
//*******************************************************************//
///
循迹部分///
///

//             循迹模块设置               //
#define leftA_track_PIN 3
#define leftB_track_PIN 4
#define middle_track_PIN 12
#define righA_track_PIN 7
#define righB_track_PIN 13


//----------------------------------算法部分-----------------------------
---//
void obstacle( );      //避障小车算法
                       //循迹小车算法/*  read_sensor_values(); calc_pid(
); motor_control();  */     
//--------------------------------------------------------------------------//

void setup()
{
  Serial.begin(9600); //串口波特率115200（PC端使用）
  track_pinint( );     //循迹引脚初始化
  motor_pinint();        //电机引脚初始化

}
void loop()
{
    read_sensor_values();         //循迹小车
    calc_pid();
    motor_control();
}

/*循迹模块引脚初始化*/
void track_pinint( )
{ 
  pinMode (leftA_track_PIN, INPUT); //设置引脚为输入引脚
  pinMode (leftB_track_PIN, INPUT); //设置引脚为输入引脚
  pinMode (middle_track_PIN, INPUT);//设置引脚为输入引脚
  pinMode (righA_track_PIN, INPUT); //设置引脚为输入引脚
  pinMode (righB_track_PIN, INPUT); //设置引脚为输入引脚
}

/*电机引脚初始化*/
void motor_pinint( )
{
  pinMode (leftA_PIN, OUTPUT); //设置引脚为输出引脚
  pinMode (leftB_PIN, OUTPUT); //设置引脚为输出引脚
  pinMode (righA_PIN, OUTPUT); //设置引脚为输出引脚
  pinMode (righB_PIN, OUTPUT); //设置引脚为输出引脚
  }

/**************************************************
stop子函数—停止子函数
函数功能：控制车停止
**************************************************/
void _stop()
{
  analogWrite(leftA_PIN,0);      
  analogWrite(leftB_PIN,0);         //左轮静止不动
  analogWrite(righA_PIN,0);      
  analogWrite(righB_PIN,0);         //右轮静止不动
}
//速度设定范围(-255,255)
void motorsWrite(int speedL, int speedR)
{
  if (speedR > 0) {
    analogWrite(leftA_PIN, speedR);
    analogWrite(leftB_PIN, 0);
  } else {
    analogWrite(leftA_PIN, 0);
    analogWrite(leftB_PIN, -speedR);
  }
 
  if (speedL > 0) {
    analogWrite(righA_PIN, speedL);
    analogWrite(righB_PIN, 0);
  } else {
    analogWrite(righA_PIN, 0);
    analogWrite(righB_PIN, -speedL);
  }
}
//速度设定范围(-100，100)
void motorsWritePct(int speedLpct, int speedRpct) {
  //speedLpct, speedRpct ranges from -100 to 100
  motorsWrite(speedLpct * 2.55, speedRpct * 2.55);
}
void motorsStop() 
{
  analogWrite(leftA_PIN,0);      
  analogWrite(leftB_PIN,0);         //左轮静止不动
  analogWrite(righA_PIN,0);      
  analogWrite(righB_PIN,0);         //右轮静止不动
}
 
void read_sensor_values()
{
  sensor[0] = digitalRead(leftA_track_PIN);
  sensor[1] = digitalRead(leftB_track_PIN);
  sensor[2] = digitalRead(middle_track_PIN);
  sensor[3] = digitalRead(righA_track_PIN);
  sensor[4] = digitalRead(righB_track_PIN);
  
    if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3
] == 1) && (sensor[4] == 1)) {
      decide = 1;//十字路口 1 1 1 1 1   直行
    } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (
sensor[3] == 1) && (sensor[4] == 0)) {
      decide = 1;//十字路口 0 1 1 1 0   直行
    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (
sensor[3] == 1) && (sensor[4] == 1)) {
      decide = 2;//90度路口 0 0 1 1 1    右转90度
    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (
sensor[3] == 1) && (sensor[4] == 0)) {
      decide = 2;//90度路口 0 0 1 1 0    右转90度 
    } else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (
sensor[3] == 0) && (sensor[4] == 0)) {
      decide = 3;//90度路口 1 1 1 0 0    左转90度 
    } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (
sensor[3] == 0) && (sensor[4] == 0)) {
      decide = 3;//90度路口 0 1 1 0 0    左转90度 
    } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (
sensor[3] == 0) && (sensor[4] == 0)) {
      decide = 3;//向上锐角 0 1 1 0 0    向上锐角 
    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (
sensor[3] == 0) && (sensor[4] == 1)) {
      error = 2;//          0 0 0 0 1
    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (
sensor[3] == 1) && (sensor[4] == 0)) {
      error = 1;//          0 0 0 1 0
    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (
sensor[3] == 0) && (sensor[4] == 0)) {
      error = 0;//          0 0 1 0 0
    } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 0) && (
sensor[3] == 0) && (sensor[4] == 0)) {
      error = -1;//         0 1 0 0 0
    } else if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0) && (
sensor[3] == 0) && (sensor[4] == 0)) {
      error = -2;//         1 0 0 0 0
    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (
sensor[3] == 0) && (sensor[4] == 0)) {
      if (error == -2) {//  0 0 0 0 0
        error = -3;
      }else{
        error = 3;
      }
    }
}
void calc_pid()
{
  P = error;
  I = I + error;
  D = error - previous_error;
 
  PID_value = (Kp * P) + (Ki * I) + (Kd * D);
 
  previous_error = error;
}
void motor_control()
{
  int left_motor_speed = initial_motor_speed - PID_value;
  int right_motor_speed = initial_motor_speed + PID_value;
  
  if(left_motor_speed < -255){
    left_motor_speed = -255;
  }
  if(left_motor_speed > 255){
    left_motor_speed = 255;
  }
  motorsWrite(left_motor_speed,right_motor_speed);
 
  Serial.print("move_A: ");
  Serial.print(left_motor_speed, OCT);
  Serial.print(" move_B: ");
  Serial.print(right_motor_speed, OCT);
  Serial.print(" error: ");
  Serial.print(error, OCT);
  Serial.print(" P: ");
  Serial.print(Kp, OCT);
  Serial.print(" PID_value: ");
  Serial.print(PID_value, OCT);
  Serial.println();
}

附加：不含PID的代码

#define leftA_PIN 5
#define leftB_PIN 6
#define righA_PIN 9
#define righB_PIN 10

#define leftA_track_PIN 3
#define leftB_track_PIN 4
#define middle_track_PIN 12
#define righA_track_PIN 7
#define righB_track_PIN 13

void motor_pinint( );                               //引脚初始化
void _stop();                                       //停车
void forward();  
void turnSRight();                                 //小右转
void turnSLeft();                                  //小左转
void turnRight();                                  //右转
void turnLeft();                                   //左转
void nine(); 
void ten() ;
int error = 0;
int sensor[5] = {0, 0, 0, 0, 0};                    //5个传感器数值的数组 
int read_sensor_values(void);                      //读取初值 

void setup() {
  Serial.begin(9600); //串口波特率9600（PC端使用）
  track_pinint( );    //循迹引脚初始化
  motor_pinint();     //电机引脚初始化
}

void loop() {
  static int b=0;
  b=read_sensor_values();         //循迹小车
  switch (b){              //读取初值  
     case 0:  forward(); Serial.println(1); break; //直行
     case -1: turnSRight();Serial.println(2); break; //小左
     case -2: turnRight();Serial.println(3);break; //大左 
     case 1:  turnSLeft(); Serial.println(4); break; //小右
     case 2:  turnLeft();Serial.println(5); break; //大右    
     case 3:  _stop();Serial.println(6); break; //停
//     case -3: turnLeft(); Serial.println(7);break; //大左
//     case 4:  forward();  Serial.println(9);break; //直行   
     case 5:   nine() ;  Serial.println(9);break; //右转90度
     case 6:    ten();   Serial.println(9);break; 
     default: _stop();Serial.println(8);break; 
  }

}

/*循迹模块引脚初始化*/
void track_pinint( )
{ 
  pinMode (leftA_track_PIN, INPUT); //设置引脚为输入引脚
  pinMode (leftB_track_PIN, INPUT); //设置引脚为输入引脚
  pinMode (middle_track_PIN, INPUT);//设置引脚为输入引脚
  pinMode (righA_track_PIN, INPUT); //设置引脚为输入引脚
  pinMode (righB_track_PIN, INPUT); //设置引脚为输入引脚
}

/*电机引脚初始化*/
void motor_pinint( )
{
  pinMode (leftA_PIN, OUTPUT); //设置引脚为输出引脚
  pinMode (leftB_PIN, OUTPUT); //设置引脚为输出引脚
  pinMode (righA_PIN, OUTPUT); //设置引脚为输出引脚
  pinMode (righB_PIN, OUTPUT); //设置引脚为输出引脚
  }

/**************************************************
stop子函数—停止子函数
函数功能：控制车停止
**************************************************/
void _stop()
{
   analogWrite(leftA_PIN,0);      
  analogWrite(leftB_PIN,0);         //左轮静止不动
  analogWrite(righA_PIN,0);      
  analogWrite(righB_PIN,0);         //右轮静止不动
}
int read_sensor_values()
{
  sensor[0] = digitalRead(leftA_track_PIN);
  sensor[1] = digitalRead(leftB_track_PIN);
  sensor[2] = digitalRead(middle_track_PIN);
  sensor[3] = digitalRead(righA_track_PIN);
  sensor[4] = digitalRead(righB_track_PIN);
  //1为遇到黑线
//   if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 1)) {
//      error = 2;//          00001
//    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 1) && (sensor[4] == 0)) {
//      error = 1;//          00010
//    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0)) {
//      error = 0;//          00100
//    } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {
//      error = -1;//         01000
//    } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0)) {
//      error = -1;//         01100
//    }  else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 0)) {
//      error = 1;//          00110
//    } else if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {
//      error = -2;//         10000
//    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {
//      if (error == -2) {//  00000
//        error = 4;//-3;
//      }else{
//        error = 4;//3;
//      }
//    }
//    else error = -error;  
 //000-001-010-011-100-101-110-111
           if ((sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0)) {
      error = 0;//          000  停
    } else if ((sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 1)) {
      error = -2;//          001  右转
    } else if ((sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 0)) {
      error = 0;//          010  直行
    } else if ((sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1)) {
      error = -1;//         011  右转
    } else if ((sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 0)) {
      error = 2;//         100  左转
    }  else if ((sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0)) {
      error = 1;//         110  左转
    } else if ((sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 1)) {
      error = 0;//          111  直行
    } /*else if ((sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 1)) {
      error = 1;//          101
    }*/
    if (sensor[4] == 1)  error=5;
    if (sensor[0] == 1)  error=6;
    for(int i=1;i<4;i++)
    Serial.print(sensor[i]);
    Serial.println( );
    return error;
}
/********************** ****************************
forward子函数——前进子函数
函数功能：控制车前进
**************************************************/
 void forward()
{
  analogWrite(leftA_PIN,60);      
  analogWrite(leftB_PIN,0);         //左轮前进60
  analogWrite(righA_PIN,60);      
  analogWrite(righB_PIN,0);         //右轮前进
}
/**************************************************
turnLeft子函数——大左转子函数
函数功能：控制车大左转
**************************************************/
void turnLeft() 
{
  analogWrite(leftA_PIN,40);      
  analogWrite(leftB_PIN,0);         //左轮前进20
  analogWrite(righA_PIN,140);      
  analogWrite(righB_PIN,0);         //右轮前进70
}
/**************************************************
turnRight子函数——小右转子函数
函数功能：控制车大右转弯
**************************************************/
void turnRight()
{
  analogWrite(leftA_PIN,140);      
  analogWrite(leftB_PIN,0);         //左轮前进
  analogWrite(righA_PIN,40);      
  analogWrite(righB_PIN,0);         //右轮前进
}
/**************************************************
turnLeft子函数——大左转子函数
函数功能：控制车小左转
**************************************************/
void turnSLeft()
{
  analogWrite(leftA_PIN,60);      
  analogWrite(leftB_PIN,0);         //左轮前进20
  analogWrite(righA_PIN,110);      
  analogWrite(righB_PIN,0);         //右轮前进30
}
/**************************************************
turnRight子函数——小右转子函数
函数功能：控制车小右转弯
**************************************************/
void turnSRight()
{
  analogWrite(leftA_PIN,110);      
  analogWrite(leftB_PIN,0);         //左轮前进
  analogWrite(righA_PIN,60);      
  analogWrite(righB_PIN,0);         //右轮前进
}
void nine() 
{
  analogWrite(leftA_PIN,140);      
  analogWrite(leftB_PIN,0);         //左轮前进20
  analogWrite(righA_PIN,0);      
  analogWrite(righB_PIN,0);         //右轮前进70
}
void ten() 
{
  analogWrite(leftA_PIN,0);      
  analogWrite(leftB_PIN,0);         //左轮前进20
  analogWrite(righA_PIN,140);      
  analogWrite(righB_PIN,0);         //右轮前进70
}