Buggy bois / Mbed 2 deprecated headache

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

PID.h@4:208f5279143a, 2019-03-23 (annotated)

Committer:
mazdo25
Date:
Sat Mar 23 19:46:09 2019 +0000
Revision:
4:208f5279143a
Child:
5:f1613df66ceb
latest working, but line following not;

Who changed what in which revision?

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