## Digital PID controller implementation in C++

Started by 5 years ago●12 replies●latest reply 5 years ago●8041 viewsHello 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{ public: 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); private: 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

ControlBlocks::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): m_Ref(ref), m_Act(act), m_Trk(trk), m_BitsArray(bitsArray), m_BlkSig(blkSig), m_Output(output), m_DirAct(directActing), m_Kp(Kp), m_Ti(Ti), m_Td(Td), m_N(N), m_ExecPer(execPer), m_OutMin(outMin), m_OutMax(outMax), m_OutBlk(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; } ControlBlocks::PID::~PID(){ // 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 if(m_DirAct){ m_Err = *m_Ref - *m_Act; // e(k) = r(k) - y(k)} else{ 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; }else{ 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) }else{ // 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++

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.

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?

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

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.

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 <<<)))

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.

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

# PID Without a PhD

Tim Wescott●April 26, 2016●11 comments

(and given 19 beers !!)

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)

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.

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)

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.

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.

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.