Buggy bois / Mbed 2 deprecated UDITTEST

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

PID2.h@6:477382219bcf, 2019-03-26 (annotated)

Committer:
mazdo25
Date:
Tue Mar 26 16:19:47 2019 +0000
Revision:
6:477382219bcf
Parent:
5:f1613df66ceb
Child:
9:cefa177c1353
LATEST WORKING

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mazdo25 5:f1613df66ceb 1 class PID2
mazdo25 5:f1613df66ceb 2 {
mazdo25 5:f1613df66ceb 3 private:
mazdo25 5:f1613df66ceb 4 float Kp_, Ki_, Kd_, Ts, PrevErr, PrevPrevErr, prevControlAction, setPoint;
mazdo25 5:f1613df66ceb 5
mazdo25 5:f1613df66ceb 6 float inMin_;
mazdo25 5:f1613df66ceb 7 float inMax_;
mazdo25 5:f1613df66ceb 8
mazdo25 5:f1613df66ceb 9 float* outMin_;
mazdo25 5:f1613df66ceb 10 float* outMax_;
mazdo25 5:f1613df66ceb 11
mazdo25 5:f1613df66ceb 12 public:
mazdo25 5:f1613df66ceb 13 PID2 (float Kp, float Ki, float Kd, float sampleTime)
mazdo25 5:f1613df66ceb 14 {
mazdo25 5:f1613df66ceb 15 PrevErr = 0;
mazdo25 5:f1613df66ceb 16 PrevPrevErr = 0;
mazdo25 5:f1613df66ceb 17 prevControlAction = 0;
mazdo25 5:f1613df66ceb 18 setPoint = 0;
mazdo25 5:f1613df66ceb 19
mazdo25 5:f1613df66ceb 20 inMin_ = -1.0f;
mazdo25 5:f1613df66ceb 21 inMax_ = 1.0f;
mazdo25 5:f1613df66ceb 22
mazdo25 5:f1613df66ceb 23 Ts = sampleTime;
mazdo25 5:f1613df66ceb 24 Kp_ = Kp;
mazdo25 5:f1613df66ceb 25 Ki_ = Ki;
mazdo25 5:f1613df66ceb 26 Kd_ = Kd;
mazdo25 5:f1613df66ceb 27 }
mazdo25 5:f1613df66ceb 28
mazdo25 5:f1613df66ceb 29 float compute (float currVal)
mazdo25 5:f1613df66ceb 30 {
mazdo25 5:f1613df66ceb 31 float currErr = setPoint - currVal;
mazdo25 5:f1613df66ceb 32 float controlAction;
mazdo25 5:f1613df66ceb 33
mazdo25 5:f1613df66ceb 34 if (currVal > inMax_) {inMax_ = currVal;}
mazdo25 5:f1613df66ceb 35 if (currVal < inMin_) {inMin_ = currVal;}
mazdo25 5:f1613df66ceb 36
mazdo25 5:f1613df66ceb 37 controlAction = prevControlAction - (PrevErr*Kp_) + (Kp_*currErr)+ (Ki_*Ts*currErr) + ((Kd_/Ts)*(currErr - PrevErr - PrevErr + PrevPrevErr));
mazdo25 5:f1613df66ceb 38
mazdo25 5:f1613df66ceb 39 prevControlAction = controlAction;
mazdo25 5:f1613df66ceb 40 PrevPrevErr = PrevErr;
mazdo25 5:f1613df66ceb 41 PrevErr = currErr;
mazdo25 5:f1613df66ceb 42 //scale the control Action to the correct output limits and output
mazdo25 6:477382219bcf 43 controlAction = ((((controlAction)- inMin_)/(inMax_ - inMin_)) * (*outMax_ - *outMin_)) + *outMin_;
mazdo25 6:477382219bcf 44
mazdo25 6:477382219bcf 45 if (controlAction > *outMax_) {return *outMax_;} else if (controlAction < *outMin_) {return *outMin_;} else {return controlAction;}
mazdo25 6:477382219bcf 46
mazdo25 5:f1613df66ceb 47 }
mazdo25 5:f1613df66ceb 48
mazdo25 5:f1613df66ceb 49 void setSetPoint(float _sP)
mazdo25 5:f1613df66ceb 50 {
mazdo25 5:f1613df66ceb 51 setPoint = _sP;
mazdo25 5:f1613df66ceb 52 }
mazdo25 5:f1613df66ceb 53
mazdo25 5:f1613df66ceb 54 void setInputLimits(float inMin,float inMax)
mazdo25 5:f1613df66ceb 55 {
mazdo25 5:f1613df66ceb 56 if (inMin > inMax) {return;}
mazdo25 5:f1613df66ceb 57
mazdo25 6:477382219bcf 58 PrevErr = scaler(inMax_,inMin_,inMin,inMax,PrevErr);
mazdo25 6:477382219bcf 59 PrevPrevErr = scaler(inMax_,inMin_,inMin,inMax,PrevPrevErr);
mazdo25 6:477382219bcf 60 prevControlAction = scaler(inMax_,inMin_,inMin,inMax,prevControlAction);
mazdo25 6:477382219bcf 61
mazdo25 5:f1613df66ceb 62 inMin_ = inMin;
mazdo25 5:f1613df66ceb 63 inMax_ = inMax;
mazdo25 5:f1613df66ceb 64 }
mazdo25 5:f1613df66ceb 65
mazdo25 5:f1613df66ceb 66 void assignLimitAddress(float *outMax, float *outMin)
mazdo25 5:f1613df66ceb 67 {
mazdo25 5:f1613df66ceb 68 outMin_ = outMax;
mazdo25 5:f1613df66ceb 69 outMax_ = outMin;
mazdo25 5:f1613df66ceb 70 }
mazdo25 5:f1613df66ceb 71
mazdo25 5:f1613df66ceb 72 float scaler(float prevMin, float prevMax, float newMin, float newMax, float var)
mazdo25 5:f1613df66ceb 73 {
mazdo25 5:f1613df66ceb 74 if (var > prevMax) {var = prevMax;}
mazdo25 5:f1613df66ceb 75 if (var < prevMin) {var = prevMin;}
mazdo25 5:f1613df66ceb 76 return (((var-prevMin)/(prevMax - prevMin))*(newMax-newMin))+newMin;
mazdo25 5:f1613df66ceb 77 }
mazdo25 5:f1613df66ceb 78
mazdo25 5:f1613df66ceb 79 float returnPrevCA()
mazdo25 5:f1613df66ceb 80 {
mazdo25 5:f1613df66ceb 81 return prevControlAction;
mazdo25 5:f1613df66ceb 82 }
mazdo25 5:f1613df66ceb 83
mazdo25 6:477382219bcf 84 float returnOutMax()
mazdo25 6:477382219bcf 85 {
mazdo25 6:477382219bcf 86 return *outMax_;
mazdo25 6:477382219bcf 87 }
mazdo25 6:477382219bcf 88
mazdo25 5:f1613df66ceb 89 };
mazdo25 5:f1613df66ceb 90
mazdo25 5:f1613df66ceb 91 //(((temp-inMin)/(inMax - inMin))*(outMax-outMin))+outMin