znzcqy的博客

上次我们打造了一辆基于Arduino的循迹小车，并且加入了PID算法。
https://znzcqy.github.io/post/Nzx8AZmvk/

但是小车的表现却不尽如人意，于是我对代码进行了改进，并且不再使用L298N电机的ENA,ENB使能端，对线路进行了简化，直接采用Arduino的带PMW的引脚来对小车进行速度控制。电池方面，换成可充电的锂电池（3.7V X3），以保证小车的速度。在上次的赛道中，赛道总长度大约为5.14m，要实现在7s内跑完全程的目标，就必须要更高的速度。

推荐使用正版arduino开发板

下面是改进后的代码

#define righA_PIN 5
#define righB_PIN 3
#define leftA_PIN 10
#define leftB_PIN 9
#define leftA_track_PIN 2
#define leftB_track_PIN 4
#define middle_track_PIN 6
#define righA_track_PIN 7
#define righB_track_PIN 8
float Kp = 15 , Ki = 0, Kd = 200;                   //pid弯道参数参数
float error = 0, P = 0, I = 0, D = 0, PID_value = 0;//pid直道参数
float previous_error = 0, previous_I = 0;           //误差值
int sensor[5] = {0, 0, 0, 0, 0};                    //
//5个传感器数值的数组
static int initial_motor_speed = 200;                //初始速度
void read_sensor_values(void);                      //读取初值
void calc_pid(void);                                //计算pid
void motor_control(void);                           //电机控制
void motor_pinint( );     //引脚初始化
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); //设置引脚为输出引脚
}
//速度设定范围(-255,255)
void motorsWrite(int speedL, int speedR)
{
  if (speedL > 0) {
    analogWrite(leftA_PIN, speedL);
    analogWrite(leftB_PIN, 0);
  } else {
    analogWrite(leftA_PIN, 0);
    analogWrite(leftB_PIN, -speedL);
  }
  if (speedR > 0) {
    analogWrite(righA_PIN, speedR);
    analogWrite(righB_PIN, 0);
  } else {
    analogWrite(righA_PIN, 0);
    analogWrite(righB_PIN, -speedR);
  }
}

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] != 0) {
    error = -4;//01111
  } else if (sensor[0] == 0 && sensor[1] == 0 && sensor[2] != 0 && sensor[3] != 0 && sensor[4] != 0) {
    error = -3;//00111
  } else if (sensor[1] == 0 && sensor[0] != 0 && sensor[2] != 0 && sensor[3] != 0 && sensor[4] != 0) {
    error = -2;//10111
  } else if (sensor[1] == 0 && sensor[0] != 0 && sensor[2] == 0 && sensor[3] != 0 && sensor[4] != 0) {
    error = -1;//10011
  } else if (sensor[2] == 0 && sensor[1] != 0 && sensor[0] != 0 && sensor[3] != 0 && sensor[4] != 0) {
    error = 0;//11011
  } else if (sensor[3] == 0 && sensor[1] != 0 && sensor[2] == 0 && sensor[0] != 0 && sensor[4] != 0) {
    error = 1;//11001
  } else if (sensor[3] == 0 && sensor[1] != 0 && sensor[2] != 0 && sensor[0] != 0 && sensor[4] != 0) {
    error = 2;//11101
  } else if (sensor[4] == 0 && sensor[1] != 0 && sensor[2] != 0 && sensor[3] == 0 && sensor[0] != 0) {
    error = 3;//11100
  } else if (sensor[4] == 0 && sensor[1] != 0 && sensor[2] != 0 && sensor[3] != 0 && sensor[0] != 0) {
    error = 4;//11110
  } else if (sensor[0] != 0 && sensor[1] != 0 && sensor[2] != 0 && sensor[3] != 0 && sensor[4] != 0) {
    error = 0;//11111
  }
}
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 ;
  int right_motor_speed ;
  left_motor_speed = initial_motor_speed + PID_value;
  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;
  }
  if (right_motor_speed < -255) {
    right_motor_speed = -255;
  }
  if (right_motor_speed > 255) {
    right_motor_speed = 255;
  }
  motorsWrite(left_motor_speed, right_motor_speed);
}

最后再说明一下，PID参数需要自己调节，通常先把速度调到50，Ki,Kd先赋值为0，调节Kp，直到能循迹了，再把速度调大，调节Kd即可，Ki是积分常数，一般为0，如果小车不稳定，可以适当赋值。实测小车速度赋值为200时，跑完一圈大概5s。这时我们可以适当把速度再调高，挑战速度的极限，通常需要更换摩擦力更大的赛道地面。