接前面博客的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);}