File content as of revision 0:0b57d295ca56:

#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で使わないが、まとめてエクセルに保存を推奨)
}