Buy an Axon, Axon II, or Axon Mote and build a great robot, while helping to support SoR.
0 Members and 1 Guest are viewing this topic.
/* 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 - 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; }};
Math looks OK.On the Derivitive control - that tends to get jumpy due to noise on the input - one often does a little filtering of the input to the derivitive. But that complicates the tuning, too much filtering and the D does nothing. YMMV
filtered_error = (1 - filter_constant)*filtered_error + filter_constant*error
Thank you. It's all about (tweaking) constants, I see In Quote from: jkerns on May 12, 2013, 10:19:57 AMfiltered_error = (1 - filter_constant)*filtered_error + filter_constant*errorIs the bold 'filtered_error' a previous filtered_error value or...?
Also: is it likely you would dynamically (at runtime) switch using P, I or D terms for a single controller process? I mean do I need to take into account that all methods update the delta-time, I-accumulator and last-error variables in order to allow mixed calling of the P, PI, PD and PID methods?
Yes, I wondered about that (about I). It keeps building up and up and you would have to undo all that. To my naive mind that would cause overshoot and oscillation. So if the I-term reaches the max value it just stops accumulating? Or when the combined output (return value from my methods) reaches max value? I would need some value range management in my class anyway, especially when mulitple P/I/D Terms are added together...
Thinking on the I-term some more, I would even say that if your error is 0 (or within the acceptable error-band) I would simply stop using the I-term altogether and perhaps reset it (to what value I don't know - some % of its current value?) for when it is used later on (when we travel outside the error band) it would otherwise make the system 'jerk'.
PS: Had already found your YT channel and I have no trouble sleeping ;-) Thanx.
Another option to deal with constantly accumulating I error is to use a leaky integrator for the I term, just like you do for the D term. Except for the D term, it's a single-pole low-pass filter; for the I term it's a single-term high-pass filter.
previousError - newError
newError - previousError
I've never switched controller types in mid stream. It isn't uncommon to modify the gains as a function of operating conditions (referred to as gain scheduling) but switching between PI and PD or whatever would be unusual.
/* 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 = ((Error - previous-Error) / deltaTime) * gainD PI = P + I PD = P + D PID = P + I + D*/template<class BaseT, typename T>class PID : public BaseT{public: // pass a gain of zero to bypass a specific term. T Process(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; _integralAcc = 0; // Good idea?? } return error; } inline T CalcP(T error, T gain) { if (gain == 0) return 0; return error * gain; } inline T CalcI(T error, unsigned int deltaTime, T gain) { if (gain != 0) { _integralAcc += (error * deltaTime) * gain; } return _integralAcc; } inline T CalcD(T error, unsigned int deltaTime, T gain) { T value = 0; if (gain != 0) { value = ((error - _lastError) / deltaTime) * gain; } _lastError = error; return value; }};
newError - prevError is just -(prevError - newError) so you just flip the sign of the D gain.
Let's look at an example, if the error is increasing positive, you want the output from the controller to go negative to compensate. Right?
Depending on which way you subtract (old-new or new-old) you get a different sign on the error term so you would need a different sign on the D gain to get the correction to work in the proper direction.
Pet peeve: Even though it's well known that there are many conventions in various ways, authors tend to always forget to document what conventions they use in their particular writing/paper/tutorial.