File content as of revision 17:1ac855d69c78:

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

#include "controller.h"
#include "UVWpwm.h"
#include "fast_math.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);

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);

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;

//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の機器定数等の設定, 制御器の初期化
    float  r2, r3;


    // 対象の機器定数 PA 5HP scroll from IPEC2000 "High Efficiency Control for Interior Permanent Magnet Synchronous Motor"
    // outside diameter of stator   150 mm
    // outside diameter of rotor    84.0 mm
    // width of rotor           70.0 mm
    // maximum speed        7500 r/min  (min=900rpm)
    // maximum torque       15.0 Nm
    // Ψa           0.176 Wb
    // Ld           3.50 mH
    // Lq           6.30 mH
    // Ra           0.143Ω
    // Rc           200Ω
#ifdef SIMULATION
    p.Ld = 0.0035;  // H
    p.Lq = 0.0063;  // H
    p.Lq0 = p.Lq;
    p.Lq1 = 0;
    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.iab[0] =0;    p.iab[1] = 0;   // iab = [iα;iβ];
    p.vab[0] =0;    p.vab[1] = 0;   // vab = [vα;vβ];
        // UVW座標からαβ座標への変換行列Cuvwの設定
    r2 = sqrt(2.);//1.414213562373095;//2^(1/2);
    r3 = sqrt(3.);//1.732050807568877;//3^(1/2);
    //  p.Cuvw =[ r2/r3 -1/r2/r3 -1/r2/r3; ...
    //                0     1/r2 -1/r2   ];
    p.Cuvw[0][0] = r2/r3;   p.Cuvw[0][1] = -1./r2/r3;   p.Cuvw[0][2] = -1./r2/r3;
    p.Cuvw[1][0] =     0;   p.Cuvw[1][1] =  1/r2   ;    p.Cuvw[1][2] = -1./r2;

    p.w = 0;

    // 制御器の初期化
    vl.iq_ref=0;        // ｑ軸電流指令[A]
    vl.w_lpf = 0;       // 検出した速度[rad/s]
    vl.eI = 0;          // 速度制御用偏差の積分値（積分項）
    il.eI_idq[0] = 0;   // d軸電流制御用偏差の積分値（積分項）
    il.eI_idq[1] = 0;   // q軸電流制御用偏差の積分値（積分項）
    il.e_old[0] = 0;    // d軸電流制御用偏差の1サンプル過去の値
    il.e_old[1] = 0;    // q軸電流制御用偏差の1サンプル過去の値
#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(float idq_act[2]){
//  ｄｑ座標電流PID制御器（電流マイナーループのフィードバック制御）
//      入力：指令dq座標電流 idq_ref [A], 実dq座標電流 idq_act [A], PI制御積分項 eI, サンプル時間 TS0 [s]
//      出力：dq軸電圧指令 vdq_ref [A]
//       [vdq_ref,eI_idq] = idq_control(idq_ref,idq_act,eI_idq,ts);
    float  e[2], ed[2];

    // ｄｑ電流偏差の計算
    e[0] = il.idq_ref[0] - idq_act[0];
    e[1] = il.idq_ref[1] - idq_act[1];

    // ｄｑ電流偏差の積分項の計算
    il.eI_idq[0] = il.eI_idq[0] + TS0*e[0];
    il.eI_idq[1] = il.eI_idq[1] + TS0*e[1];


    // ｄｑ電流偏差の微分値の計算
    ed[0] = (e[0]-il.e_old[0])/TS0;
    ed[1] = (e[1]-il.e_old[1])/TS0;
    il.e_old[0] = e[0];  // 電流偏差の1サンプル過去の値を更新
    il.e_old[1] = e[1];  // 電流偏差の1サンプル過去の値を更新

    // PID制御
    //  vdq_ref = [Kp_d 0;0 Kp_q]*e + [Ki_d 0;0 Ki_q]*eI;
    il.vdq_ref[0] = iKPd*e[0] + iKId*il.eI_idq[0] + iKDd*ed[0];
    il.vdq_ref[1] = iKPq*e[1] + iKIq*il.eI_idq[1] + iKDq*ed[0];
}

void current_loop(){    // 電流制御マイナーループ
    float  th, c, s, Cdq[2][2], iu, iv, iab[2], idq_act[2], vab_ref[2], tmp, prev[2];
  if( f_find_origin==1 ){
    th = p.th_const;
  }else{
    th = p.th[0];
  }

    // αβ座標からｄｑ座標への変換行列Cdqの設定
#if 1   //BUG!! if move sqrt2 to fast_math.h, sim starts and completed without working!?
    c = cos(th);
    s = sin(th);
#else
    c = (float)(_cos(th/(PI/3.)*(float)DEG60+0.5))/65535.;
    s = (float)(_sin(th/(PI/3.)*(float)DEG60+0.5))/65535.;
#endif
    Cdq[0][0]= c;   Cdq[0][1]=s;    //Cdq ={{ c, s}
    Cdq[1][0]=-s;   Cdq[1][1]=c;    //       {-s, c]};

    iu = p.iuvw[0];
    iv = p.iuvw[1];
//  iw = -(iu + iv);    // iu+iv+iw=0であることを利用してiw を計算

    // iab = p.Cuvw*[iu;iv;iw];
//  iab[0] = p.Cuvw[0][0]*iu + p.Cuvw[0][1]*iv + p.Cuvw[0][2]*iw;
//  iab[1] = p.Cuvw[1][0]*iu + p.Cuvw[1][1]*iv + p.Cuvw[1][2]*iw;
//  iab[0] = p.Cuvw[0][0]*iu + p.Cuvw[0][1]*(iv+iw);
//  iab[1] =                   p.Cuvw[1][1]*(iv-iw);
    iab[0] = (p.Cuvw[0][0]-p.Cuvw[0][1])*iu;
    iab[1] = p.Cuvw[1][1]*(iu+2*iv);

    // αβ座標電流をｄｑ座標電流に変換
    //idq_act = Cdq * iab;
    idq_act[0] = Cdq[0][0]*iab[0] + Cdq[0][1]*iab[1];
    idq_act[1] = Cdq[1][0]*iab[0] + Cdq[1][1]*iab[1];

    // dq電流制御（電流フィードバック制御）
//  [vdq_ref,eI_idq] = idq_control(idq_ref,idq_act,eI_idq,ts);
#ifdef USE_CURRENT_CONTROL
    idq_control(idq_act);
#else
    il.vdq_ref[0] = il.idq_ref[0]/iqMAX*vdqMAX;
    il.vdq_ref[1] = il.idq_ref[1]/iqMAX*vdqMAX;
#endif
    // ｄｑ軸電圧指令ベクトルの大きさをMAX制限してアンチワインドアップ対策
    // if( norm(vdq_ref) > vdqmax ){    vdq_ref= vdqmax/norm(vdq_ref)*vdq_ref}
    // anti-windup: if u=v_ref is saturated, then reduce eI. 
    //電圧振幅の2乗 vd^2+vq^2 を計算
    tmp=il.vdq_ref[0]*il.vdq_ref[0]+il.vdq_ref[1]*il.vdq_ref[1];
    if( tmp > SQRvdqMAX ){  // 電圧振幅の2乗がVMAXより大きいとき
        prev[0] = il.vdq_ref[0];    // vdを記憶
        prev[1] = il.vdq_ref[1];    // vqを記憶
        tmp = sqrt2(SQRvdqMAX/tmp); // 振幅をVMAXまで小さくする比を求める
        il.vdq_ref[0] = tmp*il.vdq_ref[0]; // vdにその比をかける
        il.vdq_ref[1] = tmp*il.vdq_ref[1]; // vqにその比をかける
        il.eI_idq[0] -= (prev[0]-il.vdq_ref[0])/iKId; // 振幅を小さくした分、
        if( il.eI_idq[0]<0 ){   il.eI_idq[0]=0;}      // I項を小さくする
        il.eI_idq[1] -= (prev[1]-il.vdq_ref[1])/iKIq; // q軸にも同じ処理
        if( il.eI_idq[1]<0 ){   il.eI_idq[1]=0;}
    }
//#define DOUKI
#ifdef DOUKI
il.vdq_ref[0]=0;
il.vdq_ref[1]=vdqMAX;
#endif
//analog_out=il.vdq_ref[1]/3.3+0.4;//koko
    // ｄｑ座標指令電圧 vd_ref, vq_refからiα, iβを計算
    // vab_ref = Cdq'*vdq_ref;
    vab_ref[0] = Cdq[0][0]*il.vdq_ref[0] + Cdq[1][0]*il.vdq_ref[1];
    vab_ref[1] = Cdq[0][1]*il.vdq_ref[0] + Cdq[1][1]*il.vdq_ref[1];
//analog_out=vab_ref[1]/3.3+0.4;

    // モータに印加するUVW相電圧を計算 （vα, vβからvu, vv, vwを計算）
    //  vu = √(2/3)*va;
    //  vv = -1/√6*va + 1/√2*vb;
    //  vw = -1/√6*va - 1/√2*vb;
    //  p.Cuvw =[ r2/r3 -1/r2/r3 -1/r2/r3; ...
    //                0     1/r2 -1/r2   ];
    // p.vuvw = p.Cuvw'*vab_ref;
    p.vuvw[0] = p.Cuvw[0][0]*vab_ref[0];
    p.vuvw[1] = p.Cuvw[0][1]*vab_ref[0] + p.Cuvw[1][1]*vab_ref[1];
    p.vuvw[2] = -p.vuvw[0] - p.vuvw[1];
//  p.vuvw[0] = p.Cuvw[0][0]*vab_ref[0] + p.Cuvw[1][0]*vab_ref[1];
//  p.vuvw[2] = p.Cuvw[0][2]*vab_ref[0] + p.Cuvw[1][2]*vab_ref[1];
}


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

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

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

    ed = (e-vl.e_old)/TS1;          // 速度偏差の微分値の計算
    vl.e_old = e;                   // 速度偏差の1サンプル過去の値を更新

    // ＰＩ制御
    vl.iq_ref = wKp*e + wKi*vl.eI + wKd*ed; // ＰＩＤ制御器の出力を計算
}

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

    // 速度ωを求めるために、位置θをオイラー微分して、一次ローパスフィルタに通す。
    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);
tmp=vl.w_lpf/(2*PI) /20; if(tmp>1) tmp=1;else if(tmp<0) tmp=0;
analog_out=tmp;//tmp;//koko

    // 速度制御：速度偏差が入力され、q軸電流指令を出力。
//  [iq_ref,eI] = vel_control(w_ref,w_act,eI,ts);
    vel_control();

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

    // 電流ベクトル制御
    if( vl.iq_ref>=0 ){ tmp = vl.tan_beta_ref;}     // 負のトルクを発生させるときはidは負のままでiqを正から負にする
    else{           tmp = -vl.tan_beta_ref;}// Tm = p((phi+(Ld-Lq)id) iqより
    //idq_ref = {{-tmp, 1}}*iq_ref;
    idq_ref[0] = -tmp*vl.iq_ref;    idq_ref[1] = vl.iq_ref;

    // ｄｑ軸電流の目標値を速度制御メインループから電流制御マイナーループへ渡す。
    il.idq_ref[0] = idq_ref[0];
    il.idq_ref[1] = idq_ref[1];
  if( f_find_origin==1 ){
    il.idq_ref[0] = iqMAX/1.0;  // idをプラス、iqをゼロにして、
    il.idq_ref[1] = 0;          // 無負荷のときにθ=0とさせる。
  }
}

void    vuvw2pwm(){ // vu, vv, vwより、UVW相の上アームPWMを発生
    float   duty_u, duty_v, duty_w;

    duty_u = p.vuvw[0]/vdqMAX+0.5; // dutyを計算
    duty_v = p.vuvw[1]/vdqMAX+0.5; // dutyを計算
    duty_w = p.vuvw[2]/vdqMAX+0.5; // dutyを計算
    uvw[0].duty = duty_u;  // dutyをPWM発生関数に渡す
    uvw[1].duty = duty_v;  // dutyをPWM発生関数に渡す
    uvw[2].duty = duty_w;  // dutyをPWM発生関数に渡す
}

#ifdef SIMULATION
void    sim_motor(){
//  モータシミュレータ
//      入力信号：UVW相電圧p.vuvw [V]、負荷トルクp.TL [Nm]
//      出力信号：モータ角度p.th[0] [rad], モータ速度p.w [rad/s], モータUVW相電流p.iuvw [A]
//      p = motor(p, ts);   // IPM, ｄｑ座標
    float c, s, Cdq[2][2], idq_dot[2], id,iq, vdq[2], idq[2], Tall,TL, Cdq_inv[2][2];
analog_out=p.vuvw[0]/100.+0.5;//debug
    // vu, vv, vwからvα, vβを計算
    p.vab[0] = p.Cuvw[0][0]*p.vuvw[0] + p.Cuvw[0][1]*p.vuvw[1] + p.Cuvw[0][2]*p.vuvw[2];
    p.vab[1] = p.Cuvw[1][0]*p.vuvw[0] + p.Cuvw[1][1]*p.vuvw[1] + p.Cuvw[1][2]*p.vuvw[2];
//printf("vab=%f, %f ",p.vab[0],p.vab[1]);scanf("%f",&c);

    // αβ座標からｄｑ座標への変換行列Cdqの設定
    c = cos(p.th[0]);
    s = sin(p.th[0]);
    // Cdq =[ c s; ...
    //       -s c];
    Cdq[0][0] = c;  Cdq[0][1] = s;
    Cdq[1][0] =-s;  Cdq[1][1] = c;

    // ｖα, ｖβからｖｄ, ｖｑを計算
    //  vd = c*p.va + s*p.vb;
    //  vq =-s*p.va + c*p.vb;
    // vdq = Cdq * p.vab;
    vdq[0] = Cdq[0][0]*p.vab[0] + Cdq[0][1]*p.vab[1];
    vdq[1] = Cdq[1][0]*p.vab[0] + Cdq[1][1]*p.vab[1];

    // iα, iβからid, iqを計算
    //  id = c*p.ia + s*p.ib;
    //  iq =-s*p.ia + c*p.ib;
    // idq = Cdq * p.iab;
    idq[0] = Cdq[0][0]*p.iab[0] + Cdq[0][1]*p.iab[1];
    idq[1] = Cdq[1][0]*p.iab[0] + Cdq[1][1]*p.iab[1];

    // get id,iq
    //  id_dot = (1.0/p.Ld) * ( vd - (p.R*id - p.w*p.Lq*iq) );
    //  iq_dot = (1.0/p.Lq) * ( vq - (p.R*iq + p.w*p.Ld*id + p.w*p.phi) );
    // idq_dot = [p.Ld 0;0 p.Lq]\( vdq - p.R*idq - p.w*[0 -p.Lq;p.Ld 0]*idq - p.w*[0;p.phi]);
    idq_dot[0] = (1.0/p.Ld) * ( vdq[0] - (p.R*idq[0] - p.w*p.Lq*idq[1]) );
    idq_dot[1] = (1.0/p.Lq) * ( vdq[1] - (p.R*idq[1] + p.w*p.Ld*idq[0] + p.w*p.phi) );
    //  id = id + ts * id_dot;
    //  iq = iq + ts * iq_dot;
    p.idq[0] = idq[0] + TS0*idq_dot[0];
    p.idq[1] = idq[1] + TS0*idq_dot[1];
    id = p.idq[0];
    iq = p.idq[1];

    // 磁気飽和を考慮したLqの計算
    p.Lq = p.Lq0 + p.Lq1*abs(iq);
    if( p.Lq < p.Ld )
        p.Lq = p.Ld;

    // モータトルクの計算
    p.Tm = p.p * (p.phi + (p.Ld-p.Lq)*id) * iq;

    // モータ速度ωの計算
    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]>4*PI)
        p.th[0] = p.th[0] - 4*PI;

    if( p.th[0]<0 )
        p.th[0] = p.th[0] + 4*PI;

    // ｄｑ座標からαβ座標への変換行列Cdq_invの設定
    c = cos(p.th[0]);
    s = sin(p.th[0]);
    // Cdq_inv =[ c -s; ...
    //            s c];
    Cdq_inv[0][0] = c;  Cdq_inv[0][1] =-s;
    Cdq_inv[1][0] = s;  Cdq_inv[1][1] = c;

    // id, iqからiα, iβを計算
    //p.iab = Cdq_inv * p.idq;
    p.iab[0] = Cdq_inv[0][0]*p.idq[0] + Cdq_inv[0][1]*p.idq[1];
    p.iab[1] = Cdq_inv[1][0]*p.idq[0] + Cdq_inv[1][1]*p.idq[1];

    // αβ座標からUVW座標への変換行列Cuvw_inv=Cuvw'
    // iα, iβからiu, iv, iwを計算
    //  iu = r2/r3*ia;
    //  iv = -1/r2/r3*ia + 1/r2*ib;
    //  iw = -1/r2/r3*ia - 1/r2*ib;
    //p.iuvw = p.Cuvw' * p.iab;
    p.iuvw[0] = p.Cuvw[0][0]*p.iab[0] + p.Cuvw[1][0]*p.iab[1];
    p.iuvw[1] = p.Cuvw[0][1]*p.iab[0] + p.Cuvw[1][1]*p.iab[1];
    p.iuvw[2] = p.Cuvw[0][2]*p.iab[0] + p.Cuvw[1][2]*p.iab[1];
}
#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]=p.vuvw[0];
        data[_count_data][2]=vl.w_lpf; data[_count_data][3]=_count*TS0; data[_count_data][4]=il.vdq_ref[1];
        _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;
    
    p.th[1] = p.th[0];  // thを更新
 #ifdef SIMULATION
    // モータシミュレータ
    sim_motor();    // IPM, ｄｑ座標
 #else
#ifdef DOUKI
led1=1;
p.th[0] += 2*PI*TS0 * 1; if(p.th[0]>4*PI){   p.th[0]-=4*PI;}
//debug[0]=p.th[0]/PI*180;
analog_out=debug[0]/180*PI/4/PI;
led1=0;
#else
    // 位置θをセンサで検出
    p.th[0] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI;   // get angle [rad] from encoder
debug[0]=p.th[0]/PI*180;
debug[1]=p.th[0]/(2*PI);    debug[1]=debug[1]-(int)debug[1];    if(debug[1]<0)  debug[1]+=1;
debug[0]=debug[1]*360;
//analog_out=debug[1];
#endif
 #endif
    current_loop(); // 電流制御マイナーループ(idq_ref to vuvw)
    vuvw2pwm();     // vuvw to pwm
//    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.5f[V]\t%8.2f [Hz]\t%8.2f\t%8.2f\r\n", 
        _time, il.vdq_ref[1], 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].
}