Hi,

I just finished my first PID code I wrote from scratch (from the theory) in order to fully understand it. Because I am also a noob in math, I thought I would be fun to let you guys comment on how I am doing.

I did study some other implementations of PID algorithms and each had small (and not so small) variation in how the math was done. That's why I decided to give my interpretation.

This class is part of my

Arduino Template Library. It is written specifically to cover one responsibility only and work with the other (template) classes I have in the library. So for instance, the PID class does not do time tracking, I have other classes in the library for that. The user of the class has to typedef its own class hierarchies for the situation the classes are used in.

/*

BaseT is used as a base class and implements:

T getFeedback()

unsigned int getDeltaTime()

T getSmallestAcceptableError()

T is the data type that hold the values. Either float or double.

Algorithm:

Error = SetPoint - Feedback

P = Error * gainP

I = Sum(previous-I's) + ((Error * deltaTime) * gainI)

D = (previous-Error / deltaTime) * gainD

PI = P + I

PD = P + D

PID = P + I + D

*/

template<class BaseT, typename T>

class PID : public BaseT

{

public:

T P(T setPoint, T gainP)

{

T input = BaseT::getFeedback();

T error = CalcError(setPoint, input);

return CalcP(error, gainP);

}

T P_D(T setPoint, T gainP, T gainD)

{

T input = BaseT::getFeedback();

T error = CalcError(setPoint, input);

unsigned int deltaTime = BaseT::getDeltaTime();

return CalcP(error, gainP) + CalcD(error, deltaTime, gainD);

}

T P_I(T setPoint, T gainP, T gainI)

{

T input = BaseT::getFeedback();

T error = CalcError(setPoint, input);

unsigned int deltaTime = BaseT::getDeltaTime();

return CalcP(error, gainP) + CalcI(error, deltaTime, gainI);

}

T P_I_D(T setPoint, T gainP, T gainI, T gainD)

{

T input = BaseT::getFeedback();

T error = CalcError(setPoint, input);

unsigned int deltaTime = BaseT::getDeltaTime();

return CalcP(error, gainP) + CalcI(error, deltaTime, gainI) + CalcD(error, deltaTime, gainD);

}

private:

T _integralAcc;

T _lastError;

inline T CalcError(T setPoint, T input)

{

T error = setPoint - input;

if (error < BaseT::getSmallestAcceptableError() && error > 0 ||

error > -BaseT::getSmallestAcceptableError() && error < 0)

{

error = 0;

}

return error;

}

inline T CalcP(T error, T gain)

{

return error * gain;

}

inline T CalcI(T error, unsigned int deltaTime, T gain)

{

_integralAcc += (error * deltaTime) * gain;

return _integralAcc;

}

inline T CalcD(T error, unsigned int deltaTime, T gain)

{

T value = ((_lastError - error) / deltaTime) * gain;

_lastError = error;

return value;

}

};

So please comment on the correctness of the math especially.

Thanx!