基于PID算法的循迹小车

这一期为创客们带来基于PID算法的循迹小车制作

1.标准赛道示意图：
（该赛道包含了：左直角、右直角、十字路口等路况）

2.灰度传感器安装示意图：
（可根据实际情况调节各传感器之间的间距）

3.硬件原理图：

4.控制逻辑：

5.程序如下：

#define leftA_PIN 7
#define leftB_PIN 6
#define left_Pwm_PIN 5

#define STBY 8

#define rightA_PIN 9
#define rightB_PIN 10
#define right_Pwm_PIN 11

#define leftA_track_PIN 2
#define leftB_track_PIN 3
#define middle_track_PIN 4
#define rightA_track_PIN 12
#define rightB_track_PIN 13
 
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();             //停车

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 (rightA_track_PIN, INPUT); //设置引脚为输入引脚
  pinMode (rightB_track_PIN, INPUT); //设置引脚为输入引脚
}
 
/*电机引脚初始化*/
void motor_pinint()
{
  pinMode (leftA_PIN, OUTPUT); //设置引脚为输出引脚
  pinMode (leftB_PIN, OUTPUT); //设置引脚为输出引脚
  pinMode (left_Pwm_PIN, OUTPUT); //设置引脚为输出引脚
  
  pinMode (STBY, OUTPUT); //设置引脚为输出引脚
  
  pinMode (rightA_PIN, OUTPUT); //设置引脚为输出引脚
  pinMode (rightB_PIN, OUTPUT); //设置引脚为输出引脚
  pinMode (right_Pwm_PIN, OUTPUT); //设置引脚为输出引脚
}
 
//停车函数
void _stop()
{
    digitalWrite(leftA_PIN, LOW);
    digitalWrite(leftB_PIN, LOW);
    analogWrite(left_Pwm_PIN,255);   //左轮静止不动
      
    digitalWrite(rightA_PIN, LOW);
    digitalWrite(rightB_PIN, LOW);
    analogWrite(right_Pwm_PIN,255);   //右轮静止不动
}

//速度设定范围(-255,255)
void motorsWrite(int speedL, int speedR)
{
  digitalWrite(STBY, HIGH);
  
  if (speedR > 0) 
  {
    digitalWrite(rightA_PIN, HIGH);
    digitalWrite(rightB_PIN, LOW);
    analogWrite(right_Pwm_PIN,speedR);
    
  } 
  else 
  {
    digitalWrite(rightA_PIN, LOW);
    digitalWrite(rightB_PIN, HIGH);
    analogWrite(right_Pwm_PIN,-speedR);
  }
 
  if (speedL > 0) 
  {
    digitalWrite(leftA_PIN, HIGH);
    digitalWrite(leftB_PIN, LOW);
    analogWrite(left_Pwm_PIN,speedL);
  }
  else 
  {
    digitalWrite(leftA_PIN, LOW);
    digitalWrite(leftB_PIN, HIGH);
    analogWrite(left_Pwm_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() 
{
    digitalWrite(leftA_PIN, LOW);
    digitalWrite(leftB_PIN, LOW);
    analogWrite(left_Pwm_PIN,255);   //左轮静止不动
      
    digitalWrite(rightA_PIN, LOW);
    digitalWrite(rightB_PIN, LOW);
    analogWrite(right_Pwm_PIN,255);   //右轮静止不动
}
 
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(rightA_track_PIN);
  sensor[4] = digitalRead(rightB_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();
} 

传送门：https://blog.csdn.net/qq_38351824/article/details/81275262
详情请关注亿航创客官方抖音账号：EhangGroup