nucho
/
RTno_MotorControl
SimplePID.h@1:7f0fc0d1f777, 2011-08-01 (annotated)
- Committer:
- nucho
- Date:
- Mon Aug 01 14:46:08 2011 +0000
- Revision:
- 1:7f0fc0d1f777
- Parent:
- 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 | }; |