29 lines
562 B
C++
29 lines
562 B
C++
#include "pid.h"
|
|
|
|
static inline float Saturation(float val, float min, float max)
|
|
{
|
|
if (val < min) val = min;
|
|
if (val > max) val = max;
|
|
return val;
|
|
}
|
|
|
|
float PID_Update(float Value, float Current, PID_Data& Coef, float de)
|
|
{
|
|
float p, i, d;
|
|
|
|
float e = Value - Current;
|
|
|
|
p = e * Coef.P.C;
|
|
p = Saturation(p, Coef.P.Min, Coef.P.Max);
|
|
|
|
i = Coef.i;
|
|
i += e * Coef.I.C;
|
|
i = Saturation(i, Coef.I.Min, Coef.I.Max);
|
|
Coef.i = i;
|
|
|
|
d = de * Coef.D.C;
|
|
d = Saturation(d, Coef.D.Min, Coef.D.Max);
|
|
|
|
return Saturation(p + i + d, Coef.Min, Coef.Max);
|
|
}
|