nucho
SimplePID.h@0:3c49891bc39d, 2011-07-29 (annotated)
- Committer:
- nucho
- Date:
- Fri Jul 29 11:23:44 2011 +0000
- Revision:
- 0:3c49891bc39d
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| nucho | 0:3c49891bc39d | 1 | /************************** |
| nucho | 0:3c49891bc39d | 2 | This class referred to following URL. |
| nucho | 0:3c49891bc39d | 3 | http://monoist.atmarkit.co.jp/mn/articles/1007/26/news083.html |
| nucho | 0:3c49891bc39d | 4 | **************************/ |
| nucho | 0:3c49891bc39d | 5 | |
| nucho | 0:3c49891bc39d | 6 | class SimplePID { |
| nucho | 0:3c49891bc39d | 7 | public: |
| nucho | 0:3c49891bc39d | 8 | SimplePID(double P,double I,double D,double rate) { |
| nucho | 0:3c49891bc39d | 9 | _gainP=P; |
| nucho | 0:3c49891bc39d | 10 | _gainI=I; |
| nucho | 0:3c49891bc39d | 11 | _gainD=D; |
| nucho | 0:3c49891bc39d | 12 | _rate = rate; |
| nucho | 0:3c49891bc39d | 13 | |
| nucho | 0:3c49891bc39d | 14 | _diff_new=0; |
| nucho | 0:3c49891bc39d | 15 | _diff_old=0; |
| nucho | 0:3c49891bc39d | 16 | } |
| nucho | 0:3c49891bc39d | 17 | |
| nucho | 0:3c49891bc39d | 18 | void setGoal(int goal) |
| nucho | 0:3c49891bc39d | 19 | { |
| nucho | 0:3c49891bc39d | 20 | _goal = goal; |
| nucho | 0:3c49891bc39d | 21 | } |
| nucho | 0:3c49891bc39d | 22 | |
| nucho | 0:3c49891bc39d | 23 | double compute(int current) { |
| nucho | 0:3c49891bc39d | 24 | double p,i,d; |
| nucho | 0:3c49891bc39d | 25 | |
| nucho | 0:3c49891bc39d | 26 | _diff_old=_diff_new; |
| nucho | 0:3c49891bc39d | 27 | _diff_new=current-_goal; |
| nucho | 0:3c49891bc39d | 28 | |
| nucho | 0:3c49891bc39d | 29 | _integral += (_diff_old + _diff_new)/2.0 * _rate; |
| nucho | 0:3c49891bc39d | 30 | |
| nucho | 0:3c49891bc39d | 31 | p= _gainP*_diff_new; |
| nucho | 0:3c49891bc39d | 32 | i= _gainI*_integral; |
| nucho | 0:3c49891bc39d | 33 | d= _gainD*(_diff_new - _diff_old) *_rate; |
| nucho | 0:3c49891bc39d | 34 | |
| nucho | 0:3c49891bc39d | 35 | return math_limit(p+i+d,_min,_max); |
| nucho | 0:3c49891bc39d | 36 | } |
| nucho | 0:3c49891bc39d | 37 | |
| nucho | 0:3c49891bc39d | 38 | void setLimits(int min,int max){ |
| nucho | 0:3c49891bc39d | 39 | _min = min; |
| nucho | 0:3c49891bc39d | 40 | _max = max; |
| nucho | 0:3c49891bc39d | 41 | } |
| nucho | 0:3c49891bc39d | 42 | |
| nucho | 0:3c49891bc39d | 43 | |
| nucho | 0:3c49891bc39d | 44 | private: |
| nucho | 0:3c49891bc39d | 45 | int _diff_new,_diff_old,_goal; |
| nucho | 0:3c49891bc39d | 46 | double _integral,_rate; |
| nucho | 0:3c49891bc39d | 47 | |
| nucho | 0:3c49891bc39d | 48 | double _gainP,_gainI,_gainD; |
| nucho | 0:3c49891bc39d | 49 | int _min,_max; |
| nucho | 0:3c49891bc39d | 50 | |
| nucho | 0:3c49891bc39d | 51 | int math_limit(double ctrl,double min,double max) { |
| nucho | 0:3c49891bc39d | 52 | int ret=ctrl; |
| nucho | 0:3c49891bc39d | 53 | if(ctrl < min) ret=min; |
| nucho | 0:3c49891bc39d | 54 | if(ctrl >max) ret=max; |
| nucho | 0:3c49891bc39d | 55 | |
| nucho | 0:3c49891bc39d | 56 | return ret; |
| nucho | 0:3c49891bc39d | 57 | } |
| nucho | 0:3c49891bc39d | 58 | }; |