float PID(float e,float kp,float ki,float kd)
{
static float e_s=0,sum=0;//e_s用于保存上一次的误差值,用于计算微分项。Sum用于计算累加和,计算积分项。
float r;
sum+=e;//计算积分累加和
r=kp*e+ki*sum+kd*(e-e_s);//从左至右分别是比例项、积分项、微分项
e_s=e;//保存这一次的误差值用于下一次微分计算
return r;
}
参考一下C语言的例程
float PID(float e,float kp,float ki,float kd)
{
static float e_s=0,sum=0;//e_s用于保存上一次的误差值,用于计算微分项。Sum用于计算累加和,计算积分项。
float r;
sum+=e;//计算积分累加和
r=kp*e+ki*sum+kd*(e-e_s);//从左至右分别是比例项、积分项、微分项
e_s=e;//保存这一次的误差值用于下一次微分计算
return r;
}
参考一下C语言的例程
举报