James Batchelar / PidController

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

PidController.cpp@3:c169d08a9d0b, 2017-09-06 (annotated)

Committer:
batchee7
Date:
Wed Sep 06 23:45:40 2017 +0000
Revision:
3:c169d08a9d0b
Parent:
2:dd64c2cf9066 
Corrections to output generation

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 2:dd64c2cf9066 4 PidController::PidController(){
batchee7 1:15c12a119814 5 elapsedTime =0;
batchee7 1:15c12a119814 6 mode = MANUAL;
batchee7 1:15c12a119814 7 }
batchee7 1:15c12a119814 8
batchee7 2:dd64c2cf9066 9 int PidController::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 3:c169d08a9d0b 51 if (collectDiagnostics){BuildDiagMessage(SP,PV, CV, K_p*error, IntegralAction, DerivativeAction);}
batchee7 1:15c12a119814 52
batchee7 1:15c12a119814 53 //Convert from mm/s to 0-100%
batchee7 3:c169d08a9d0b 54 return (int)(CV*scalar);
batchee7 1:15c12a119814 55 }
batchee7 1:15c12a119814 56
batchee7 2:dd64c2cf9066 57 void PidController::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 2:dd64c2cf9066 67 void PidController::StartDiag(void){
batchee7 1:15c12a119814 68 elapsedTime =0;
batchee7 3:c169d08a9d0b 69 collectDiagnostics = true;
batchee7 1:15c12a119814 70 }
batchee7 3:c169d08a9d0b 71
batchee7 3:c169d08a9d0b 72 void PidController::EndDiag(void){
batchee7 3:c169d08a9d0b 73 collectDiagnostics = false;
batchee7 3:c169d08a9d0b 74
batchee7 1:15c12a119814 75 }
batchee7 1:15c12a119814 76
batchee7 3:c169d08a9d0b 77 void PidController::BuildDiagMessage(float SetPoint, float ProcessVar, float PWM, float PropAction, float IntAction, float DifAction){
batchee7 3:c169d08a9d0b 78 sprintf(diagMsg, "P %d %0.4f %0.4f %0.1f %0.4f %0.4f %0.4f\n",
batchee7 3:c169d08a9d0b 79 elapsedTime, SetPoint, ProcessVar, PWM, PropAction, IntAction, DifAction);
batchee7 3:c169d08a9d0b 80 elapsedTime += RATE;
batchee7 3:c169d08a9d0b 81 if (elapsedTime > 32000){sprintf(diagMsg, "R");}
batchee7 3:c169d08a9d0b 82 }
batchee7 3:c169d08a9d0b 83