robostep8th / Mbed 2 deprecated SCforMatlab_ver3

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:   mbed Encoder

SCforMatlab.cpp@0:2ffc34248c08, 2021-02-18 (annotated)

Committer:
mrtkhmbed
Date:
Thu Feb 18 15:12:09 2021 +0000
Revision:
0:2ffc34248c08
SCforMatlab

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mrtkhmbed 0:2ffc34248c08 1 #include "mbed.h"
mrtkhmbed 0:2ffc34248c08 2 #include "EC.h"
mrtkhmbed 0:2ffc34248c08 3 #include "SCforMatlab.h"
mrtkhmbed 0:2ffc34248c08 4 SCforMatlab::SCforMatlab(PinName pwm_F,PinName pwm_B,float period_us,Ec &ec,float dt) :
mrtkhmbed 0:2ffc34248c08 5 forward(pwm_F),back(pwm_B)
mrtkhmbed 0:2ffc34248c08 6 {
mrtkhmbed 0:2ffc34248c08 7 ////////////////////////////////変数のデフォルト設定
mrtkhmbed 0:2ffc34248c08 8 ec_=&ec;
mrtkhmbed 0:2ffc34248c08 9 forward.period_us(period_us);
mrtkhmbed 0:2ffc34248c08 10 back.period_us(period_us);
mrtkhmbed 0:2ffc34248c08 11 delta_t=dt;
mrtkhmbed 0:2ffc34248c08 12 forward=0;
mrtkhmbed 0:2ffc34248c08 13 back=0;
mrtkhmbed 0:2ffc34248c08 14 duty=0;
mrtkhmbed 0:2ffc34248c08 15 start_cal=0.1;
mrtkhmbed 0:2ffc34248c08 16 end_cal=0.7;
mrtkhmbed 0:2ffc34248c08 17 measure_mode=MEASURE_CW;
mrtkhmbed 0:2ffc34248c08 18 duty_max=17;//最大dutyを0.85に
mrtkhmbed 0:2ffc34248c08 19 ec_->timer_.reset();
mrtkhmbed 0:2ffc34248c08 20 if(dt<=0.02) {//システム同定の測定データを十分に確保するため回転時間を調整
mrtkhmbed 0:2ffc34248c08 21 turn_time=0.5;
mrtkhmbed 0:2ffc34248c08 22 } else {
mrtkhmbed 0:2ffc34248c08 23 turn_time=1.0;
mrtkhmbed 0:2ffc34248c08 24 }
mrtkhmbed 0:2ffc34248c08 25 }
mrtkhmbed 0:2ffc34248c08 26 void SCforMatlab::saveOmega()
mrtkhmbed 0:2ffc34248c08 27 {
mrtkhmbed 0:2ffc34248c08 28 ec_->calOmega();
mrtkhmbed 0:2ffc34248c08 29 omega[save_count]=(ec_->getOmega())*GEER;
mrtkhmbed 0:2ffc34248c08 30 input_duty[save_count]=duty;
mrtkhmbed 0:2ffc34248c08 31 save_count++;
mrtkhmbed 0:2ffc34248c08 32 }
mrtkhmbed 0:2ffc34248c08 33 void SCforMatlab::motorTurn(float out)
mrtkhmbed 0:2ffc34248c08 34 {
mrtkhmbed 0:2ffc34248c08 35 if(out>0) {
mrtkhmbed 0:2ffc34248c08 36 forward=out;
mrtkhmbed 0:2ffc34248c08 37 back=0;
mrtkhmbed 0:2ffc34248c08 38 } else {
mrtkhmbed 0:2ffc34248c08 39 forward=0;
mrtkhmbed 0:2ffc34248c08 40 back=-out;
mrtkhmbed 0:2ffc34248c08 41 }
mrtkhmbed 0:2ffc34248c08 42 }
mrtkhmbed 0:2ffc34248c08 43 void SCforMatlab::measureTurn()
mrtkhmbed 0:2ffc34248c08 44 {
mrtkhmbed 0:2ffc34248c08 45 motorTurn(duty);
mrtkhmbed 0:2ffc34248c08 46 saveOmega();
mrtkhmbed 0:2ffc34248c08 47 }
mrtkhmbed 0:2ffc34248c08 48 void SCforMatlab::showOmega()
mrtkhmbed 0:2ffc34248c08 49 {
mrtkhmbed 0:2ffc34248c08 50 printf("\n\r\nMeasured value for Matlab\r\n");
mrtkhmbed 0:2ffc34248c08 51 printf("\n\r\ncopy all data\r\n");
mrtkhmbed 0:2ffc34248c08 52 printf("\nnum\tduty\t\tomega\r\n");
mrtkhmbed 0:2ffc34248c08 53 for(int i=0; i<save_count; i++) {
mrtkhmbed 0:2ffc34248c08 54 printf("%d\t%f\t%f\r\n",i,input_duty[i],omega[i]);
mrtkhmbed 0:2ffc34248c08 55 wait(0.01);
mrtkhmbed 0:2ffc34248c08 56 }
mrtkhmbed 0:2ffc34248c08 57 printf("\r\n");
mrtkhmbed 0:2ffc34248c08 58 }
mrtkhmbed 0:2ffc34248c08 59 void SCforMatlab::setCDRange(float start,float end)
mrtkhmbed 0:2ffc34248c08 60 {
mrtkhmbed 0:2ffc34248c08 61 int start_int=start/0.05; //0.05刻みで入力してもらえんかった場合の保険
mrtkhmbed 0:2ffc34248c08 62 int end_int=end/0.05;
mrtkhmbed 0:2ffc34248c08 63 start_cal=start_int*0.05;
mrtkhmbed 0:2ffc34248c08 64 end_cal=end_int*0.05;
mrtkhmbed 0:2ffc34248c08 65 }
mrtkhmbed 0:2ffc34248c08 66 void SCforMatlab::setDutyMax(float duty_max_)
mrtkhmbed 0:2ffc34248c08 67 {
mrtkhmbed 0:2ffc34248c08 68 duty_max=duty_max_/0.05;
mrtkhmbed 0:2ffc34248c08 69 }
mrtkhmbed 0:2ffc34248c08 70 void SCforMatlab::calC_Dvalue() //最小二乗法によりC,D値を算出し、SpeedControllerの引数の形式で表示
mrtkhmbed 0:2ffc34248c08 71 {
mrtkhmbed 0:2ffc34248c08 72 printf("Calculate C,Dvalue\r\n");
mrtkhmbed 0:2ffc34248c08 73 float sum_omega[2]= {};
mrtkhmbed 0:2ffc34248c08 74 float sum_duty[2]= {};
mrtkhmbed 0:2ffc34248c08 75 float sum_omega2[2]= {};
mrtkhmbed 0:2ffc34248c08 76 float sum_omegaduty[2]= {};
mrtkhmbed 0:2ffc34248c08 77 int num_sumple;
mrtkhmbed 0:2ffc34248c08 78 int start_num=start_cal/0.05;
mrtkhmbed 0:2ffc34248c08 79 int end_num=end_cal/0.05;
mrtkhmbed 0:2ffc34248c08 80 num_sumple=end_num-start_num+1;
mrtkhmbed 0:2ffc34248c08 81 for(int i=start_num; i<=end_num; i++) {
mrtkhmbed 0:2ffc34248c08 82 sum_omega[MEASURE_CW]+=least_squares[i][MEASURE_CW];
mrtkhmbed 0:2ffc34248c08 83 sum_duty[MEASURE_CW]+=0.05*i;
mrtkhmbed 0:2ffc34248c08 84 sum_omega2[MEASURE_CW]+=least_squares[i][MEASURE_CW]*least_squares[i][MEASURE_CW];
mrtkhmbed 0:2ffc34248c08 85 sum_omegaduty[MEASURE_CW]+=0.05*i*least_squares[i][MEASURE_CW];
mrtkhmbed 0:2ffc34248c08 86 sum_omega[MEASURE_CCW]+=least_squares[i][MEASURE_CCW];
mrtkhmbed 0:2ffc34248c08 87 sum_duty[MEASURE_CCW]+=0.05*i;
mrtkhmbed 0:2ffc34248c08 88 sum_omega2[MEASURE_CCW]+=least_squares[i][MEASURE_CCW]*least_squares[i][MEASURE_CCW];
mrtkhmbed 0:2ffc34248c08 89 sum_omegaduty[MEASURE_CCW]+=0.05*i*least_squares[i][MEASURE_CCW];
mrtkhmbed 0:2ffc34248c08 90 }
mrtkhmbed 0:2ffc34248c08 91 cf_=num_sumple*sum_omegaduty[MEASURE_CW]-sum_omega[MEASURE_CW]*sum_duty[MEASURE_CW];
mrtkhmbed 0:2ffc34248c08 92 cf_/=num_sumple*sum_omega2[MEASURE_CW]-sum_omega[MEASURE_CW]*sum_omega[MEASURE_CW];
mrtkhmbed 0:2ffc34248c08 93 df_=sum_omega2[MEASURE_CW]*sum_duty[MEASURE_CW]-sum_omegaduty[MEASURE_CW]*sum_omega[MEASURE_CW];
mrtkhmbed 0:2ffc34248c08 94 df_/=num_sumple*sum_omega2[MEASURE_CW]-sum_omega[MEASURE_CW]*sum_omega[MEASURE_CW];
mrtkhmbed 0:2ffc34248c08 95 cb_=num_sumple*sum_omegaduty[MEASURE_CCW]-sum_omega[MEASURE_CCW]*sum_duty[MEASURE_CCW];
mrtkhmbed 0:2ffc34248c08 96 cb_/=num_sumple*sum_omega2[MEASURE_CCW]-sum_omega[MEASURE_CCW]*sum_omega[MEASURE_CCW];
mrtkhmbed 0:2ffc34248c08 97 db_=sum_omega2[MEASURE_CCW]*sum_duty[MEASURE_CCW]-sum_omegaduty[MEASURE_CCW]*sum_omega[MEASURE_CCW];
mrtkhmbed 0:2ffc34248c08 98 db_/=num_sumple*sum_omega2[MEASURE_CCW]-sum_omega[MEASURE_CCW]*sum_omega[MEASURE_CCW];
mrtkhmbed 0:2ffc34248c08 99 printf("C,Dvalue(for SpeedController)\r\n\n");
mrtkhmbed 0:2ffc34248c08 100 printf("(cf,df,cb,db);\r\n");
mrtkhmbed 0:2ffc34248c08 101 printf("(%f,%f,%f,%f);\r\n",cf_,df_,cb_,db_);
mrtkhmbed 0:2ffc34248c08 102 }
mrtkhmbed 0:2ffc34248c08 103 void SCforMatlab::measureMotor()//メイン文で一回これを処理すれば測定・出力が完了する
mrtkhmbed 0:2ffc34248c08 104 {
mrtkhmbed 0:2ffc34248c08 105 printf("Start Measure\r\n");
mrtkhmbed 0:2ffc34248c08 106 ////////////////////////////////////////////////////////正転
mrtkhmbed 0:2ffc34248c08 107 printf("Forward!\r\n");
mrtkhmbed 0:2ffc34248c08 108 save_count=0;
mrtkhmbed 0:2ffc34248c08 109 duty_count=0;
mrtkhmbed 0:2ffc34248c08 110 measure_mode=MEASURE_CW; //正転方向測定開始
mrtkhmbed 0:2ffc34248c08 111 ticker_.attach(callback(this,&SCforMatlab::measureTurn),delta_t);
mrtkhmbed 0:2ffc34248c08 112 while(duty_count<duty_max) { //デューティ比の上限は8.5になっている。上げる場合はMatlab用の配列の要素数が足りるか注意。0.70以下に下げる場合は最小二乗法の計算も書き換えが必要
mrtkhmbed 0:2ffc34248c08 113 least_squares[duty_count][MEASURE_CW]=(ec_->getOmega())*GEER;
mrtkhmbed 0:2ffc34248c08 114 duty_count++;
mrtkhmbed 0:2ffc34248c08 115 duty=duty_count*0.05;
mrtkhmbed 0:2ffc34248c08 116 wait(turn_time);
mrtkhmbed 0:2ffc34248c08 117 }
mrtkhmbed 0:2ffc34248c08 118 while(duty>0.05) { //急停止防止。徐々に減速して停止する。
mrtkhmbed 0:2ffc34248c08 119 duty-=0.05;
mrtkhmbed 0:2ffc34248c08 120 wait(0.1);
mrtkhmbed 0:2ffc34248c08 121 }
mrtkhmbed 0:2ffc34248c08 122 duty=0;
mrtkhmbed 0:2ffc34248c08 123 wait(2);
mrtkhmbed 0:2ffc34248c08 124 /////////////////////////////////////////////////////////逆転
mrtkhmbed 0:2ffc34248c08 125 printf("Back!\r\n");
mrtkhmbed 0:2ffc34248c08 126 duty_count=0;
mrtkhmbed 0:2ffc34248c08 127 measure_mode=MEASURE_CCW; //逆転方向測定開始
mrtkhmbed 0:2ffc34248c08 128 //ticker_.attach(callback(this,&SCforMatlab::measureTurn),delta_t);
mrtkhmbed 0:2ffc34248c08 129 while(duty_count<duty_max) { //デューティ比の上限は8.5になっている。上げる場合は配列の要素数が足りるか注意してください。
mrtkhmbed 0:2ffc34248c08 130 least_squares[duty_count][MEASURE_CCW]=(ec_->getOmega())*GEER;
mrtkhmbed 0:2ffc34248c08 131 duty_count++;
mrtkhmbed 0:2ffc34248c08 132 duty=duty_count*(-0.05);
mrtkhmbed 0:2ffc34248c08 133 wait(turn_time);
mrtkhmbed 0:2ffc34248c08 134 }
mrtkhmbed 0:2ffc34248c08 135 while(duty<-0.05) { //急停止防止。徐々に減速して停止する。
mrtkhmbed 0:2ffc34248c08 136 duty+=0.05;
mrtkhmbed 0:2ffc34248c08 137 wait(0.1);
mrtkhmbed 0:2ffc34248c08 138 }
mrtkhmbed 0:2ffc34248c08 139 duty=0;
mrtkhmbed 0:2ffc34248c08 140 wait(0.05);
mrtkhmbed 0:2ffc34248c08 141 ticker_.detach();
mrtkhmbed 0:2ffc34248c08 142 //////////////////////////////
mrtkhmbed 0:2ffc34248c08 143 showOmega();//Matlabで解析するデータを表示（エクセルにコピペしてください。）
mrtkhmbed 0:2ffc34248c08 144 calC_Dvalue();//SpeedController用の係数を表示(こちらはMatlabで使わないが、まとめてエクセルに保存を推奨)
mrtkhmbed 0:2ffc34248c08 145 }