ロボステ6期 / Mbed 2 deprecated NHK2020-arm-sub2

12/18

Dependencies:   mbed Encoder_

SpeedController/SpeedController.cpp@1:bc34fdc4e16b, 2019-12-18 (annotated)

Committer:
aoikoizumi
Date:
Wed Dec 18 02:33:45 2019 +0000
Revision:
1:bc34fdc4e16b
Parent:
0:6e2abd0956f1 
f

Who changed what in which revision?

UserRevisionLine numberNew 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 }