PID.h

00001 class PID
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     PID (float Kp, float Ki, float Kd, float sampleTime)
00014     {
00015         PrevErr = 0;
00016         PrevPrevErr = 0;
00017         prevControlAction = 0;
00018         setPoint = 0;
00019         
00020         outMin_ = -1.0f;
00021         outMax_ = 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 = setPoint - currVal;
00032     float controlAction;
00033     
00034     //if (currVal > *inMax_) {*inMax_ = currVal; *inMin_ = -1.0f*currVal;}
00035     //if (currVal < *inMin_) {*inMin_ = currVal; *inMax_ = -1.0f*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 setOutputLimits(float outMin,float outMax)
00055     {
00056         if (outMin > outMax) {return;}
00057         
00058         outMin_ = outMin;
00059         outMax_ = outMax;
00060     }
00061     
00062     void assignLimitAddress(float *inMax, float *inMin)
00063     {
00064         inMax_ = inMax; 
00065         inMin_ = inMin;
00066     }
00067     
00068     float returnInMax() 
00069     {
00070         return  *inMax_;
00071     }
00072     
00073     float scaler(float prevMin, float prevMax, float newMin, float newMax, float var)
00074     {
00075         if (var > prevMax) {var = prevMax;}
00076         if (var < prevMin) {var = prevMin;}
00077         return (((var-prevMin)/(prevMax - prevMin))*(newMax-newMin))+newMin;
00078     }
00079     
00080     float returnPrevCA()
00081     {
00082         return   prevControlAction;
00083     }
00084     
00085 };
00086 
00087 //(((temp-inMin)/(inMax - inMin))*(outMax-outMin))+outMin