Diff: SimplePID.h

Revision:

0:3c49891bc39d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SimplePID.h	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,58 @@
+/**************************
+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;
+    }
+};
\ No newline at end of file