ロボステ6期 / Mbed 2 deprecated ball_snatch

e

Dependencies:   mbed Encoder_

SpeedController/SpeedController.cpp@0:a40701dd7f26, 2019-12-13 (annotated)

Committer:
aoikoizumi
Date:
Fri Dec 13 06:49:35 2019 +0000
Revision:
0:a40701dd7f26
e

Who changed what in which revision?

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