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<b?a:b)?
#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;?
}