File content as of revision 12:459af534d1ee:

//  BLDCmotor.cpp: 各種３相同期モータに対するセンサあり運転のシミュレーション
//      Kosaka Lab. 121215
#include "mbed.h"
#include "QEI.h"

#include "controller.h"
#include "Hbridge.h"
Serial pc(USBTX, USBRX);        // Display on tera term in PC 

motor_parameters            p;  // モータの定数、信号など
current_loop_parameters     il; // 電流制御マイナーループの定数、変数
velocity_loop_parameters    vl; // 速度制御メインループの定数、変数

QEI encoder (CH_A, CH_B, NC, N_ENC, QEI::X4_ENCODING);
//  QEI(PinName     channelA, mbed pin for channel A input.
//      PinName     channelB, mbed pin for channel B input.
//      PinName     index,    mbed pin for channel Z input. (index channel input Z phase th=0), (pass NC if not needed).
//      int         pulsesPerRev, Number of pulses in one revolution(=360 deg).
//      Encoding    encoding = X2_ENCODING, X2 is default. X2 uses interrupts on the rising and falling edges of only channel A where as 
//                    X4 uses them on both channels.
//  )
//  void     reset (void)
//     Reset the encoder. 
//  int      getCurrentState (void)
//     Read the state of the encoder. 
//  int      getPulses (void)
//     Read the number of pulses recorded by the encoder. 
//  int      getRevolutions (void)
//     Read the number of revolutions recorded by the encoder on the index channel. 
/*********** User setting for control parameters (end) ***************/

AnalogOut   analog_out(DA_PORT);
AnalogIn VshuntR_Uplus(p19);    // *3.3 [V], Volt of shunt R_SHUNT[Ohm]. The motor current i = v_shunt_r/R_SHUNT [A]
AnalogIn VshuntR_Uminus(p20);    // *3.3 [V], Volt of shunt R_SHUNT[Ohm]. The motor current i = v_shunt_r/R_SHUNT [A]
AnalogIn VshuntR_Vplus(p16);    // *3.3 [V], Volt of shunt R_SHUNT[Ohm]. The motor current i = v_shunt_r/R_SHUNT [A]
AnalogIn VshuntR_Vminus(p17);    // *3.3 [V], Volt of shunt R_SHUNT[Ohm]. The motor current i = v_shunt_r/R_SHUNT [A]

unsigned long _count;   // sampling number
float   _time;          // time[s]
float   debug[20];      // for debug
float   disp[10];       // for printf to avoid interrupted by quicker process
DigitalOut  led1(LED1);
DigitalOut  led2(LED2);
DigitalOut  led3(LED3);
DigitalOut  led4(LED4);

#ifdef  GOOD_DATA
float data[1000][5];    // memory to save data offline instead of "online fprintf".
unsigned int    count3; // 
unsigned int    count2=(int)(TS2/TS0); // 
unsigned short _count_data=0;
#endif
DigitalOut  debug_p26(p26); // p17 for debug
DigitalOut  debug_p25(p25); // p17 for debug
//DigitalOut  debug_p24(p24); // p17 for debug
//AnalogIn VCC(p19);          // *3.3 [V], Volt of VCC for motor
//AnalogIn VCC2(p20);         // *3.3 [V], Volt of (VCC-R i), R=2.5[Ohm]. R is for preventing too much i when deadtime is failed.

unsigned short  f_find_origin;  // flag to find the origin of the rotor angle theta

#if 1   //BUG!! if move sqrt2 to fast_math.h, sim starts and completed without working!?
float  sqrt2(float x){    // √xのx=1まわりのテイラー展開 √x = 1 + 1/2*(x-1) -1/4*(x-1)^2 + ...
//  return((1+x)*0.5);      // 一次近似
    return(x+(1-x*x)*0.25); // 二次近似
}
#endif

void init_parameters(){    // IPMSMの機器定数等の設定, 制御器の初期化
#ifdef SIMULATION
    p.L = 0.0063;   // H
    p.R = 0.143;    // Ω
    p.phi = 0.176;  // V s
    p.Jm = 0.00018; // Nms^2
    p.p = 2;        // 極対数
#endif
    p.th[0] = p.th[1] = 0;
    p.w = 0;
    p.i = 0;
    p.v =0;

    p.w = 0;

    // 制御器の初期化
    vl.i_ref=0;         // 電流指令[A]
    vl.w_lpf = 0;       // 検出した速度[rad/s]
    vl.eI_w = 0;        // 速度制御用偏差の積分値（積分項）
    il.eI_i = 0;        // 電流制御用偏差の積分値（積分項）
#ifndef SIMULATION
    encoder.reset();    // set encoder counter zero
    p.th[0] = p.th[1] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI;   // get angle [rad] from encoder
#endif
}

void idq_control(){
//  ｄｑ座標電流PID制御器（電流マイナーループのフィードバック制御）
//      入力：指令電流 i_ref [A], 実電流 p.i [A], PI制御積分項 eI, サンプル時間 TS0 [s]
//      出力：電圧指令 v_ref [A]
    float  e;

//debug[0]=il.i_ref;
    // ｄｑ電流偏差の計算
    e = il.i_ref - p.i;

    // ｄｑ電流偏差の積分項の計算
    il.eI_i = il.eI_i + TS0*e;

    // ＰＩ制御
    il.v_ref = iKP*e + iKI*il.eI_i;

}

void current_loop(){    // 電流制御マイナーループ
    // 電流センサによってiu, iv を検出
#ifndef SIMULATION
    p.i = (VshuntR_Uplus - VshuntR_Uminus) /R_SHUNT;      // get i [A] from shunt resistance;
#endif
//debug[0]=p.i;
    // dq電流制御（電流フィードバック制御）
debug[0]=il.i_ref;
#ifdef USE_CURRENT_CONTROL
    idq_control();
#else
    il.v_ref = il.i_ref/iMAX*vMAX;
#endif
    // 電圧指令の大きさをMAX制限
    // anti-windup: if u=v_ref is saturated, then reduce eI. 
    if( il.v_ref > vMAX ){
        il.eI_i -= (il.v_ref - vMAX)/iKI;    if( il.eI_i<0 ){   il.eI_i=0;}
        il.v_ref = vMAX;
    }else if( il.v_ref < -vMAX ){
        il.eI_i -= (il.v_ref + vMAX)/iKI;    if( il.eI_i>0 ){   il.eI_i=0;}
        il.v_ref = -vMAX;
    }
    p.v = il.v_ref;

    p.th[1] = p.th[0];  // thを更新
}


void vel_control(){
//  速度制御器：速度偏差が入力され、q軸電流指令を出力。
//      入力：指令速度 w_ref [rad/s], 実速度 w_lpf [rad/s], PI制御積分項 eI, サンプル時間 TS1 [s]
//      出力：電流指令 i_ref [A]
    float  e;

    // 速度偏差の計算
    e = vl.w_ref - vl.w_lpf;

    // 速度偏差の積分値の計算
    vl.eI_w = vl.eI_w + TS1*e;

    // ＰＩ制御
    vl.i_ref = wKp*e + wKi*vl.eI_w;
}

void velocity_loop(){   // 速度制御メインループ(w_ref&beta_ref to idq_ref)
    float  tmp;

    // 速度ωを求めるために、位置θをオイラー微分して、一次ローパスフィルタに通す。
#if 1
    tmp = p.th[0]-p.th[1];
    while( tmp> PI ){ tmp -= 2*PI;}
    while( tmp<-PI ){ tmp += 2*PI;}
    vl.w_lpf = iLPF*vl.w_lpf + tmp/TS0 *(1-iLPF);
#else
    vl.w_lpf = p.th[0];
#endif
    // 速度制御：速度偏差が入力され、電流指令を出力。
    vel_control();

    // 電流指令のMAX制限（異常に大きい指令値を制限する）
    // anti-windup: if u=i_ref is saturated, then reduce eI. 
    if( vl.i_ref > iMAX ){
        vl.eI_w -= (vl.i_ref - iMAX)/wKi;    if( vl.eI_w<0 ){   vl.eI_w=0;}
        vl.i_ref = iMAX;
    }else if( vl.i_ref < -iMAX ){
        vl.eI_w -= (vl.i_ref + iMAX)/wKi;    if( vl.eI_w>0 ){   vl.eI_w=0;}
        vl.i_ref = -iMAX;
    }
//debug[0]=vl.eI_w;

    // 電流の目標値を速度制御メインループから電流制御マイナーループへ渡す。
    il.i_ref = vl.i_ref;
//debug[0]=il.i_ref;
}

void    v2Hbridge(){ // vより、PWMを発生
    float   duty;

//    duty = (p.v/vMAX+1)*0.5;
//    IN.duty = duty;
    duty = p.v/vMAX;
    if( duty>=0 ){
        IN.duty = duty;
        if( IN.fReverse[0]==1 ){
            IN.fDeadtime = 1;
        }
        IN.fReverse[0] = 0;
    }else{
        IN.duty = -duty;
        if( IN.fReverse[0]==0 ){
            IN.fDeadtime = 1;
        }
        IN.fReverse[0] = 1;
    }
}

#ifdef SIMULATION
void    sim_motor(){
//  モータシミュレータ
//      入力信号：電圧p.v [V]、負荷トルクp.TL [Nm]
//      出力信号：モータ角度p.th[0] [rad], モータ速度p.w [rad/s], モータ電流p.i [A]
    float i_dot, Tall, TL;
analog_out=p.v/100.+0.5;//debug
//debug[0]=p.v;
    // get i
    i_dot = (1.0/p.L) * ( p.v - (p.R*p.i + p.w*p.phi) );
    p.i = p.i + TS0*i_dot;

    // モータトルクの計算
    p.Tm = p.p * p.phi * p.i;

    // モータ速度ωの計算
    if( abs(p.w) > 5*2*PI )
        if( p.w>=0 )    TL= p.TL;
        else            TL=-p.TL;
    else
        TL = p.w/(5*2*PI)*p.TL;

    Tall = p.Tm - TL;
    p.w = p.w + TS0 * (1.0 / p.Jm) * Tall;

    // モータ角度θの計算
    p.th[0] = p.th[0] + TS0 * p.w;
    if( p.th[0]>2*PI)
        p.th[0] = p.th[0] - 2*PI;

    if( p.th[0]<0 )
        p.th[0] = p.th[0] + 2*PI;
//debug[0]=p.v;
}
#endif

void data2mbedUSB(){    // save data to mbed USB drive 
    if( _count_data<1000 ){
        data[_count_data][0]=p.th[0]/*vl.w_ref*/; data[_count_data][1]=debug[0];
        data[_count_data][2]=vl.w_lpf; data[_count_data][3]=_count*TS0; data[_count_data][4]=il.v_ref;
        _count_data++;
    }
#if 0
  if( _count_data<500 ){
    debug[0]=p.vab[0]; debug[1]=p.vab[1]; debug[2]=il.vdq_ref[0]; debug[3]=il.vdq_ref[1]; debug[4]=p.iab[0];
    debug[5]=p.iab[1]; debug[6]=p.vuvw[0]; debug[7]=p.vuvw[1]; debug[8]=p.vuvw[2]; debug[9]=p.iuvw[0];
    debug[10]=p.iuvw[1]; debug[11]=p.iuvw[2]; debug[12]=p.idq[0]; debug[13]=p.idq[1]; debug[14]=p.TL;
    debug[15]=p.Tm; debug[16]=p.w; debug[17]=vl.w_lpf; debug[18]=p.th[0]; debug[19]=_count*TS0;//_time;
//BUG    for(j=0;j<19;j++){  fprintf( fp, "%f, ",debug[j]);} fprintf( fp, "%f\n",debug[19]);
    for(j=0;j<19;j++){  printf("%f, ",debug[j]);} printf("%f\n",debug[19]);
//    for(j=0;j<19;j++){  pc.printf("%f, ",debug[j]);} pc.printf("%f\n",debug[19]);
  }
#endif
}
void timerTS0(){    // timer called every TS0[s].
    debug_p26 = 1;
    _count++;
    _time += TS0;
    
    current_loop(); // 電流制御マイナーループ(i_ref to volt)
    v2Hbridge();     // volt. to Hbridge
 #ifdef SIMULATION
//debug[0]=p.v;
    // モータシミュレータ
    sim_motor();    // IPM, ｄｑ座標
 #else
    p.th[1] = p.th[0];
    p.th[0] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI;   // get angle [rad] from encoder
 #endif
    debug_p26 = 0;
}
void timerTS1(void const *argument){
    debug_p25 = 1;
    velocity_loop();    // 速度制御メインループ(w_ref&beta_ref to idq_ref)
    debug_p25 = 0;
}

void display2PC(){  // display to tera term on PC
    pc.printf("%8.1f[s]\t%8.2f[V]\t%8.2f [Hz]\t%8.2f\t%8.2f\r\n", _time, il.v_ref, vl.w_lpf/(2*PI), vl.w_ref/(2*PI), debug[0]);  // print to tera term
//    pc.printf("%8.1f[s]\t%8.5f[V]\t%4d [deg]\t%8.2f\r\n", _time, _u, (int)(_th/(2*PI)*360.0), _r);//debug[0]*3.3/R_SHUNT);  // print to tera term
}
void timerTS2(){
}
void timerTS3(){
    data2mbedUSB();  // data2mbedUSB() is called every TS3[s].
}
void timerTS4(){
    display2PC();  // display to tera term on PC. display2PC() is called every TS4[s].
}