robostep8th / Mbed 2 deprecated SCforMatlab

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:0b57d295ca56, 2020-12-24 (annotated)

Committer:
mrtkhmbed
Date:
Thu Dec 24 08:35:33 2020 +0000
Revision:
0:0b57d295ca56
Child:
1:6557a0b818c2
SCforMatlab

Who changed what in which revision?

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