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