Digital PID controller implementation in C++

Started by steven02 5 years ago12 replieslatest reply 5 years ago7961 views

Hello all,

I have implemented a discrete PID controller as a class in the C++ language. I have attempted to include advanced features like:

*bumpless transition

*antiwind-up mechanism

*filtering of derivative part

I have also needed maybe nonstandard feature namely blocking of the PID controller which means that the PID controller is inhibited and some prescribed value is set at its output. This feature seems to work but I have encountered that as soon as I unblock the controller again a abrupt change at its output occurs. So I have got some doubts regarding my implementation. 

My implementation has following features:

a) incremental (velocity) form is used

b) integral is approximated by trapezoidal rule

c) derivative is approximated by backward difference

d) derivative part contains filtering pole for limiting amplification of noise

e) derivative is applied only on the controlled variable with negative sign as a prevention against so called derivative kick

The source code is following:

Class declaration

class PID{


PID(float* ref, 
    float* act,
    float* trk, 
    uint32_t* bitsArray, 
    uint32_t blkSig, 
    float* output, 
    bool directActing, 
    float* Kp, 
    float* Ti, 
    float* Td, 
    float* N, 
    float execPer, 
    float* outMin, 
    float* outMax, 
    float outBlk);
virtual ~PID();

void Update(void);

float*    m_Ref;      // reference value signal
float*    m_Act;      // actual value signal
float     m_Act1;     // last actual value
float     m_Act2;     // second last actual value
float*    m_Trk;      // tracking value signal
uint32_t* m_BitsArray;// pointer to bits array where the logic signal for blocking is placed
uint32_t  m_BlkSig;   // logic signal for blocking (=1) and unblocking (=0) the controller
float*    m_Output;   // controller output
bool      m_DirAct;   // true - controller is direct acting, false - controller is reverse acting
float*    m_Kp;       // proportional gain
float*    m_Ti;       // integral time constant
float*    m_Td;       // derivative time constant
float*    m_N;        // filtering pole for derivative part
float     m_ExecPer;  // controller execution period
float*    m_OutMin;   // low limit of the controller output
float*    m_OutMax;   // high limit of the controller output
float     m_OutBlk;   // controller output in case the controller is blocked
float     m_DUp;      // increment of proportional part
float     m_DUi;      // increment of integral part
float     m_DUd;      // increment of derivative part
float     m_Ud;       // derivative part
float     m_Ud1;      // last derivative part
float     m_Ud2;      // second last derivative part
float     m_DU;       // increment of control value
float     m_U;        // unsaturated control value
float     m_U1;       // last control value
float     m_SatU;     // saturated control value
float     m_Err;      // control error
float     m_Err1;     // last control error
float     m_Err2;     // second last control error
float     m_Ki;       // integral gain
float     m_Ad;       // coefficient of difference equation
float     m_Bd;       // coefficient of difference equation};

Class definition

float* ref, 
float* act, 
float* trk, 
uint32_t* bitsArray, 
uint32_t blkSig, 
float* output, 
bool directActing,                
float* Kp, 
float* Ti, 
float* Td, 
float* N, 
float execPer, 
float* outMin, 
float* outMax, 
float outBlk):
m_DUp  = 0.0;
m_DUi  = 0.0;
m_DUd  = 0.0;
m_Ud   = 0.0;
m_Ud1  = 0.0;
m_Ud2  = 0.0;
m_DU   = 0.0;
m_U    = 0.0;
m_U1   = 0.0;
m_SatU = 0.0;
m_Err  = 0.0;
m_Err1 = 0.0;
m_Err2 = 0.0;
m_Act1 = 0.0;
m_Act2 = 0.0;
m_Ki   = 0.0;
m_Ad   = 0.0;
m_Bd   = 0.0;
// TODO Auto-generated destructor stub

void ControlBlocks::PID::Update(void){

if(Utils::TestBitClr(m_BitsArray, m_BlkSig)){

// controller is not blocked

// constants
m_Ki = (*m_Kp*m_ExecPer)/(2*(*m_Ti));                 // Ki = (Kp*T)/(2*Ti)
m_Ad = *m_Td/(*m_Td+*m_N*m_ExecPer);                  // Ad = Td/(Td+N*T)
m_Bd = (*m_Kp*(*m_Td)*(*m_N))/(*m_Td+*m_N*m_ExecPer); // Bd = (Kp*Td*N)/(Td+N*T)

m_U1 = *m_Trk; // u(k-1) <- last actually used control value (for bumpless transition)

// control error
m_Err = *m_Ref - *m_Act; // e(k) = r(k) - y(k)}
m_Err = *m_Act - *m_Ref; // e(k) = y(k) - r(k)

// controller output
m_DUp = *m_Kp*(m_Err - m_Err1);                                   // dup(k) = Kp*(e(k) - e(k-1))
m_DUi = m_Ki*(m_Err + m_Err1);                                    // dui(k) = Ki*[e(k) + e(k-1)]
m_DUd = m_Ad*(m_Ud1 - m_Ud2) - m_Bd*(*m_Act - 2*m_Act1 + m_Act2); // dud(k) = Ad*[ud(k-1) - ud(k-2)] - Bd*[y(k) - 2*y(k-1) + y(k-2)]
m_Ud  = m_Ud1 + m_DUd;                                            // ud(k) = ud(k-1) + dud(k)
m_DU  = m_DUp + m_DUi + m_DUd;                                    // du(k)
m_U   = m_U1 + m_DU;                                              // u(k) = u(k-1) + du(k) 

// output limitation
if(m_U > *m_OutMax){
m_SatU = *m_OutMax;
}else if(m_U < *m_OutMin){
m_SatU = *m_OutMin;
m_SatU = m_U;

// passing value to the output
*m_Output = m_SatU;

// state variables update
m_Err2 = m_Err1; // e(k-2) = e(k-1)
m_Err1 = m_Err;  // e(k-1) = e(k)
m_Act2 = m_Act1; // y(k-2) = y(k-1)
m_Act1 = *m_Act; // y(k-1) = y(k)
m_Ud2  = m_Ud1;  // ud(k-2) = ud(k-1)
m_Ud1  = m_Ud;   // ud(k-1) = ud(k)


// controller is blocked

// passing value to the output
*m_Output = m_OutBlk;

// clear memory
m_Err2 = 0.0;
m_Err1 = 0.0;
m_Act2 = 0.0;
m_Act1 = 0.0;
m_Ud2  = 0.0;
m_Ud1  = 0.0;


Can anybody tell me whether there is any mistake in my implementation which causes the abrupt change at the output of the PID controller in case it is unblocked after it was previously blocked? Thanks in advance for any ideas.

#PID #C++

[ - ]
Reply by jimfredDecember 5, 2019

Consider rearranging your 'if' statement by changing 'm_U = m_U1 + m_DU' to something like 'if (manual_override) {m_U = m_OutBlk;} else {m_U   = m_U1 + m_DU;}' where 'manual_override' is your block signal. This effectively creates a switch that feeds m_U from either the PID calculation or the manual override/block value. It will continue to clamp within the saturation range and update 'old' values e.g., m_Err2 etc. 

Caveat: I didn't test this suggestion.

[ - ]
Reply by steven02December 5, 2019

Hello jimfred, 

first of all I would like to say thank you for your reaction. I have just tested your suggestion and the behavior is basically the same. I don't understand why the behavior is same independently of updating the state variables. Do you have any idea?

[ - ]
Reply by jimfredDecember 5, 2019

It sounds like m_DU is big. We could trace it back to find out why. It could simply be that the proportional part simply wants the output to be at a different value. 

As  CustomSarge suggested, you could constrain m_DU before applying it to m_U. You could constrain m_DU in a way similar to the output limit clamp but with different threshold values. That would limit m_DU's influence in changing m_U but, depending on the threshold value, it could limit or delay the controller's response when not in manual/block mode. 

[ - ]
Reply by CustomSargeDecember 5, 2019

I'll suggest, on block mode exit, the difference in output is being resolved in 1 "servo" loop. There should be some constant that is the maximum change per loop. If you already use one on the main control input, then block exit needs to use it too. Just a guess, my C code is hack at best. Good Hunting  <<<)))

[ - ]
Reply by jalticeDecember 5, 2019

I had a similar problem in the past. Try setting your m_Act1 and m_Act2 to equal m_Act when the block is released. This will initialize a starting point and shouldn't have an abrupt change. I think the problem is when you clear the memory, your algorithm has to catch up once the block is released. Good luck.

[ - ]
Reply by mr_banditDecember 5, 2019

Found this blog entry: https://www.embeddedrelated.com/showarticle/943.php

PID Without a PhD

Tim WescottApril 26, 201611 comments

(and given 19 beers !!)

[ - ]
Reply by kevboDecember 5, 2019

The abrupt change in output is to be expected.   

When blocked, you are forcing output to be "X" regardless of process value and setpoint, this will normally result in a large error between your setpoint and process values.

As soon as you unblock, and allow the PID to work, it sees a huge error signal, which gives a big P signal, and also a huge D signal (because you were previously forcing the error to zero)..so it responds by railing the output either to max or minimum to try to fix the error  .

This is the same as giving the controller a step input, and you should expect a similar response, which will depend on tuning.

If you want smooth response, then when you come out of blocked mode, you need to temporarily force the setpoint to the current process value, then ramp it at some rate the system can track to what you actually want.  You will still have some output shift, as the PID loop self-adjusts, but it will be much less than you have now.

It also wouldn't hurt to allow the program to calculate (but ignore) the current error, P, and D values while blocked.   But you still need to force the I value to zero to prevent windup.  This will give the control loop a bit of a "running start" when coming out of blocked mode. Basically move everything but the output and I calculations outside the IF statement, and get rid of the bit where you force state variables to zero (except for I terms)

[ - ]
Reply by matthewbarrDecember 5, 2019

It looks like your bumpless transfer assignment is supposed to prevent this from happening:

m_U1 = *m_Trk;  // u(k-1) <- last actually used control value (for bumpless transition)

The value *m_Trk needs to give you the last value assigned to the *m_Output, m_Satu in normal mode or m_OutBlk in blocked mode. If you're getting an abrupt change exiting blocked mode, perhaps *m_Trk does not reflect the last m_OutBlk value used in blocked mode.

[ - ]
Reply by jmford94December 5, 2019

Yes, to avoid a transient, the integral needs to be initialized to the value that when used in the PID calculation will give the current output value, ignoring the derivative term.  

So m_U1 should be calculated from:

 integral term = (current_output - (current_error * Kp)

[ - ]
Reply by matthewbarrDecember 5, 2019

Yes, or simply re-initialize (zero) the integral error when exiting blocked mode and let it start re-accumulating. Likewise with derivative error, re-initialize previous actual state from first sampled actual state.

This prevents initially large integral or differential error contributions coming out of blocked mode. The loop should behave as though it were in a steady state with a sudden (impulse) change in throttle. First adjustment will depend on proportional error and related Kp.

If this still produces a transition bump, then perhaps Kp is too hot and the system does not respond well to an impulse change in throttle. At this point you have to reconsider your PID tuning, or control the throttle around blocked mode transition as kevbo has suggested.

[ - ]
Reply by jmford94December 5, 2019

The problem with just zeroing it is that sometimes the integral term provides a lot of the command signal to a process, and if you zero it, it can take a long time to build back up to its steady-state value.  This can also point to a poor tuning solution as well.  

[ - ]
Reply by matthewbarrDecember 5, 2019

That's interesting, different from how I've learned to look at integral error.

I've been taught that integral error is intended to prevent the system from running happily with a small proportional error that never gets corrected. The very small proportional errors accumulate as integral error and nudge the system toward the set point. This should be a small and gradual correction, the dominant source of correction only when close to the set point with very small proportional and differential error contributions.

At least that's been my education and experience with motor control solutions, large integral contributions can be conducive to overshoot and instability.

We haven't heard from steven02 in a while, hopefully he's found a way to deal with the source of the bump in his bumpless control logic! There are definitely a number of ways to skin this cat.