Hi there!
In my earlier post, I described the equations that drive the PID controller behavior. Finally here is the PID controller implementation. The code is very simple: there are just two things to notice:
- The thread is executed once each SampleTime milliseconds. There’s no warranty that it will be actually executed exactly when it’s due, but the PID controller is robust enough to tolerate small delays… at least for our Mindstorms applications.
- The u is saturated to the defined boundaries before being assigned to the OutputSignal .
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 |
/// /// The core of the PID alogrithm computation /// protected void PIDThreadComputation(){ Thread.CurrentThread.IsBackground = true; float e; // error at instant k float e1; // error at instant k-1 float e2; // error at instant k-1 float u; // control signal at instant k float delta_u; // control signal increment at instant k // Initialization e = 0f; e1 = 0f; e2 = 0f; u = 0f; delta_u = 0f; // PID Computation while (!stopPIDThread.WaitOne(SampleTime)) { // updating "memory" variables e2 = e1; e1 = e; // updating error e = SetPoint - InputSignal; // computating delta_u and u delta_u = k1 * e + k2 * e1 + k3 * e2; u = u + delta_u; // Saturating u if (u > MaxPower) u = MaxPower; if (u < MinPower) u = MinPower; OutputSignal = (sbyte) u; } } |
You can compile the above code using Xamarin Studio 5.10.1 (Build 6), with Mono 4.2.1 as active runtime and the Monobrick Firmware Library 1.2.0.39486 to let it work on board the Lego Mindstorms Ev3 with the Monobrick firmware.
Full code is available here and I will soon post a sample application. Stay tuned… And Happy New Year!
The PID Controller – Part 3