James Batchelar / PidControllerV3

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

Dependents:   ApexPID

Fork of PidControllerV2 by James Batchelar

PidController.cpp@6:99403113343f, 2018-05-07 (annotated)

Committer:
batchee7
Date:
Mon May 07 05:15:19 2018 +0000
Revision:
6:99403113343f
Parent:
5:1206105e20bd 
InitialRelease

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 6:99403113343f 4 PidController::PidController(){
batchee7 1:15c12a119814 5 elapsedTime =0;
batchee7 1:15c12a119814 6 mode = MANUAL;
batchee7 1:15c12a119814 7 }
batchee7 1:15c12a119814 8
batchee7 6:99403113343f 9 float PidController::Calculate(float SP, float PV)
batchee7 1:15c12a119814 10 {
batchee7 1:15c12a119814 11 float CV; //(mm/s) Control Variable
batchee7 6:99403113343f 12 float ProportionalAction;
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 6:99403113343f 18 CV = SP; //Write Directly to Output
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 6:99403113343f 25 error = SP - PV;
batchee7 6:99403113343f 26 ProportionalAction = K_p*error ;
batchee7 1:15c12a119814 27 IntegralAction = K_i*(accumError + error);
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 6:99403113343f 33 CV = bias + ProportionalAction + IntegralAction + DerivativeAction;
batchee7 5:1206105e20bd 34 if (CV>0) {CV = sqrt(CV);}
batchee7 6:99403113343f 35 else if (CV<0)
batchee7 6:99403113343f 36 {
batchee7 6:99403113343f 37 CV = sqrt(CV*-1.0);
batchee7 6:99403113343f 38 CV = CV*-1.0;
batchee7 6:99403113343f 39 }
batchee7 5:1206105e20bd 40 }
batchee7 4:b590bd8fec6f 41 else
batchee7 5:1206105e20bd 42 {
batchee7 5:1206105e20bd 43 CV= 0;
batchee7 5:1206105e20bd 44 accumError = 0;
batchee7 5:1206105e20bd 45 }
batchee7 1:15c12a119814 46
batchee7 4:b590bd8fec6f 47 //-- Only allow the Controller to integrate if the output isnt saturated
batchee7 6:99403113343f 48 if ((CV < maxLimit) || (CV > minLimit)){accumError += error;}
batchee7 4:b590bd8fec6f 49
batchee7 4:b590bd8fec6f 50 //-- Save Current Input for Next Loop
batchee7 5:1206105e20bd 51 prevError = error;
batchee7 1:15c12a119814 52 }
batchee7 6:99403113343f 53
batchee7 6:99403113343f 54 //Check to See Output is Within Limits
batchee7 6:99403113343f 55 if (CV > maxLimit){CV= maxLimit;}
batchee7 6:99403113343f 56 if (CV < minLimit){CV= minLimit;}
batchee7 1:15c12a119814 57
batchee7 4:b590bd8fec6f 58 //-- Make message to send to GUI
batchee7 6:99403113343f 59 if (collectDiagnostics)//{BuildDiagMessage(SP,PV, CV, K_p*error, IntegralAction, DerivativeAction);}
batchee7 6:99403113343f 60 {
batchee7 6:99403113343f 61 int_to_byte(diagMsg, elapsedTime);
batchee7 6:99403113343f 62 float_to_byte(diagMsg +2, &SP);
batchee7 6:99403113343f 63 float_to_byte(diagMsg +6, &PV);
batchee7 6:99403113343f 64 float_to_byte(diagMsg +10, &CV);
batchee7 6:99403113343f 65 float_to_byte(diagMsg +14, &ProportionalAction);
batchee7 6:99403113343f 66 float_to_byte(diagMsg +18, &IntegralAction);
batchee7 6:99403113343f 67 float_to_byte(diagMsg +22, &DerivativeAction);
batchee7 6:99403113343f 68 }
batchee7 1:15c12a119814 69
batchee7 5:1206105e20bd 70 return CV;
batchee7 1:15c12a119814 71 }
batchee7 1:15c12a119814 72
batchee7 5:1206105e20bd 73 void PidController::UpdateSettings(float Bias, float PropGain, float IntGain, float DiffGain, float OutputMin, float OutputMax){
batchee7 1:15c12a119814 74 bias = Bias;
batchee7 1:15c12a119814 75 K_p = PropGain;
batchee7 1:15c12a119814 76 K_i = IntGain;
batchee7 1:15c12a119814 77 K_d = DiffGain;
batchee7 1:15c12a119814 78 minLimit = OutputMin;
batchee7 1:15c12a119814 79 maxLimit = OutputMax;
batchee7 5:1206105e20bd 80 return;
batchee7 1:15c12a119814 81 }
batchee7 1:15c12a119814 82
batchee7 4:b590bd8fec6f 83 void PidController::UpdateSettings(float OutputMin, float OutputMax){
batchee7 4:b590bd8fec6f 84 minLimit = OutputMin;
batchee7 4:b590bd8fec6f 85 maxLimit = OutputMax;
batchee7 5:1206105e20bd 86 return;
batchee7 4:b590bd8fec6f 87 }
batchee7 4:b590bd8fec6f 88
batchee7 4:b590bd8fec6f 89
batchee7 2:dd64c2cf9066 90 void PidController::StartDiag(void){
batchee7 1:15c12a119814 91 elapsedTime =0;
batchee7 3:c169d08a9d0b 92 collectDiagnostics = true;
batchee7 5:1206105e20bd 93 return;
batchee7 1:15c12a119814 94 }
batchee7 3:c169d08a9d0b 95
batchee7 3:c169d08a9d0b 96 void PidController::EndDiag(void){
batchee7 3:c169d08a9d0b 97 collectDiagnostics = false;
batchee7 5:1206105e20bd 98 return;
batchee7 1:15c12a119814 99 }
batchee7 1:15c12a119814 100
batchee7 6:99403113343f 101 void PidController::GetDiagnosticsMessage(char *data){
batchee7 6:99403113343f 102 memcpy(data, &diagMsg, 27 );
batchee7 5:1206105e20bd 103 return;
batchee7 3:c169d08a9d0b 104 }
batchee7 6:99403113343f 105 //-- Helper Functions
batchee7 6:99403113343f 106
batchee7 6:99403113343f 107 //-- Convert floating point to byte array
batchee7 6:99403113343f 108 void PidController::float_to_byte(char *data, float *val) {
batchee7 6:99403113343f 109 memcpy(data, val, sizeof(float));
batchee7 6:99403113343f 110 }
batchee7 3:c169d08a9d0b 111
batchee7 6:99403113343f 112 //-- Convert integer to byte array
batchee7 6:99403113343f 113 void PidController::int_to_byte(char *data, uint16_t val) {
batchee7 6:99403113343f 114 memcpy(data, &val, sizeof val);
batchee7 6:99403113343f 115 }