大家帮我看看这个模糊PID有没有逻辑错误

慎独 · 发表于 2014-1-20 16:09:09

float pid()
{
lasterror = error;
lastderror = derror;
error = value_set - value_ADC;
derror = error - lasterror;
if (error>EMAX)
{
error = EMAX;
}
else if (error<EMIN)
{
error = EMIN;
}
if (derror>ECMAX)
{
derror = ECMAX;
}
else if (derror<ECMIN)
{
derror = ECMIN;
}
if (error < error_gate[0])
error_fuzzy_value = -3;
else if (error < error_gate[1])
error_fuzzy_value = -2;
else if (error < error_gate[2])
error_fuzzy_value = -1;
else if (error < error_gate[3])
error_fuzzy_value = 0;
else if (error < error_gate[4])
error_fuzzy_value = 1;
else if (error < error_gate[5])
error_fuzzy_value = 2;
else
error_fuzzy_value = 3;
if (derror < derror_gate[0])
derror_fuzzy_value = -3;
else if (derror < derror_gate[1])
derror_fuzzy_value = -2;
else if (derror < derror_gate[2])
derror_fuzzy_value = -1;
else if (derror < derror_gate[3])
derror_fuzzy_value = 0;
else if (derror < derror_gate[4])
derror_fuzzy_value = 1;
else if (derror < derror_gate[5])
derror_fuzzy_value = 2;
else
derror_fuzzy_value = 3;
dKp = rule_dkd[error_fuzzy_value][derror_fuzzy_value];
dKi = rule_dki[error_fuzzy_value][derror_fuzzy_value];
dKd = rule_dkd[error_fuzzy_value][derror_fuzzy_value];
Kp += dKp;
Ki += dKi;
Kd += dKd;
dPk = Kp*derror + Ki*error + Kd*(derror - lastderror);
Pk += dPk;
if (Pk > OUTMAX)
Pk = OUTMAX;
return Pk;
}