James Batchelar / PidControllerV2

Simple PID Controller with Integral Windup Supports creating a diagnostics message to send to a GUI

Fork of PidController by James Batchelar

PidController.cpp@5:1206105e20bd, 2017-10-31 (annotated)

Committer:
batchee7
Date:
Tue Oct 31 03:46:43 2017 +0000
Revision:
5:1206105e20bd
Parent:
4:b590bd8fec6f 
Final updates;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
batchee7 1:15c12a119814 1 #include "mbed.h"
batchee7 2:dd64c2cf9066 2 #include "PidController.h"
batchee7 1:15c12a119814 3
batchee7 5:1206105e20bd 4 PidController::PidController(char c){
batchee7 1:15c12a119814 5 elapsedTime =0;
batchee7 1:15c12a119814 6 mode = MANUAL;
batchee7 5:1206105e20bd 7 diagChar = c;
batchee7 1:15c12a119814 8 }
batchee7 1:15c12a119814 9
batchee7 5:1206105e20bd 10 float PidController::Calculate(float SP, float PV, float ManualMV)
batchee7 1:15c12a119814 11 {
batchee7 1:15c12a119814 12 float CV; //(mm/s) Control Variable
batchee7 1:15c12a119814 13 float IntegralAction; // Integral Contribution to Output
batchee7 1:15c12a119814 14 float DerivativeAction; // Derivative Contribution to Output
batchee7 1:15c12a119814 15
batchee7 1:15c12a119814 16 if (mode == MANUAL)
batchee7 1:15c12a119814 17 {
batchee7 4:b590bd8fec6f 18 CV = ManualMV; //Write Manual Manipulated Variable
batchee7 1:15c12a119814 19 accumError = 0;
batchee7 5:1206105e20bd 20 prevError = 0;
batchee7 1:15c12a119814 21 }
batchee7 1:15c12a119814 22 else
batchee7 1:15c12a119814 23 {
batchee7 1:15c12a119814 24 //Calc error
batchee7 4:b590bd8fec6f 25 error = SP - PV;
batchee7 1:15c12a119814 26 IntegralAction = K_i*(accumError + error);
batchee7 5:1206105e20bd 27 //DerivativeAction = K_d*(PV - lastInput);
batchee7 5:1206105e20bd 28 DerivativeAction = K_d*(error - prevError);
batchee7 1:15c12a119814 29
batchee7 1:15c12a119814 30 //-- PID Calculation
batchee7 5:1206105e20bd 31 if (SP)
batchee7 5:1206105e20bd 32 {
batchee7 5:1206105e20bd 33 CV = bias + K_p*error + IntegralAction + DerivativeAction;
batchee7 5:1206105e20bd 34 if (CV>0) {CV = sqrt(CV);}
batchee7 5:1206105e20bd 35 }
batchee7 4:b590bd8fec6f 36 else
batchee7 5:1206105e20bd 37 {
batchee7 5:1206105e20bd 38 CV= 0;
batchee7 5:1206105e20bd 39 accumError = 0;
batchee7 5:1206105e20bd 40 }
batchee7 1:15c12a119814 41
batchee7 4:b590bd8fec6f 42 //-- Only allow the Controller to integrate if the output isnt saturated
batchee7 4:b590bd8fec6f 43 if ((CV < maxLimit) || (CV > minLimit))
batchee7 1:15c12a119814 44 {
batchee7 1:15c12a119814 45 accumError += error;
batchee7 1:15c12a119814 46 }
batchee7 4:b590bd8fec6f 47
batchee7 4:b590bd8fec6f 48 //-- Save Current Input for Next Loop
batchee7 5:1206105e20bd 49 //lastInput = PV;
batchee7 5:1206105e20bd 50 prevError = error;
batchee7 4:b590bd8fec6f 51
batchee7 4:b590bd8fec6f 52 //Check to See Output is Within Limits
batchee7 4:b590bd8fec6f 53 if (CV > maxLimit){CV= maxLimit;}
batchee7 4:b590bd8fec6f 54 if (CV < minLimit){CV= minLimit;}
batchee7 1:15c12a119814 55 }
batchee7 1:15c12a119814 56
batchee7 1:15c12a119814 57
batchee7 4:b590bd8fec6f 58 //-- Make message to send to GUI
batchee7 3:c169d08a9d0b 59 if (collectDiagnostics){BuildDiagMessage(SP,PV, CV, K_p*error, IntegralAction, DerivativeAction);}
batchee7 1:15c12a119814 60
batchee7 5:1206105e20bd 61 return CV;
batchee7 1:15c12a119814 62 }
batchee7 1:15c12a119814 63
batchee7 5:1206105e20bd 64 void PidController::UpdateSettings(float Bias, float PropGain, float IntGain, float DiffGain, float OutputMin, float OutputMax){
batchee7 1:15c12a119814 65 bias = Bias;
batchee7 1:15c12a119814 66 K_p = PropGain;
batchee7 1:15c12a119814 67 K_i = IntGain;
batchee7 1:15c12a119814 68 K_d = DiffGain;
batchee7 1:15c12a119814 69 minLimit = OutputMin;
batchee7 1:15c12a119814 70 maxLimit = OutputMax;
batchee7 5:1206105e20bd 71 return;
batchee7 1:15c12a119814 72 }
batchee7 1:15c12a119814 73
batchee7 4:b590bd8fec6f 74 void PidController::UpdateSettings(float OutputMin, float OutputMax){
batchee7 4:b590bd8fec6f 75 minLimit = OutputMin;
batchee7 4:b590bd8fec6f 76 maxLimit = OutputMax;
batchee7 5:1206105e20bd 77 return;
batchee7 4:b590bd8fec6f 78 }
batchee7 4:b590bd8fec6f 79
batchee7 4:b590bd8fec6f 80
batchee7 2:dd64c2cf9066 81 void PidController::StartDiag(void){
batchee7 1:15c12a119814 82 elapsedTime =0;
batchee7 3:c169d08a9d0b 83 collectDiagnostics = true;
batchee7 5:1206105e20bd 84 return;
batchee7 1:15c12a119814 85 }
batchee7 3:c169d08a9d0b 86
batchee7 3:c169d08a9d0b 87 void PidController::EndDiag(void){
batchee7 3:c169d08a9d0b 88 collectDiagnostics = false;
batchee7 5:1206105e20bd 89 return;
batchee7 1:15c12a119814 90 }
batchee7 1:15c12a119814 91
batchee7 3:c169d08a9d0b 92 void PidController::BuildDiagMessage(float SetPoint, float ProcessVar, float PWM, float PropAction, float IntAction, float DifAction){
batchee7 5:1206105e20bd 93 //sprintf(diagMsg, "%c %d %0.4f %0.4f %0.1f %0.4f %0.4f %0.4f\n", diagChar, elapsedTime, SetPoint, ProcessVar, PWM, PropAction, IntAction, DifAction);
batchee7 5:1206105e20bd 94 sprintf(diagMsg, "%c %d %0.4f %0.4f %0.1f 0.0 0.0 0.0\n", diagChar, elapsedTime, SetPoint, ProcessVar, PWM);
batchee7 3:c169d08a9d0b 95 elapsedTime += RATE;
batchee7 5:1206105e20bd 96 return;
batchee7 3:c169d08a9d0b 97 }
batchee7 3:c169d08a9d0b 98