SimplePID.h

00001 /**************************
00002 This class referred to following URL. 
00003 http://monoist.atmarkit.co.jp/mn/articles/1007/26/news083.html
00004 **************************/
00005 
00006 class SimplePID {
00007 public:
00008     SimplePID(double P,double I,double D,double rate) {
00009         _gainP=P;
00010         _gainI=I;
00011         _gainD=D;
00012         _rate = rate;
00013 
00014         _diff_new=0;
00015         _diff_old=0;
00016     }
00017     
00018     void setGoal(int goal)
00019     {
00020         _goal = goal;
00021     }
00022 
00023     double compute(int current) {
00024        double p,i,d;
00025        
00026        _diff_old=_diff_new;
00027        _diff_new=current-_goal;
00028        
00029        _integral += (_diff_old + _diff_new)/2.0 * _rate;
00030        
00031        p= _gainP*_diff_new;
00032        i= _gainI*_integral;
00033        d= _gainD*(_diff_new - _diff_old) *_rate;
00034        
00035        return math_limit(p+i+d,_min,_max);
00036     }
00037     
00038     void setLimits(int min,int max){
00039         _min = min;
00040         _max = max;
00041     }
00042     
00043     
00044 private:
00045     int _diff_new,_diff_old,_goal;
00046     double _integral,_rate;
00047 
00048     double _gainP,_gainI,_gainD;
00049     int _min,_max;
00050 
00051     int math_limit(double ctrl,double min,double max) {
00052         int ret=ctrl;
00053         if(ctrl < min)  ret=min;
00054         if(ctrl >max) ret=max;
00055         
00056         return ret;
00057     }
00058 };