nucho
/
RTno_MotorControl
SimplePID.h
- Committer:
- nucho
- Date:
- 2011-07-29
- Revision:
- 0:3c49891bc39d
File content as of revision 0:3c49891bc39d:
/************************** 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; } };