float PID::Compute(float mSetpoint, float mInput) { float mError = mSetpoint - mInput; // input error value unsigned long tn = millis(); // current time in [ms] float dt = (float)(tn - tp); // step interval float P = (float)(kp * mError); // P gain float I = (float)(Ip + ki * mError * dt / 1000.0); // I gain float D = (float)(-1 * (kd * (mInput - pInput) * 1000.0 / dt)); // D gain (measurement derivative) if (anti_windup) // anti-windup (clearing the integral value) { I = 0; } float U = P + I + D; // sum of all components pInput = mInput; // history pError = mError; Ip = I; tp = tn; return U; // output value } |