ロボステ6期 / SpeedController___2_25

2月25日

Dependencies:   Encoder

Dependents:   NHK2020-m2-2-ros_2_25

SpeedController.cpp@11:b3e2ec75a4d7, 2020-02-25 (annotated)

Committer:
yuki0701
Date:
Tue Feb 25 01:24:31 2020 +0000
Revision:
11:b3e2ec75a4d7
Parent:
6:d110c276aa5a 
a

Who changed what in which revision?

UserRevisionLine numberNew 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 1:735c0a48b445 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
yuki0701 11:b3e2ec75a4d7 29 void SpeedControl::Sc(double target_omega,int flag,double omega) //スカンジウムじゃないよ
MazeTaka 0:de44976b7670 30 {
MazeTaka 0:de44976b7670 31 double t=ec_->timer_.read();
yuki0701 11:b3e2ec75a4d7 32 //ec_->calOmega();
yuki0701 11:b3e2ec75a4d7 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 turn(duty);
MazeTaka 0:de44976b7670 45 pre_diff=diff;
MazeTaka 0:de44976b7670 46 ptsc_=t;
MazeTaka 0:de44976b7670 47 }
MazeTaka 0:de44976b7670 48
MazeTaka 0:de44976b7670 49 void SpeedControl::turn(double duty)
MazeTaka 0:de44976b7670 50 {
MazeTaka 0:de44976b7670 51 if(duty>duty_limit_)duty=duty_limit_;
MazeTaka 0:de44976b7670 52 else if(duty<-duty_limit_)duty=-duty_limit_;
MazeTaka 0:de44976b7670 53
MazeTaka 0:de44976b7670 54 if(duty>=0) {
MazeTaka 0:de44976b7670 55 pwm_F_=duty;
MazeTaka 0:de44976b7670 56 pwm_B_=0;
MazeTaka 0:de44976b7670 57 } else if(duty<0) {
MazeTaka 0:de44976b7670 58 pwm_F_=0;
MazeTaka 0:de44976b7670 59 pwm_B_=-duty;
MazeTaka 0:de44976b7670 60 }
MazeTaka 0:de44976b7670 61 }
MazeTaka 0:de44976b7670 62
MazeTaka 0:de44976b7670 63 void SpeedControl::setPDparam(double kp,double kd)
MazeTaka 0:de44976b7670 64 {
MazeTaka 0:de44976b7670 65 Kp_=kp;
MazeTaka 0:de44976b7670 66 Kd_=kd;
MazeTaka 0:de44976b7670 67 }
MazeTaka 0:de44976b7670 68
MazeTaka 0:de44976b7670 69
MazeTaka 0:de44976b7670 70 void SpeedControl::setEquation(double cf,double df,double cb,double db)
MazeTaka 0:de44976b7670 71 {
MazeTaka 0:de44976b7670 72 Cf_=cf;
MazeTaka 0:de44976b7670 73 if(cb>0)Cb_=cb;
MazeTaka 0:de44976b7670 74 else Cb_=-cb;
MazeTaka 0:de44976b7670 75 initial_Df_=df;
MazeTaka 0:de44976b7670 76 initial_Db_=-db;
MazeTaka 0:de44976b7670 77 Df_=initial_Df_;
MazeTaka 0:de44976b7670 78 Db_=initial_Db_;
MazeTaka 0:de44976b7670 79 }
MazeTaka 0:de44976b7670 80 void SpeedControl::setDutyLimit(double duty_limit)
MazeTaka 0:de44976b7670 81 {
MazeTaka 6:d110c276aa5a 82 if(0<=duty_limit && duty_limit<0.95f)duty_limit_=duty_limit;
MazeTaka 0:de44976b7670 83 else duty_limit_=0.95f;
MazeTaka 0:de44976b7670 84 }
MazeTaka 0:de44976b7670 85
MazeTaka 0:de44976b7670 86 void SpeedControl::reset()
MazeTaka 0:de44976b7670 87 {
MazeTaka 0:de44976b7670 88 ec_->reset();
MazeTaka 0:de44976b7670 89 pre_diff=0;
MazeTaka 0:de44976b7670 90 ptsc_=0;
MazeTaka 0:de44976b7670 91 Df_=initial_Df_;
MazeTaka 0:de44976b7670 92 Db_=initial_Db_;
MazeTaka 0:de44976b7670 93 }
MazeTaka 0:de44976b7670 94
MazeTaka 0:de44976b7670 95 void SpeedControl::stop()
MazeTaka 0:de44976b7670 96 {
MazeTaka 0:de44976b7670 97 pwm_F_=0;
MazeTaka 0:de44976b7670 98 pwm_B_=0;
MazeTaka 0:de44976b7670 99 }