注释:
erro:误差,当前值与目标值的差值
integral:积分,误差值的积分,用于补充误差
derivative:微分,误差值的微分,用于修正震荡
output: 输出,三个比例值乘以三个参数,修改的是三个比例值:KP KI KD
对于循迹小车:
erro:偏差值,使用灰度传感器,八个灰度传感器,每个传感器输出0 1数字量,八个传感器间的距离为固定值,我们对于偏差值的定义如下:
4 5全亮偏差值为0
4 为-1 , 4 3为-2 , 3为-3 , 2 3为-4 , 2为-5 ......
5为1 , 5 6为2 ......
integral:宏观上看,是小车对于黑线的预判程度
derivative:宏观上看,是小车的摆动程度
output:是小车pwm的调整值,改变轮子的转速
还有一种办法为加权算偏差值,笔者还未采用,贴一张图以后使用
具体PID代码:
float previous_error=0;
float integral=0;
PID DisPdate= //位置PID结构体
{
5, //赋值比例值 kp
30, //赋值积分值 ki
3 //赋值微分值 kd
};
int PositionPID(float previous_error,float integral,PID pid)
{
float KP=pid.kp,KI=pid.ki,KD=pid.kd;
float error,derivative;
volatile float pwm;
if(gary_sensor_read(L4_IO,L4_PIN)==0&&gary_sensor_read(L5_IO,L5_PIN)==0) //gary_sensor_read检测灰度传感器的数值,1为亮 0为灭,0为检测到黑线
error=0;
else if(gary_sensor_read(L4_IO,L4_PIN)==0)
error=-1;
else if(gary_sensor_read(L4_IO,L4_PIN)==0&&gary_sensor_read(L3_IO,L3_PIN)==0)
error=-2;
else if(gary_sensor_read(L3_IO,L3_PIN)==0)
error=-3;
else if(gary_sensor_read(L3_IO,L3_PIN)==0&&gary_sensor_read(L2_IO,L2_PIN)==0)
error=-4;
else if(gary_sensor_read(L2_IO,L2_PIN)==0)
error=-5;
else if(gary_sensor_read(L1_IO,L1_PIN)==0&&gary_sensor_read(L2_IO,L2_PIN)==0)
error=-6;
else if(gary_sensor_read(L1_IO,L1_PIN)==0)
error=-7;
else if(gary_sensor_read(L5_IO,L5_PIN)==0)
error=1;
else if(gary_sensor_read(L5_IO,L5_PIN)==0&&gary_sensor_read(L6_IO,L6_PIN)==0)
error=2;
else if(gary_sensor_read(L6_IO,L6_PIN)==0)
error=3;
else if(gary_sensor_read(L6_IO,L6_PIN)==0&&gary_sensor_read(L7_IO,L7_PIN)==0)
error=4;
else if(gary_sensor_read(L7_IO,L7_PIN)==0)
error=5;
else if(gary_sensor_read(L7_IO,L7_PIN)==0&&gary_sensor_read(L8_IO,L8_PIN)==0)
error=6;
else if(gary_sensor_read(L8_IO,L8_PIN)==0)
error=7;
integral += error;
derivative = (error - previous_error);
pwm = KP*error + KI*integral + KD*derivative;
previous_error = error;
return pwm; //这里返回的是整数,若想要更精确可换为小数
}
main函数中:
...初始化省略
while(1)
{
adjustment=PositionPID(previous_error,integral,DisPdate); //PID输出调整值
res1 = faultpwm+adjustment; //调整值加上默认值
res2 = faultpwm-adjustment;
if(res1>600)
res1 = 600;
else if(res1<0)
res1 = 0;
if(res2>600)
res2 = 600;
else if(res2 <0)
res2 = 0; //限幅,若无此步,小车会失速
if(adjustment>0)
{
motorL_set(1,res1);
motorR_set(1,res2); //调整左右电机速度
}
else if(adjustment<0)
{
motorR_set(1,res2);
motorL_set(1,res1);
}
else if(gary_sensor_read(L1_IO,L1_PIN)==1&&gary_sensor_read(L2_IO,L2_PIN)==1&&gary_sensor_read(L3_IO,L3_PIN)==1&&gary_sensor_read(L4_IO,L4_PIN)==1&&gary_sensor_read(L5_IO,L5_PIN)==1&&gary_sensor_read(L6_IO,L6_PIN)==1&&gary_sensor_read(L7_IO,L7_PIN)==1&&gary_sensor_read(L8_IO,L8_PIN)==1)
car_stop(); //若灯全亮则无黑场,此时小车停止
}