【arduino】三路pid循迹小车

这是个很久之前的项目，没有保存照片，故使用文字+图片说明

先提前说明，想用pid就需要足够的精度，数字信号很难满足，故选用模拟信号，51系列建议放弃，没有自带模拟接口，只能外接转换器，非常麻烦

pid的好处就是跑的非常稳

以下是正文

首先是传感器部分，使用三个带模拟信号的红外传感器（大部分都是带的，d引脚输出数字，a引脚输出模拟），并排放置，如图差不多就行了，传感器之间可以留有一定间隙，根据个人调整。

每个传感器都是模拟量，也就是越黑值就越大，

 然后我们把最左边的值减去最右边的值，就能得到另一个函数图像，大概这样，画的有点丑

  再融入中间的传感器，把左右偏转，得出这样一个图像，可能传感器本身误差比较大，跟图像有所区别

直接贴代码，这部分是难点，主要是对数据的处理，实际环境变化传感器出现的误差会很难受，可以选择较好的红外传感器或者灰度传感器

int GetTraceDate()
{
  int Data_Out;//定义数据输出变量
  int Left_AD,Right_AD,Mid_AD;//定义左右中传感器AD值变量
  
  Left_AD=analogRead(A0);
  Mid_AD=analogRead(A1);
  Right_AD=analogRead(A2);
//左值减去右值
  Data_Out=(Left_AD-Right_AD+D_AD_VALUE);//D_AD_VALUE表示居中时误差，把车放在最中间显示的值
//底下这段是对图像进行处理，使得更接近理论以及运行过程的bug，自行调整即可
  if(Left_AD > _H && Mid_AD > _H && Right_AD > _H) 
    stop = 1;
  else 
    stop = 0;
  if(Mid_AD>200)
  {
    flag = 0;
  }
  if((Mid_AD <= LEFT_THERSH) && (Data_Out < -20))
  { //left
    Data_Out= (2*LEFT_MAX-Data_Out);
    flag--;
  } 
  else if((Mid_AD <= RIGHT_THERSH) && (Data_Out > -10))
  { //right
    Data_Out= (2*RIGHT_MAX-Data_Out);
    flag++;
  }
  if(Left_AD < _L && Mid_AD < _L && Right_AD < _L)
  {
       Serial.print("---");
       if(flag<0)
       {//left
          Data_Out = LEFT_MAX*2;
       }
       else
       {//right
          Data_Out = RIGHT_MAX*2;
       }
  }
// Data_Out是最重要的返回值，表示车子位于线的方向以及距离，不然无法使用pid公式
  return Data_Out;
}

 车身就没什么好讲的了，使用了一个l298n带电机，好处是可以一次带两个，图片为网图（侵权联系删除），接线方法大概就这样，使用差速转向成本低，快捷，缺点是车身容易摆动

 这里贴上完整代码

#define LEFT_MAX -680
#define RIGHT_MAX 330
#define LEFT_THERSH 35
#define RIGHT_THERSH 37
#define D_AD_VALUE -8
#define _L 45
#define _H 500

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

float Kp = 0.05, Ki = 0, Kd = 0;                    //pid弯道参数参数 
float error = 0, P = 0, I = 0, D = 0, PID_value = 0;//pid直道参数 
float previous_error = 0, previous_I = 0;           //误差值
int initial_motor_speed = 100;                //初始速度
//unsigned int PWM_L, PWM_R, T = 0;
int flag = 0;
int stop = 0;
void motor_pinint( )
{
  pinMode (leftA_PIN, OUTPUT); //设置引脚为输出引脚
  pinMode (leftB_PIN, OUTPUT); //设置引脚为输出引脚
  pinMode (righA_PIN, OUTPUT); //设置引脚为输出引脚
  pinMode (righB_PIN, OUTPUT); //设置引脚为输出引脚
}
//获取循迹传感器输出函数
int GetTraceDate()
{
  int Data_Out;//定义数据输出变量
  int Left_AD,Right_AD,Mid_AD;//定义左右中传感器AD值变量
  
  Left_AD=analogRead(A0);
  Mid_AD=analogRead(A1);
  Right_AD=analogRead(A2);
  Data_Out=(Left_AD-Right_AD+D_AD_VALUE);
  Serial.print(Data_Out);
  Serial.print("   ");
  if(Left_AD > _H && Mid_AD > _H && Right_AD > _H) 
    stop = 1;
  else 
    stop = 0;
  if(Mid_AD>200)
  {
    flag = 0;
  }
  if((Mid_AD <= LEFT_THERSH) && (Data_Out < -20))
  { //left
    Data_Out= (2*LEFT_MAX-Data_Out);
    flag--;
  } 
  else if((Mid_AD <= RIGHT_THERSH) && (Data_Out > -10))
  { //right
    Data_Out= (2*RIGHT_MAX-Data_Out);
    flag++;
  }
  if(Left_AD < _L && Mid_AD < _L && Right_AD < _L)
  {
       Serial.print("---");
       if(flag<0)
       {//left
          Data_Out = LEFT_MAX*2;
       }
       else
       {//right
          Data_Out = RIGHT_MAX*2;
       }
  }
  return Data_Out;
}

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 < 1)
    left_motor_speed = 1;
  if(right_motor_speed < 1)
    left_motor_speed = 1;
  
  analogWrite(leftA_PIN, left_motor_speed);
  analogWrite(leftB_PIN, 0);
  analogWrite(righA_PIN, right_motor_speed);
  analogWrite(righB_PIN, 0);
  
  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(" PID_value: ");
  Serial.print(PID_value);
  Serial.println();
}
void _stop()
{
  analogWrite(leftA_PIN, 0);
  analogWrite(leftB_PIN, 0);
  analogWrite(righA_PIN, 0);
  analogWrite(righB_PIN, 0);
}
void setup() {
  Serial.begin(9600);
  motor_pinint();
}

void loop() {
  error = GetTraceDate();
  if(!stop) 
  {
    calc_pid();
    motor_control();
  }
  else 
  {
    Serial.println("stop");
    _stop();
  }
//  Serial.print(GetTraceDate());
//  Serial.print("   ");
//  Serial.print(analogRead(A0));
//  Serial.print("   ");
//  Serial.print(analogRead(A1));
//  Serial.print("   ");
//  Serial.print(analogRead(A2));
//  Serial.print("   ");
//  Serial.print(flag);
//  Serial.println();
  
}

代码照搬大概率是用不了的，因为需要根据车子进行实际修改，赛道也有影响