ロボステ6期 / SpeedController___2_25

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Encoder

Dependents:   NHK2020-m2-2-ros_2_25

SpeedController.cpp@10:daf0e763bf35, 2019-11-14 (annotated)

Committer:
MazeTaka
Date:
Thu Nov 14 07:58:38 2019 +0000
Revision:
10:daf0e763bf35
Parent:
6:d110c276aa5a 
Add the function to get Duty ratio

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
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 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 10:daf0e763bf35 62 double SpeedControl::getDutyF(){
MazeTaka 10:daf0e763bf35 63 return pwm_F_.read();
MazeTaka 10:daf0e763bf35 64 }
MazeTaka 10:daf0e763bf35 65
MazeTaka 10:daf0e763bf35 66 double SpeedControl::getDutyB(){
MazeTaka 10:daf0e763bf35 67 return pwm_B_.read();
MazeTaka 10:daf0e763bf35 68 }
MazeTaka 0:de44976b7670 69
MazeTaka 0:de44976b7670 70 void SpeedControl::setPDparam(double kp,double kd)
MazeTaka 0:de44976b7670 71 {
MazeTaka 0:de44976b7670 72 Kp_=kp;
MazeTaka 0:de44976b7670 73 Kd_=kd;
MazeTaka 0:de44976b7670 74 }
MazeTaka 0:de44976b7670 75
MazeTaka 0:de44976b7670 76
MazeTaka 0:de44976b7670 77 void SpeedControl::setEquation(double cf,double df,double cb,double db)
MazeTaka 0:de44976b7670 78 {
MazeTaka 0:de44976b7670 79 Cf_=cf;
MazeTaka 0:de44976b7670 80 if(cb>0)Cb_=cb;
MazeTaka 0:de44976b7670 81 else Cb_=-cb;
MazeTaka 0:de44976b7670 82 initial_Df_=df;
MazeTaka 0:de44976b7670 83 initial_Db_=-db;
MazeTaka 0:de44976b7670 84 Df_=initial_Df_;
MazeTaka 0:de44976b7670 85 Db_=initial_Db_;
MazeTaka 0:de44976b7670 86 }
MazeTaka 0:de44976b7670 87 void SpeedControl::setDutyLimit(double duty_limit)
MazeTaka 0:de44976b7670 88 {
MazeTaka 6:d110c276aa5a 89 if(0<=duty_limit && duty_limit<0.95f)duty_limit_=duty_limit;
MazeTaka 0:de44976b7670 90 else duty_limit_=0.95f;
MazeTaka 0:de44976b7670 91 }
MazeTaka 0:de44976b7670 92
MazeTaka 0:de44976b7670 93 void SpeedControl::reset()
MazeTaka 0:de44976b7670 94 {
MazeTaka 0:de44976b7670 95 ec_->reset();
MazeTaka 0:de44976b7670 96 pre_diff=0;
MazeTaka 0:de44976b7670 97 ptsc_=0;
MazeTaka 0:de44976b7670 98 Df_=initial_Df_;
MazeTaka 0:de44976b7670 99 Db_=initial_Db_;
MazeTaka 0:de44976b7670 100 }
MazeTaka 0:de44976b7670 101
MazeTaka 0:de44976b7670 102 void SpeedControl::stop()
MazeTaka 0:de44976b7670 103 {
MazeTaka 0:de44976b7670 104 pwm_F_=0;
MazeTaka 0:de44976b7670 105 pwm_B_=0;
MazeTaka 0:de44976b7670 106 }