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@4:b590bd8fec6f, 2017-10-04 (annotated)

Committer:
batchee7
Date:
Wed Oct 04 01:13:49 2017 +0000
Revision:
4:b590bd8fec6f
Parent:
3:c169d08a9d0b
Child:
5:1206105e20bd
Working upload before planning to make more generic

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 4:b590bd8fec6f 4 PidController::PidController(bool sqrt){
batchee7 1:15c12a119814 5 elapsedTime =0;
batchee7 1:15c12a119814 6 mode = MANUAL;
batchee7 4:b590bd8fec6f 7 squareRootOuptut = sqrt;
batchee7 1:15c12a119814 8 }
batchee7 1:15c12a119814 9
batchee7 4:b590bd8fec6f 10 int 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 1:15c12a119814 20 }
batchee7 1:15c12a119814 21 else
batchee7 1:15c12a119814 22 {
batchee7 1:15c12a119814 23 //Calc error
batchee7 4:b590bd8fec6f 24 error = SP - PV;
batchee7 1:15c12a119814 25 IntegralAction = K_i*(accumError + error);
batchee7 4:b590bd8fec6f 26 DerivativeAction = K_d*(PV - lastInput);
batchee7 1:15c12a119814 27
batchee7 1:15c12a119814 28 //-- PID Calculation
batchee7 4:b590bd8fec6f 29 if (SP) {
batchee7 4:b590bd8fec6f 30 CV = K_p*error + IntegralAction - DerivativeAction;
batchee7 4:b590bd8fec6f 31 if ((CV > 0) && squareRootOuptut)CV = sqrt(CV);
batchee7 4:b590bd8fec6f 32 }
batchee7 4:b590bd8fec6f 33 else
batchee7 1:15c12a119814 34
batchee7 4:b590bd8fec6f 35 CV= 0;
batchee7 4:b590bd8fec6f 36
batchee7 4:b590bd8fec6f 37
batchee7 4:b590bd8fec6f 38 //-- Only allow the Controller to integrate if the output isnt saturated
batchee7 4:b590bd8fec6f 39 if ((CV < maxLimit) || (CV > minLimit))
batchee7 1:15c12a119814 40 {
batchee7 1:15c12a119814 41 accumError += error;
batchee7 1:15c12a119814 42 }
batchee7 1:15c12a119814 43
batchee7 4:b590bd8fec6f 44 //-- Told to stop!
batchee7 4:b590bd8fec6f 45 if (!SP) accumError = 0;
batchee7 4:b590bd8fec6f 46
batchee7 4:b590bd8fec6f 47 //-- Save Current Input for Next Loop
batchee7 4:b590bd8fec6f 48 lastInput = PV;
batchee7 4:b590bd8fec6f 49
batchee7 4:b590bd8fec6f 50 //Check to See Output is Within Limits
batchee7 4:b590bd8fec6f 51 if (CV > maxLimit){CV= maxLimit;}
batchee7 4:b590bd8fec6f 52 if (CV < minLimit){CV= minLimit;}
batchee7 1:15c12a119814 53 }
batchee7 1:15c12a119814 54
batchee7 1:15c12a119814 55
batchee7 4:b590bd8fec6f 56 //-- Make message to send to GUI
batchee7 3:c169d08a9d0b 57 if (collectDiagnostics){BuildDiagMessage(SP,PV, CV, K_p*error, IntegralAction, DerivativeAction);}
batchee7 1:15c12a119814 58
batchee7 4:b590bd8fec6f 59 return (int)(CV);
batchee7 1:15c12a119814 60 }
batchee7 1:15c12a119814 61
batchee7 2:dd64c2cf9066 62 void PidController::UpdateSettings(float Bias, float PropGain, float IntGain, float DiffGain, float OutputMin, float OutputMax, float OutputScale){
batchee7 1:15c12a119814 63 bias = Bias;
batchee7 1:15c12a119814 64 K_p = PropGain;
batchee7 1:15c12a119814 65 K_i = IntGain;
batchee7 1:15c12a119814 66 K_d = DiffGain;
batchee7 1:15c12a119814 67 minLimit = OutputMin;
batchee7 1:15c12a119814 68 maxLimit = OutputMax;
batchee7 1:15c12a119814 69 scalar = OutputScale;
batchee7 1:15c12a119814 70 }
batchee7 1:15c12a119814 71
batchee7 4:b590bd8fec6f 72 void PidController::UpdateSettings(float OutputMin, float OutputMax){
batchee7 4:b590bd8fec6f 73 minLimit = OutputMin;
batchee7 4:b590bd8fec6f 74 maxLimit = OutputMax;
batchee7 4:b590bd8fec6f 75 }
batchee7 4:b590bd8fec6f 76
batchee7 4:b590bd8fec6f 77
batchee7 2:dd64c2cf9066 78 void PidController::StartDiag(void){
batchee7 1:15c12a119814 79 elapsedTime =0;
batchee7 3:c169d08a9d0b 80 collectDiagnostics = true;
batchee7 1:15c12a119814 81 }
batchee7 3:c169d08a9d0b 82
batchee7 3:c169d08a9d0b 83 void PidController::EndDiag(void){
batchee7 3:c169d08a9d0b 84 collectDiagnostics = false;
batchee7 3:c169d08a9d0b 85
batchee7 1:15c12a119814 86 }
batchee7 1:15c12a119814 87
batchee7 3:c169d08a9d0b 88 void PidController::BuildDiagMessage(float SetPoint, float ProcessVar, float PWM, float PropAction, float IntAction, float DifAction){
batchee7 4:b590bd8fec6f 89 sprintf(diagMsg, "P %d %0.4f %0.4f %0.1f %0.4f %0.4f %0.4f\n", elapsedTime, SetPoint, ProcessVar, PWM, PropAction, IntAction, DifAction);
batchee7 3:c169d08a9d0b 90 elapsedTime += RATE;
batchee7 3:c169d08a9d0b 91 if (elapsedTime > 32000){sprintf(diagMsg, "R");}
batchee7 3:c169d08a9d0b 92 }
batchee7 3:c169d08a9d0b 93