接前面博客的PID巡线部分代码：

采用两个灰度传感器循黑线测试。

PID公式：

编程时具体用到的公式：

#define left_pin    A0//左侧模拟灰度传感器
#define right_pin   A1//右侧模拟灰度传感器
#define LFwheel_1   50//左前轮引脚1
#define LFwheel_2   52//左前轮引脚2
#define LFpwm       11//左前轮PWM调速
#define RFwheel_1   46//右前轮引脚1
#define RFwheel_2   48//右前轮引脚2
#define RFpwm       10//右前轮PWM调速
#define LBwheel_1   47//左后轮引脚1
#define LBwheel_2   49//左后轮引脚2
#define LBpwm       13//左后轮PWM调速
#define RBwheel_1   51//右后轮引脚1
#define RBwheel_2   53//右后轮引脚2
#define RBpwm       12//右后轮PWM调速

//P、I、D 参数需要根据具体情况进行设置
float kp=0.5;
float ki=0.5;
float kd=0.5;
int left_val=0;
int right_val=0;
int lastError=0;
int allError=0;
int v_st=200;
int v_turn=0;
void setup() {
  Serial.begin(115200);
}

void loop() {
//获取两个模拟灰度传感器的数值
  left_val=analogRead(left_pin);
  right_val=analogRead(right_pin);
  //调用PID调节函数
  PID(left_val,right_val);
  //调用PD调节函数
  //PD(left_val,right_val);
  //调用P调节函数
  //P(left_val,right_val);
  //调用运动函数
  sport(v_turn);
  delay(100);
}
//PID调节函数
void PID(int left_val,int right_val){
  int Error=left_val-right_val;
  allError+=Error;
  float p=kp*Error;
  float i=ki*allError;
  float d=kd*(Error-lastError);
  lastError=Error;
  v_turn=int(p+i+d);
}
//P调节函数
void P(int left_val,int right_val){
  int Error=left_val-right_val;
  float p=kp*Error;
  v_turn=int(p);
}
//PD调节函数
void PD(int left_val,int right_val){
  int Error=left_val-right_val;
  float p=kp*Error;
  float d=kd*(Error-lastError);
  lastError=Error;
  v_turn=int(p+d);
}
//运动函数
void sport(int v_turn){
  int speed_left=v_st+v_turn;
  int speed_right=v_st-v_turn;

  //左前轮
  analogWrite(LFpwm,speed_left);
  digitalWrite(LFwheel_1,HIGH);
  digitalWrite(LFwheel_2,LOW);
  
  //右前轮
  analogWrite(RFpwm,speed_right);
  digitalWrite(RFwheel_1,HIGH);
  digitalWrite(RFwheel_2,LOW);

  //左后轮
  analogWrite(LBpwm,speed_left);
  digitalWrite(LBwheel_1,LOW);
  digitalWrite(LBwheel_2,HIGH);
  
  //右后轮
  analogWrite(RBpwm,speed_right);
  digitalWrite(RBwheel_1,LOW);
  digitalWrite(RBwheel_2,HIGH);
}