#include "pid.h"
float get_pid(pidDef *pid,float target,float actual)
{
pid->err = target - actual;
pid->integ += pid->err;
if(pid->integ > pid->integ_limit)
{
pid->integ = pid->integ_limit;
}
else if(pid->integ < -pid->integ_limit)
{
pid->integ = -pid->integ_limit;
}
pid->diff = pid->err - pid->last_err;
pid->last_err = pid->err;
pid->out = pid->kp*pid->err + pid->ki * pid->integ + pid->kd*pid->diff;
if(pid->out >= pid->out_limit)
pid->out = pid->out_limit;
if(pid->out <= -pid->out_limit)
pid->out = -pid->out_limit;
//输出斜率限制
if(pid->ramp>0)
{
float ramp = pid->out - pid->out_last;
if(ramp > pid->ramp)
{
pid->out = pid->out_last + pid->ramp;
}
else if(ramp < -pid->ramp)
{
pid->out = pid->out_last - pid->ramp;
}
}
pid->out_last = pid->out;
return pid->out;
}
