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

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) && (sen