James Batchelar / PidController

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

pidContoller.cpp@1:15c12a119814, 2017-09-04 (annotated)

Committer:
batchee7
Date:
Mon Sep 04 15:50:32 2017 +0000
Revision:
1:15c12a119814
Update Release Ready for Testing;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
batchee7 1:15c12a119814 1 #include "mbed.h"
batchee7 1:15c12a119814 2 #include "pidContoller.h"
batchee7 1:15c12a119814 3
batchee7 1:15c12a119814 4 pidContoller::pidContoller(){
batchee7 1:15c12a119814 5 elapsedTime =0;
batchee7 1:15c12a119814 6 mode = MANUAL;
batchee7 1:15c12a119814 7 }
batchee7 1:15c12a119814 8
batchee7 1:15c12a119814 9 int pidContoller::Calculate(float SP, float PV)
batchee7 1:15c12a119814 10 {
batchee7 1:15c12a119814 11 float CV; //(mm/s) Control Variable
batchee7 1:15c12a119814 12 float IntegralAction; // Integral Contribution to Output
batchee7 1:15c12a119814 13 float DerivativeAction; // Derivative Contribution to Output
batchee7 1:15c12a119814 14
batchee7 1:15c12a119814 15 if (mode == MANUAL)
batchee7 1:15c12a119814 16 {
batchee7 1:15c12a119814 17 CV = SP;
batchee7 1:15c12a119814 18 accumError = 0;
batchee7 1:15c12a119814 19 lastError = 0;
batchee7 1:15c12a119814 20 }
batchee7 1:15c12a119814 21 else
batchee7 1:15c12a119814 22 {
batchee7 1:15c12a119814 23 //Calc error
batchee7 1:15c12a119814 24 error = SP -PV;
batchee7 1:15c12a119814 25 IntegralAction = K_i*(accumError + error);
batchee7 1:15c12a119814 26 DerivativeAction = K_d*(error - lastError);
batchee7 1:15c12a119814 27
batchee7 1:15c12a119814 28 //-- PID Calculation
batchee7 1:15c12a119814 29 CV = bias + K_p*error, + IntegralAction - DerivativeAction;
batchee7 1:15c12a119814 30
batchee7 1:15c12a119814 31 //-- Clamp Integral if Output is Saturated
batchee7 1:15c12a119814 32 if ((CV > maxLimit) || (CV < minLimit))
batchee7 1:15c12a119814 33 {
batchee7 1:15c12a119814 34 accumError = accumError;
batchee7 1:15c12a119814 35 }
batchee7 1:15c12a119814 36 else
batchee7 1:15c12a119814 37 {
batchee7 1:15c12a119814 38 accumError += error;
batchee7 1:15c12a119814 39 }
batchee7 1:15c12a119814 40
batchee7 1:15c12a119814 41 //-- Set Error for Next Scan
batchee7 1:15c12a119814 42 lastError = error;
batchee7 1:15c12a119814 43 }
batchee7 1:15c12a119814 44
batchee7 1:15c12a119814 45
batchee7 1:15c12a119814 46 //Check to See Output is Within Limits
batchee7 1:15c12a119814 47 if (CV > maxLimit){CV= maxLimit;}
batchee7 1:15c12a119814 48 if (CV < minLimit){CV= minLimit;}
batchee7 1:15c12a119814 49
batchee7 1:15c12a119814 50
batchee7 1:15c12a119814 51 if (collectDiagnostics){BuildDiagMessage(SP,PV, CV*scalar, K_p*error, IntegralAction, DerivativeAction);}
batchee7 1:15c12a119814 52
batchee7 1:15c12a119814 53 //Convert from mm/s to 0-100%
batchee7 1:15c12a119814 54 return (int)(CV*scalar)*10;
batchee7 1:15c12a119814 55 }
batchee7 1:15c12a119814 56
batchee7 1:15c12a119814 57 void pidContoller::UpdateSettings(float Bias, float PropGain, float IntGain, float DiffGain, float OutputMin, float OutputMax, float OutputScale){
batchee7 1:15c12a119814 58 bias = Bias;
batchee7 1:15c12a119814 59 K_p = PropGain;
batchee7 1:15c12a119814 60 K_i = IntGain;
batchee7 1:15c12a119814 61 K_d = DiffGain;
batchee7 1:15c12a119814 62 minLimit = OutputMin;
batchee7 1:15c12a119814 63 maxLimit = OutputMax;
batchee7 1:15c12a119814 64 scalar = OutputScale;
batchee7 1:15c12a119814 65 }
batchee7 1:15c12a119814 66
batchee7 1:15c12a119814 67 void pidContoller::StartDiag(void){
batchee7 1:15c12a119814 68 elapsedTime =0;
batchee7 1:15c12a119814 69 }
batchee7 1:15c12a119814 70
batchee7 1:15c12a119814 71 void pidContoller::BuildDiagMessage(float SP, float PV, float PWM, float PropAction, float IntAction, float DifAction){
batchee7 1:15c12a119814 72 sprintf(diagMsg, "P %d %0.4f, %0.4f %0.1f %0.4f %0.4f %0.4f\n",
batchee7 1:15c12a119814 73 elapsedTime, SP, PV, PWM, PropAction, IntAction, DifAction);
batchee7 1:15c12a119814 74 elapsedTime += RATE;
batchee7 1:15c12a119814 75 if (elapsedTime > 32000){EndDiag();}
batchee7 1:15c12a119814 76 }
batchee7 1:15c12a119814 77