|
发表于 2014-9-9 15:01:35
|
显示全部楼层
typedef struct _PID {
LONG SetPoint; // 设定目标 Desired Value
float p; // 比例常数 Proportional Const
float i; // 积分常数 Integral Const
float d; // 微分常数 Derivative Const
LONG LastError; // Error[-1]
LONG PrevError; // Error[-2]
LONG SumError; // Sums of Errors
LONG LastResult; // 上一次阀值
}PID,*PPID;
#define DEFAULT_PROPORTIONAL_CONST 0.1f
#define DEFAULT_INTEGRAL_CONST 0.01f
#define DEFAULT_DERIVATIVE_CONST 0.0f
/*
获取此次控制阀值
LastValue: 上一秒流量值
*/
LONG PidGetResult(PID *pPid,LONG LastValue)
{
LONG ReadValue;
LONG dError,Error;
LONG rDelta;
ReadValue = LastValue;
if(LastValue < pPid->SetPoint) //比满值小
{
if(pPid->LastResult >= pPid->SetPoint) //上次控制量大于等于满值
ReadValue = pPid->SetPoint; //当做此次读数为满值
}
Error = pPid->SetPoint - ReadValue; // 误差
pPid->SumError += Error; // 积分 累计误差 *FIXME*此项是否需要定期清空
dError = pPid->LastError - pPid->PrevError; // 当前微分 误差偏差
pPid->PrevError = pPid->LastError;
pPid->LastError = Error;
rDelta = (LONG)(pPid->p * Error // 比例项 调节速率
+ pPid->i * pPid->SumError // 积分项 长久的误差影响
+ pPid->d * dError); // 微分项 一般不设此项
//计算下一秒控制量
rDelta += pPid->SetPoint;
rDelta = rDelta > pPid->SetPoint ? pPid->SetPoint : rDelta;
rDelta = rDelta < 0 ? 0 : rDelta;
//保存为上一次控制量
pPid->LastResult = rDelta;
return rDelta;
}
/*
初始化PID数据结构
TargetValue: 期望阀值
*/
VOID PidInitialize(PID *pPid,LONG TargetValue)
{
memset(pPid,0,sizeof(PID));
pPid->SetPoint = TargetValue;
pPid->LastResult = TargetValue;
pPid->p = AdvancedTrafficSet.Proportion;
pPid->i = AdvancedTrafficSet.Integral;
pPid->d = AdvancedTrafficSet.Derivative;
} |
|