nucho
/
RTno_MotorControl
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