File content as of revision 1:7f0fc0d1f777:

/**************************
This class referred to following URL. 
http://monoist.atmarkit.co.jp/mn/articles/1007/26/news083.html
**************************/

class SimplePID {
public:
    SimplePID(double P,double I,double D,double rate) {
        _gainP=P;
        _gainI=I;
        _gainD=D;
        _rate = rate;

        _diff_new=0;
        _diff_old=0;
    }
    
    void setGoal(int goal)
    {
        _goal = goal;
    }

    double compute(int current) {
       double p,i,d;
       
       _diff_old=_diff_new;
       _diff_new=current-_goal;
       
       _integral += (_diff_old + _diff_new)/2.0 * _rate;
       
       p= _gainP*_diff_new;
       i= _gainI*_integral;
       d= _gainD*(_diff_new - _diff_old) *_rate;
       
       return math_limit(p+i+d,_min,_max);
    }
    
    void setLimits(int min,int max){
        _min = min;
        _max = max;
    }
    
    
private:
    int _diff_new,_diff_old,_goal;
    double _integral,_rate;

    double _gainP,_gainI,_gainD;
    int _min,_max;

    int math_limit(double ctrl,double min,double max) {
        int ret=ctrl;
        if(ctrl < min)  ret=min;
        if(ctrl >max) ret=max;
        
        return ret;
    }
};