PID2.h

00001 class PID2
00002 {
00003     private:
00004     float Kp_, Ki_, Kd_, Ts, PrevErr, PrevPrevErr, prevControlAction, setPoint;
00005     
00006     float inMin_;
00007     float inMax_;
00008     
00009     float* outMin_;
00010     float* outMax_;
00011     
00012     public:
00013     PID2 (float Kp, float Ki, float Kd, float sampleTime)
00014     {
00015         PrevErr = 0;
00016         PrevPrevErr = 0;
00017         prevControlAction = 0;
00018         setPoint = 0;
00019         
00020         inMin_ = -1.0f;
00021         inMax_ = 1.0f;
00022         
00023         Ts = sampleTime;
00024         Kp_ = Kp;
00025         Ki_ = Ki;
00026         Kd_ = Kd;
00027     }
00028     
00029     float compute (float currVal)
00030     {
00031     float currErr = currVal - setPoint;
00032     float controlAction;
00033     
00034     if (currVal > inMax_) {inMax_ = currVal;}
00035     if (currVal < inMin_) {inMin_ = currVal;}
00036     
00037     controlAction = prevControlAction - (PrevErr*Kp_) + (Kp_*currErr)+ (Ki_*Ts*currErr) + ((Kd_/Ts)*(currErr - PrevErr - PrevErr + PrevPrevErr));
00038    
00039     prevControlAction = controlAction;
00040     PrevPrevErr = PrevErr;
00041     PrevErr = currErr;
00042     //scale the control Action to the correct output limits and output
00043     controlAction = ((((controlAction)- inMin_)/(inMax_ - inMin_)) * (*outMax_ - *outMin_)) + *outMin_;
00044     
00045     if (controlAction > *outMax_) {return *outMax_;} else if (controlAction < *outMin_) {return *outMin_;} else {return controlAction;}
00046     
00047     }
00048     
00049     void setSetPoint(float _sP)
00050     {
00051         setPoint = _sP;
00052     }
00053     
00054     void setInputLimits(float inMin,float inMax)
00055     {
00056         if (inMin > inMax) {return;}
00057         
00058         PrevErr = scaler(inMax_,inMin_,inMin,inMax,PrevErr);
00059         PrevPrevErr = scaler(inMax_,inMin_,inMin,inMax,PrevPrevErr);
00060         prevControlAction = scaler(inMax_,inMin_,inMin,inMax,prevControlAction);
00061         
00062         inMin_ = inMin;
00063         inMax_ = inMax;
00064     }
00065     
00066     void assignLimitAddress(float *outMax, float *outMin)
00067     {
00068         outMin_ = outMin; 
00069         outMax_ = outMax;
00070     }
00071     
00072     float scaler(float prevMin, float prevMax, float newMin, float newMax, float var)
00073     {
00074         if (var > prevMax) {var = prevMax;}
00075         if (var < prevMin) {var = prevMin;}
00076         return (((var-prevMin)/(prevMax - prevMin))*(newMax-newMin))+newMin;
00077     }
00078     
00079     float returnPrevCA()
00080     {
00081         return   prevControlAction;
00082     }
00083     
00084     float returnOutMax()
00085     {
00086         return *outMax_;
00087     }
00088     
00089 };
00090 
00091 //(((temp-inMin)/(inMax - inMin))*(outMax-outMin))+outMin