typedef struct{
float limit; //输出限幅
float target; //设置量
float feedback; //实测量
float Kp; //比例系数
float Ki; //积分系数
float Kd; //微分系数
float eSum; //误差积分
float e0; //当前误差
float e1; //上一次误差
}PIDType;
#define max(a, b) (a>b? a:b)
#define min(a, b) (a#define range(x, a, b) (min(max(x, a), b))
float pid_pos_update(PIDType *p)
{
float pe, ie, de;
float out=0;
//计算当前误差
p->e0 = p->target - p->feedback;
//误差积分
p->eSum += p->e0;
//误差微分
de = p->e0 - p->e1;
pe = p->e0;
ie = p->eSum;
p->e1 = p->e0;
//数据增量
out = pe*(p->Kp) + ie*(p->Ki) + de*(p->Kd);
//输出限幅
out = range(out, -p->limit, p->limit);
return out;
}