Dependencies: Encoder
Dependents: NHK2020-m2-2-ros_2_25
SpeedController.cpp@0:de44976b7670, 2019-07-24 (annotated)
- Committer:
- MazeTaka
- Date:
- Wed Jul 24 17:11:01 2019 +0000
- Revision:
- 0:de44976b7670
- Child:
- 1:735c0a48b445
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| MazeTaka | 0:de44976b7670 | 1 | #include "mbed.h" |
| MazeTaka | 0:de44976b7670 | 2 | #include "EC.h" |
| MazeTaka | 0:de44976b7670 | 3 | #include "SpeedController.h" |
| MazeTaka | 0:de44976b7670 | 4 | |
| MazeTaka | 0:de44976b7670 | 5 | SpeedControl::SpeedControl(PinName pwm_F,PinName pwm_B,int us,Ec &ec) : |
| MazeTaka | 0:de44976b7670 | 6 | Kp_(0),Kd_(0),pre_diff(0),ptsc_(0),Cf_(1/45),Cb_(-1/45),Df_(0),Db_(0),initial_Df_(0),initial_Db_(0),duty_limit_(0.95),pwm_F_(pwm_F),pwm_B_(pwm_B) |
| MazeTaka | 0:de44976b7670 | 7 | { |
| MazeTaka | 0:de44976b7670 | 8 | ec_=&ec; |
| MazeTaka | 0:de44976b7670 | 9 | pwm_F_.period_us(us); |
| MazeTaka | 0:de44976b7670 | 10 | pwm_B_.period_us(us); |
| MazeTaka | 0:de44976b7670 | 11 | } |
| MazeTaka | 0:de44976b7670 | 12 | void SpeedControl::period(double s) |
| MazeTaka | 0:de44976b7670 | 13 | { |
| MazeTaka | 0:de44976b7670 | 14 | pwm_F_.period(s); |
| MazeTaka | 0:de44976b7670 | 15 | pwm_B_.period(s); |
| MazeTaka | 0:de44976b7670 | 16 | } |
| MazeTaka | 0:de44976b7670 | 17 | void SpeedControl::period_ms(int ms) |
| MazeTaka | 0:de44976b7670 | 18 | { |
| MazeTaka | 0:de44976b7670 | 19 | pwm_F_.period_ms(ms); |
| MazeTaka | 0:de44976b7670 | 20 | pwm_B_.period_ms(ms); |
| MazeTaka | 0:de44976b7670 | 21 | } |
| MazeTaka | 0:de44976b7670 | 22 | void SpeedControl::period_us(int us) |
| MazeTaka | 0:de44976b7670 | 23 | { |
| MazeTaka | 0:de44976b7670 | 24 | pwm_F_.period_us(us); |
| MazeTaka | 0:de44976b7670 | 25 | pwm_B_.period_us(us); |
| MazeTaka | 0:de44976b7670 | 26 | } |
| MazeTaka | 0:de44976b7670 | 27 | |
| MazeTaka | 0:de44976b7670 | 28 | |
| MazeTaka | 0:de44976b7670 | 29 | void SpeedControl::Sc(double target_omega) //スカンジウムじゃないよ |
| MazeTaka | 0:de44976b7670 | 30 | { |
| MazeTaka | 0:de44976b7670 | 31 | double t=ec_->timer_.read(); |
| MazeTaka | 0:de44976b7670 | 32 | ec_->calOmega(); |
| MazeTaka | 0:de44976b7670 | 33 | double omega=ec_->getOmega(); |
| MazeTaka | 0:de44976b7670 | 34 | double diff=target_omega-omega; |
| MazeTaka | 0:de44976b7670 | 35 | double pid=Kp_*diff+Kd_*(diff-pre_diff)/(t-ptsc_); |
| MazeTaka | 0:de44976b7670 | 36 | double duty; |
| MazeTaka | 0:de44976b7670 | 37 | if(target_omega>0) { |
| MazeTaka | 0:de44976b7670 | 38 | Df_+=Cf_*pid; |
| MazeTaka | 0:de44976b7670 | 39 | duty=Cf_*target_omega+Df_; |
| MazeTaka | 0:de44976b7670 | 40 | } else if(target_omega<0) { |
| MazeTaka | 0:de44976b7670 | 41 | Db_+=Cb_*pid; |
| MazeTaka | 0:de44976b7670 | 42 | duty=Cb_*target_omega+Db_; |
| MazeTaka | 0:de44976b7670 | 43 | } else duty=0; |
| MazeTaka | 0:de44976b7670 | 44 | if(duty<-duty_limit_)duty=-duty_limit_; |
| MazeTaka | 0:de44976b7670 | 45 | else if(duty>duty_limit_)duty=duty_limit_; |
| MazeTaka | 0:de44976b7670 | 46 | turn(duty); |
| MazeTaka | 0:de44976b7670 | 47 | pre_diff=diff; |
| MazeTaka | 0:de44976b7670 | 48 | ptsc_=t; |
| MazeTaka | 0:de44976b7670 | 49 | //printf("tw=%.2f,w=%.2f,diff=%.2f,pid=%.2f,Df=%.2f,Db=%.2f,duty=%.4f\r\n",target_omega,omega,diff,pid,Df_,Db_,duty); |
| MazeTaka | 0:de44976b7670 | 50 | } |
| MazeTaka | 0:de44976b7670 | 51 | |
| MazeTaka | 0:de44976b7670 | 52 | void SpeedControl::turn(double duty) |
| MazeTaka | 0:de44976b7670 | 53 | { |
| MazeTaka | 0:de44976b7670 | 54 | if(duty>duty_limit_)duty=duty_limit_; |
| MazeTaka | 0:de44976b7670 | 55 | else if(duty<-duty_limit_)duty=-duty_limit_; |
| MazeTaka | 0:de44976b7670 | 56 | |
| MazeTaka | 0:de44976b7670 | 57 | if(duty>=0) { |
| MazeTaka | 0:de44976b7670 | 58 | pwm_F_=duty; |
| MazeTaka | 0:de44976b7670 | 59 | pwm_B_=0; |
| MazeTaka | 0:de44976b7670 | 60 | } else if(duty<0) { |
| MazeTaka | 0:de44976b7670 | 61 | pwm_F_=0; |
| MazeTaka | 0:de44976b7670 | 62 | pwm_B_=-duty; |
| MazeTaka | 0:de44976b7670 | 63 | } |
| MazeTaka | 0:de44976b7670 | 64 | } |
| MazeTaka | 0:de44976b7670 | 65 | |
| MazeTaka | 0:de44976b7670 | 66 | void SpeedControl::setPDparam(double kp,double kd) |
| MazeTaka | 0:de44976b7670 | 67 | { |
| MazeTaka | 0:de44976b7670 | 68 | Kp_=kp; |
| MazeTaka | 0:de44976b7670 | 69 | Kd_=kd; |
| MazeTaka | 0:de44976b7670 | 70 | } |
| MazeTaka | 0:de44976b7670 | 71 | |
| MazeTaka | 0:de44976b7670 | 72 | |
| MazeTaka | 0:de44976b7670 | 73 | void SpeedControl::setEquation(double cf,double df,double cb,double db) |
| MazeTaka | 0:de44976b7670 | 74 | { |
| MazeTaka | 0:de44976b7670 | 75 | Cf_=cf; |
| MazeTaka | 0:de44976b7670 | 76 | if(cb>0)Cb_=cb; |
| MazeTaka | 0:de44976b7670 | 77 | else Cb_=-cb; |
| MazeTaka | 0:de44976b7670 | 78 | initial_Df_=df; |
| MazeTaka | 0:de44976b7670 | 79 | initial_Db_=-db; |
| MazeTaka | 0:de44976b7670 | 80 | Df_=initial_Df_; |
| MazeTaka | 0:de44976b7670 | 81 | Db_=initial_Db_; |
| MazeTaka | 0:de44976b7670 | 82 | } |
| MazeTaka | 0:de44976b7670 | 83 | void SpeedControl::setDutyLimit(double duty_limit) |
| MazeTaka | 0:de44976b7670 | 84 | { |
| MazeTaka | 0:de44976b7670 | 85 | if(0<=duty_limit && duty_limit<=0.95f)duty_limit_=duty_limit; |
| MazeTaka | 0:de44976b7670 | 86 | else duty_limit_=0.95f; |
| MazeTaka | 0:de44976b7670 | 87 | } |
| MazeTaka | 0:de44976b7670 | 88 | |
| MazeTaka | 0:de44976b7670 | 89 | void SpeedControl::reset() |
| MazeTaka | 0:de44976b7670 | 90 | { |
| MazeTaka | 0:de44976b7670 | 91 | ec_->reset(); |
| MazeTaka | 0:de44976b7670 | 92 | pre_diff=0; |
| MazeTaka | 0:de44976b7670 | 93 | ptsc_=0; |
| MazeTaka | 0:de44976b7670 | 94 | Df_=initial_Df_; |
| MazeTaka | 0:de44976b7670 | 95 | Db_=initial_Db_; |
| MazeTaka | 0:de44976b7670 | 96 | } |
| MazeTaka | 0:de44976b7670 | 97 | |
| MazeTaka | 0:de44976b7670 | 98 | void SpeedControl::stop() |
| MazeTaka | 0:de44976b7670 | 99 | { |
| MazeTaka | 0:de44976b7670 | 100 | pwm_F_=0; |
| MazeTaka | 0:de44976b7670 | 101 | pwm_B_=0; |
| MazeTaka | 0:de44976b7670 | 102 | } |