Diff: SCforMatlab.cpp

Revision:

0:0b57d295ca56

Child:

1:6557a0b818c2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SCforMatlab.cpp	Thu Dec 24 08:35:33 2020 +0000
@@ -0,0 +1,153 @@
+#include "mbed.h"
+#include "EC.h"
+#include "SCforMatlab.h"
+SCforMatlab::SCforMatlab(PinName pwm_F,PinName pwm_B,float period_us,Ec &ec,float dt) :
+    forward(pwm_F),back(pwm_B)
+{
+    ec_=&ec;
+    forward.period_us(period_us);
+    back.period_us(period_us);
+    delta_t=dt;
+    forward=0;
+    back=0;
+    duty=0;
+    start_cal=0.7;
+    end_cal=0.1;
+    measure_mode=MEASURE_CW;
+    ec_->timer_.reset();
+    if(dt<=0.02) {
+        turn_time=0.5;
+    } else {
+        turn_time=1.0;
+    }
+}
+void SCforMatlab::saveOmega()
+{
+    ec_->calOmega();
+    omega[save_count]=(ec_->getOmega())*GEER;
+    input_duty[save_count]=duty;
+    save_count++;
+}
+void SCforMatlab::motorTurn(float out)
+{
+    if(out>0) {
+        forward=out;
+        back=0;
+    } else {
+        forward=0;
+        back=-out;
+    }
+}
+void SCforMatlab::measureTurn()
+{
+    motorTurn(duty);
+    saveOmega();
+}
+void SCforMatlab::showOmega()
+{
+    printf("\n\r\nMeasured value for Matlab\r\n");
+    printf("\n\r\ncopy all data\r\n");
+    printf("\nnum\tduty\t\tomega\r\n");
+    for(int i=0; i<save_count; i++) {
+        printf("%d\t%f\t%f\r\n",i,input_duty[i],omega[i]);
+        wait(0.01);
+    }
+    printf("\r\n");
+}
+void SCforMatlab::setCDRange(float start,float end)
+{
+    int start_int=start/0.05;  //0.05刻みで入力してもらえんかった場合の保険
+    int end_int=end/0.05;
+    start_cal=start_int*0.05;
+    end_cal=end_int*0.05;
+}
+void SCforMatlab::calC_Dvalue()  //最小二乗法によりC,D値を算出し、SpeedControllerの引数の形式で表示
+{
+    printf("Calculate C,Dvalue\r\n");
+    float sum_omega[2]= {};
+    float sum_duty[2]= {};
+    float sum_omega2[2]= {};
+    float sum_omegaduty[2]= {};
+    int num_sumple;
+    int start_num=start_cal/0.05;
+    int end_num=end_cal/0.05;
+    num_sumple=end_num-start_num+1;
+    for(int i=start_num; i<=end_num; i++) {
+        sum_omega[MEASURE_CW]+=least_squares[i][MEASURE_CW];
+        sum_duty[MEASURE_CW]+=0.05*i;
+        sum_omega2[MEASURE_CW]+=least_squares[i][MEASURE_CW]*least_squares[i][MEASURE_CW];
+        sum_omegaduty[MEASURE_CW]+=0.05*i*least_squares[i][MEASURE_CW];
+        sum_omega[MEASURE_CCW]+=least_squares[i][MEASURE_CCW];
+        sum_duty[MEASURE_CCW]+=0.05*i;
+        sum_omega2[MEASURE_CCW]+=least_squares[i][MEASURE_CCW]*least_squares[i][MEASURE_CCW];
+        sum_omegaduty[MEASURE_CCW]+=0.05*i*least_squares[i][MEASURE_CCW];
+    }
+    cf_=num_sumple*sum_omegaduty[MEASURE_CW]-sum_omega[MEASURE_CW]*sum_duty[MEASURE_CW];
+    cf_/=num_sumple*sum_omega2[MEASURE_CW]-sum_omega[MEASURE_CW]*sum_omega[MEASURE_CW];
+    df_=sum_omega2[MEASURE_CW]*sum_duty[MEASURE_CW]-sum_omegaduty[MEASURE_CW]*sum_omega[MEASURE_CW];
+    df_/=num_sumple*sum_omega2[MEASURE_CW]-sum_omega[MEASURE_CW]*sum_omega[MEASURE_CW];
+    cb_=num_sumple*sum_omegaduty[MEASURE_CCW]-sum_omega[MEASURE_CCW]*sum_duty[MEASURE_CCW];
+    cb_/=num_sumple*sum_omega2[MEASURE_CCW]-sum_omega[MEASURE_CCW]*sum_omega[MEASURE_CCW];
+    db_=sum_omega2[MEASURE_CCW]*sum_duty[MEASURE_CCW]-sum_omegaduty[MEASURE_CCW]*sum_omega[MEASURE_CCW];
+    db_/=num_sumple*sum_omega2[MEASURE_CCW]-sum_omega[MEASURE_CCW]*sum_omega[MEASURE_CCW];
+    printf("C,Dvalue(for SpeedController)\r\n\n");
+    printf("(cf,df,cb,db);\r\n");
+    printf("(%f,%f,%f,%f);\r\n",cf_,df_,cb_,db_);
+}
+void SCforMatlab::measureMotor()//メイン文で一回これを処理すれば測定・出力が完了する
+{
+    printf("Start Measure\r\n");
+////////////////////////////////////////////////////////正転
+    printf("Forward!\r\n");
+    save_count=0;
+    duty_count=0;
+    measure_mode=MEASURE_CW;                          //正転方向測定開始
+    ticker_.attach(callback(this,&SCforMatlab::measureTurn),delta_t);
+    while(duty<=0.85) {  //デューティ比の上限は8.5になっている。上げる場合はMatlab用の配列の要素数が足りるか注意。0.70以下に下げる場合は最小二乗法の計算も書き換えが必要
+        least_squares[duty_count][MEASURE_CW]=(ec_->getOmega())*GEER;
+        duty_count++;
+        duty=duty_count*0.05;
+        wait(turn_time);
+    }
+    while(duty>0.05) {      //急停止防止。徐々に減速して停止する。
+        duty-=0.05;
+        wait(0.1);
+    }
+    duty=0;
+    wait(2);
+/////////////////////////////////////////////////////////逆転
+    printf("Back!\r\n");
+    duty_count=0;
+    measure_mode=MEASURE_CCW;                          //逆転方向測定開始
+    ticker_.attach(callback(this,&SCforMatlab::measureTurn),delta_t);
+    while(duty>=-0.85) { //デューティ比の上限は8.5になっている。上げる場合は配列の要素数が足りるか注意してください。
+        least_squares[duty_count][MEASURE_CCW]=(ec_->getOmega())*GEER;
+        duty_count++;
+        duty=duty_count*(-0.05);
+        wait(turn_time);
+    }
+    while(duty<-0.05) {      //急停止防止。徐々に減速して停止する。
+        duty+=0.05;
+        wait(0.1);
+    }
+    duty=0;
+    wait(0.05);
+    ticker_.detach();
+//////////////////////////////
+    showOmega();//Matlabで解析するデータを表示（エクセルにコピペしてください。）
+    calC_Dvalue();//SpeedController用の係数を表示(こちらはMatlabで使わないが、まとめてエクセルに保存を推奨)
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+