ロボステ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@9:32f31b0f5815, 2019-11-14 (annotated)

Committer:
maxnagazumi
Date:
Thu Nov 14 07:22:09 2019 +0000
Revision:
9:32f31b0f5815
Parent:
6:d110c276aa5a 
new sicker ver

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;
maxnagazumi 9:32f31b0f5815 47 printf("%f ",duty);
MazeTaka 0:de44976b7670 48 }
MazeTaka 0:de44976b7670 49
MazeTaka 0:de44976b7670 50 void SpeedControl::turn(double duty)
MazeTaka 0:de44976b7670 51 {
MazeTaka 0:de44976b7670 52 if(duty>duty_limit_)duty=duty_limit_;
MazeTaka 0:de44976b7670 53 else if(duty<-duty_limit_)duty=-duty_limit_;
MazeTaka 0:de44976b7670 54
MazeTaka 0:de44976b7670 55 if(duty>=0) {
MazeTaka 0:de44976b7670 56 pwm_F_=duty;
MazeTaka 0:de44976b7670 57 pwm_B_=0;
MazeTaka 0:de44976b7670 58 } else if(duty<0) {
MazeTaka 0:de44976b7670 59 pwm_F_=0;
MazeTaka 0:de44976b7670 60 pwm_B_=-duty;
MazeTaka 0:de44976b7670 61 }
MazeTaka 0:de44976b7670 62 }
MazeTaka 0:de44976b7670 63
MazeTaka 0:de44976b7670 64 void SpeedControl::setPDparam(double kp,double kd)
MazeTaka 0:de44976b7670 65 {
MazeTaka 0:de44976b7670 66 Kp_=kp;
MazeTaka 0:de44976b7670 67 Kd_=kd;
MazeTaka 0:de44976b7670 68 }
MazeTaka 0:de44976b7670 69
MazeTaka 0:de44976b7670 70
MazeTaka 0:de44976b7670 71 void SpeedControl::setEquation(double cf,double df,double cb,double db)
MazeTaka 0:de44976b7670 72 {
MazeTaka 0:de44976b7670 73 Cf_=cf;
MazeTaka 0:de44976b7670 74 if(cb>0)Cb_=cb;
MazeTaka 0:de44976b7670 75 else Cb_=-cb;
MazeTaka 0:de44976b7670 76 initial_Df_=df;
MazeTaka 0:de44976b7670 77 initial_Db_=-db;
MazeTaka 0:de44976b7670 78 Df_=initial_Df_;
MazeTaka 0:de44976b7670 79 Db_=initial_Db_;
MazeTaka 0:de44976b7670 80 }
MazeTaka 0:de44976b7670 81 void SpeedControl::setDutyLimit(double duty_limit)
MazeTaka 0:de44976b7670 82 {
MazeTaka 6:d110c276aa5a 83 if(0<=duty_limit && duty_limit<0.95f)duty_limit_=duty_limit;
MazeTaka 0:de44976b7670 84 else duty_limit_=0.95f;
MazeTaka 0:de44976b7670 85 }
MazeTaka 0:de44976b7670 86
MazeTaka 0:de44976b7670 87 void SpeedControl::reset()
MazeTaka 0:de44976b7670 88 {
MazeTaka 0:de44976b7670 89 ec_->reset();
MazeTaka 0:de44976b7670 90 pre_diff=0;
MazeTaka 0:de44976b7670 91 ptsc_=0;
MazeTaka 0:de44976b7670 92 Df_=initial_Df_;
MazeTaka 0:de44976b7670 93 Db_=initial_Db_;
MazeTaka 0:de44976b7670 94 }
MazeTaka 0:de44976b7670 95
MazeTaka 0:de44976b7670 96 void SpeedControl::stop()
MazeTaka 0:de44976b7670 97 {
MazeTaka 0:de44976b7670 98 pwm_F_=0;
MazeTaka 0:de44976b7670 99 pwm_B_=0;
MazeTaka 0:de44976b7670 100 }