controller.cpp

00001 //  controller.cpp: 各種３相同期モータに対するセンサあり運転のシミュレーション
00002 //      Kosaka Lab. 130905
00003 #include "mbed.h"
00004 #include "QEI.h"
00005 
00006 #include "controller.h"
00007 #include "UVWpwm.h"
00008 #include "fast_math.h"
00009 Serial pc(USBTX, USBRX);        // Display on tera term in PC 
00010 
00011 motor_parameters            p;  // モータの定数、信号など
00012 current_loop_parameters     il; // 電流制御マイナーループの定数、変数
00013 velocity_loop_parameters    vl; // 速度制御メインループの定数、変数
00014 
00015 QEI encoder (CH_A, CH_B, NC, N_ENC, QEI::X4_ENCODING);
00016 //  QEI(PinName     channelA, mbed pin for channel A input.
00017 //      PinName     channelB, mbed pin for channel B input.
00018 //      PinName     index,    mbed pin for channel Z input. (index channel input Z phase th=0), (pass NC if not needed).
00019 //      int         pulsesPerRev, Number of pulses in one revolution(=360 deg).
00020 //      Encoding    encoding = X2_ENCODING, X2 is default. X2 uses interrupts on the rising and falling edges of only channel A where as 
00021 //                    X4 uses them on both channels.
00022 //  )
00023 //  void     reset (void)
00024 //     Reset the encoder. 
00025 //  int      getCurrentState (void)
00026 //     Read the state of the encoder. 
00027 //  int      getPulses (void)
00028 //     Read the number of pulses recorded by the encoder. 
00029 //  int      getRevolutions (void)
00030 //     Read the number of revolutions recorded by the encoder on the index channel. 
00031 /*********** User setting for control parameters (end) ***************/
00032 
00033 AnalogOut   analog_out(DA_PORT);
00034 
00035 unsigned long _count;   // sampling number
00036 float   _time;          // time[s]
00037 float   debug[20];      // for debug
00038 float   disp[10];       // for printf to avoid interrupted by quicker process
00039 DigitalOut  led1(LED1);
00040 DigitalOut  led2(LED2);
00041 DigitalOut  led3(LED3);
00042 DigitalOut  led4(LED4);
00043 
00044 float data[1000][5];    // memory to save data offline instead of "online fprintf".
00045 unsigned int    count3; // 
00046 unsigned int    count2=(int)(TS2/TS0); // 
00047 unsigned short _count_data=0;
00048 
00049 //DigitalOut  debug_p26(p26); // p17 for debug
00050 //DigitalOut  debug_p25(p25); // p17 for debug
00051 //DigitalOut  debug_p24(p24); // p17 for debug
00052 //AnalogIn VCC(p19);          // *3.3 [V], Volt of VCC for motor
00053 //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.
00054 
00055 unsigned short  f_find_origin;  // flag to find the origin of the rotor angle theta
00056 
00057 #if 1   //BUG!! if move sqrt2 to fast_math.h, sim starts and completed without working!?
00058 float  sqrt2(float x){    // √xのx=1まわりのテイラー展開 √x = 1 + 1/2*(x-1) -1/4*(x-1)^2 + ...
00059 //  return((1+x)*0.5);      // 一次近似
00060     return(x+(1-x*x)*0.25); // 二次近似
00061 }
00062 #endif
00063 
00064 void init_parameters(){    // IPMSMの機器定数等の設定, 制御器の初期化
00065     float  r2, r3;
00066 
00067 
00068     // 対象の機器定数 PA 5HP scroll from IPEC2000 "High Efficiency Control for Interior Permanent Magnet Synchronous Motor"
00069     // outside diameter of stator   150 mm
00070     // outside diameter of rotor    84.0 mm
00071     // width of rotor           70.0 mm
00072     // maximum speed        7500 r/min  (min=900rpm)
00073     // maximum torque       15.0 Nm
00074     // Ψa           0.176 Wb
00075     // Ld           3.50 mH
00076     // Lq           6.30 mH
00077     // Ra           0.143Ω
00078     // Rc           200Ω
00079 #ifdef SIMULATION
00080     p.Ld = 0.0035;  // H
00081     p.Lq = 0.0063;  // H
00082     p.Lq0 = p.Lq;
00083     p.Lq1 = 0;
00084     p.R = 0.143;    // Ω
00085     p.phi = 0.176;  // V s
00086     p.Jm = 0.00018; // Nms^2
00087     p.p = 2;    // 極対数
00088 #endif
00089     p.th[0] = p.th[1] = 0;
00090     p.w = 0;
00091     p.iab[0] =0;    p.iab[1] = 0;   // iab = [iα;iβ];
00092     p.vab[0] =0;    p.vab[1] = 0;   // vab = [vα;vβ];
00093         // UVW座標からαβ座標への変換行列Cuvwの設定
00094     r2 = sqrt(2.);//1.414213562373095;//2^(1/2);
00095     r3 = sqrt(3.);//1.732050807568877;//3^(1/2);
00096     //  p.Cuvw =[ r2/r3 -1/r2/r3 -1/r2/r3; ...
00097     //                0     1/r2 -1/r2   ];
00098     p.Cuvw[0][0] = r2/r3;   p.Cuvw[0][1] = -1./r2/r3;   p.Cuvw[0][2] = -1./r2/r3;
00099     p.Cuvw[1][0] =     0;   p.Cuvw[1][1] =  1/r2   ;    p.Cuvw[1][2] = -1./r2;
00100 
00101     p.w = 0;
00102 
00103     // 制御器の初期化
00104     vl.iq_ref=0;        // ｑ軸電流指令[A]
00105     vl.w_lpf = 0;       // 検出した速度[rad/s]
00106     vl.eI = 0;          // 速度制御用偏差の積分値（積分項）
00107     il.eI_idq[0] = 0;   // d軸電流制御用偏差の積分値（積分項）
00108     il.eI_idq[1] = 0;   // q軸電流制御用偏差の積分値（積分項）
00109     il.e_old[0] = 0;    // d軸電流制御用偏差の1サンプル過去の値
00110     il.e_old[1] = 0;    // q軸電流制御用偏差の1サンプル過去の値
00111 #ifndef SIMULATION
00112     encoder.reset();    // set encoder counter zero
00113     p.th[0] = p.th[1] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI;   // get angle [rad] from encoder
00114 #endif
00115 }
00116 
00117 void idq_control(float idq_act[2]){
00118 //  ｄｑ座標電流PID制御器（電流マイナーループのフィードバック制御）
00119 //      入力：指令dq座標電流 idq_ref [A], 実dq座標電流 idq_act [A], PI制御積分項 eI, サンプル時間 TS0 [s]
00120 //      出力：dq軸電圧指令 vdq_ref [A]
00121 //       [vdq_ref,eI_idq] = idq_control(idq_ref,idq_act,eI_idq,ts);
00122     float  e[2], ed[2];
00123 
00124     // ｄｑ電流偏差の計算
00125     e[0] = il.idq_ref[0] - idq_act[0];
00126     e[1] = il.idq_ref[1] - idq_act[1];
00127 
00128     // ｄｑ電流偏差の積分項の計算
00129     il.eI_idq[0] = il.eI_idq[0] + TS0*e[0];
00130     il.eI_idq[1] = il.eI_idq[1] + TS0*e[1];
00131 
00132 
00133     // ｄｑ電流偏差の微分値の計算
00134     ed[0] = (e[0]-il.e_old[0])/TS0;
00135     ed[1] = (e[1]-il.e_old[1])/TS0;
00136     il.e_old[0] = e[0];  // 電流偏差の1サンプル過去の値を更新
00137     il.e_old[1] = e[1];  // 電流偏差の1サンプル過去の値を更新
00138 
00139     // PID制御
00140     //  vdq_ref = [Kp_d 0;0 Kp_q]*e + [Ki_d 0;0 Ki_q]*eI;
00141     il.vdq_ref[0] = iKPd*e[0] + iKId*il.eI_idq[0] + iKDd*ed[0];
00142     il.vdq_ref[1] = iKPq*e[1] + iKIq*il.eI_idq[1] + iKDq*ed[0];
00143 }
00144 
00145 void current_loop(){    // 電流制御マイナーループ
00146     float  th, c, s, Cdq[2][2], iu, iv, iab[2], idq_act[2], vab_ref[2], tmp, prev[2];
00147   if( f_find_origin==1 ){
00148     th = p.th_const;
00149   }else{
00150     th = p.th[0];
00151   }
00152 
00153     // αβ座標からｄｑ座標への変換行列Cdqの設定
00154 #if 1   //BUG!! if move sqrt2 to fast_math.h, sim starts and completed without working!?
00155     c = cos(th);
00156     s = sin(th);
00157 #else
00158     c = (float)(_cos(th/(PI/3.)*(float)DEG60+0.5))/65535.;
00159     s = (float)(_sin(th/(PI/3.)*(float)DEG60+0.5))/65535.;
00160 #endif
00161     Cdq[0][0]= c;   Cdq[0][1]=s;    //Cdq ={{ c, s}
00162     Cdq[1][0]=-s;   Cdq[1][1]=c;    //       {-s, c]};
00163 
00164     iu = p.iuvw[0];
00165     iv = p.iuvw[1];
00166 //  iw = -(iu + iv);    // iu+iv+iw=0であることを利用してiw を計算
00167 
00168     // iab = p.Cuvw*[iu;iv;iw];
00169 //  iab[0] = p.Cuvw[0][0]*iu + p.Cuvw[0][1]*iv + p.Cuvw[0][2]*iw;
00170 //  iab[1] = p.Cuvw[1][0]*iu + p.Cuvw[1][1]*iv + p.Cuvw[1][2]*iw;
00171 //  iab[0] = p.Cuvw[0][0]*iu + p.Cuvw[0][1]*(iv+iw);
00172 //  iab[1] =                   p.Cuvw[1][1]*(iv-iw);
00173     iab[0] = (p.Cuvw[0][0]-p.Cuvw[0][1])*iu;
00174     iab[1] = p.Cuvw[1][1]*(iu+2*iv);
00175 
00176     // αβ座標電流をｄｑ座標電流に変換
00177     //idq_act = Cdq * iab;
00178     idq_act[0] = Cdq[0][0]*iab[0] + Cdq[0][1]*iab[1];
00179     idq_act[1] = Cdq[1][0]*iab[0] + Cdq[1][1]*iab[1];
00180 
00181     // dq電流制御（電流フィードバック制御）
00182 //  [vdq_ref,eI_idq] = idq_control(idq_ref,idq_act,eI_idq,ts);
00183 #ifdef USE_CURRENT_CONTROL
00184     idq_control(idq_act);
00185 #else
00186     il.vdq_ref[0] = il.idq_ref[0]/iqMAX*vdqMAX;
00187     il.vdq_ref[1] = il.idq_ref[1]/iqMAX*vdqMAX;
00188 #endif
00189     // ｄｑ軸電圧指令ベクトルの大きさをMAX制限してアンチワインドアップ対策
00190     // if( norm(vdq_ref) > vdqmax ){    vdq_ref= vdqmax/norm(vdq_ref)*vdq_ref}
00191     // anti-windup: if u=v_ref is saturated, then reduce eI. 
00192     //電圧振幅の2乗 vd^2+vq^2 を計算
00193     tmp=il.vdq_ref[0]*il.vdq_ref[0]+il.vdq_ref[1]*il.vdq_ref[1];
00194     if( tmp > SQRvdqMAX ){  // 電圧振幅の2乗がVMAXより大きいとき
00195         prev[0] = il.vdq_ref[0];    // vdを記憶
00196         prev[1] = il.vdq_ref[1];    // vqを記憶
00197         tmp = sqrt2(SQRvdqMAX/tmp); // 振幅をVMAXまで小さくする比を求める
00198         il.vdq_ref[0] = tmp*il.vdq_ref[0]; // vdにその比をかける
00199         il.vdq_ref[1] = tmp*il.vdq_ref[1]; // vqにその比をかける
00200         il.eI_idq[0] -= (prev[0]-il.vdq_ref[0])/iKId; // 振幅を小さくした分、
00201         if( il.eI_idq[0]<0 ){   il.eI_idq[0]=0;}      // I項を小さくする
00202         il.eI_idq[1] -= (prev[1]-il.vdq_ref[1])/iKIq; // q軸にも同じ処理
00203         if( il.eI_idq[1]<0 ){   il.eI_idq[1]=0;}
00204     }
00205 //#define DOUKI
00206 #ifdef DOUKI
00207 il.vdq_ref[0]=0;
00208 il.vdq_ref[1]=vdqMAX;
00209 #endif
00210 //analog_out=il.vdq_ref[1]/3.3+0.4;//koko
00211     // ｄｑ座標指令電圧 vd_ref, vq_refからiα, iβを計算
00212     // vab_ref = Cdq'*vdq_ref;
00213     vab_ref[0] = Cdq[0][0]*il.vdq_ref[0] + Cdq[1][0]*il.vdq_ref[1];
00214     vab_ref[1] = Cdq[0][1]*il.vdq_ref[0] + Cdq[1][1]*il.vdq_ref[1];
00215 //analog_out=vab_ref[1]/3.3+0.4;
00216 
00217     // モータに印加するUVW相電圧を計算 （vα, vβからvu, vv, vwを計算）
00218     //  vu = √(2/3)*va;
00219     //  vv = -1/√6*va + 1/√2*vb;
00220     //  vw = -1/√6*va - 1/√2*vb;
00221     //  p.Cuvw =[ r2/r3 -1/r2/r3 -1/r2/r3; ...
00222     //                0     1/r2 -1/r2   ];
00223     // p.vuvw = p.Cuvw'*vab_ref;
00224     p.vuvw[0] = p.Cuvw[0][0]*vab_ref[0];
00225     p.vuvw[1] = p.Cuvw[0][1]*vab_ref[0] + p.Cuvw[1][1]*vab_ref[1];
00226     p.vuvw[2] = -p.vuvw[0] - p.vuvw[1];
00227 //  p.vuvw[0] = p.Cuvw[0][0]*vab_ref[0] + p.Cuvw[1][0]*vab_ref[1];
00228 //  p.vuvw[2] = p.Cuvw[0][2]*vab_ref[0] + p.Cuvw[1][2]*vab_ref[1];
00229 }
00230 
00231 
00232 void vel_control(){
00233 //  速度制御器：速度偏差が入力され、q軸電流指令を出力。
00234 //      入力：指令速度 w_ref [rad/s], 実速度 w_lpf [rad/s], PI制御積分項 eI, サンプル時間 TS1 [s]
00235 //      出力：q軸電流指令 iq_ref [A]
00236 //       [iq_ref,eI] = vel_control(w_ref,w_lpf,eI,ts);
00237     float  e, ed;
00238 
00239     // 速度偏差の計算
00240     e = vl.w_ref - vl.w_lpf;
00241 
00242     // 速度偏差の積分値の計算
00243     vl.eI = vl.eI + TS1*e;
00244 
00245     ed = (e-vl.e_old)/TS1;          // 速度偏差の微分値の計算
00246     vl.e_old = e;                   // 速度偏差の1サンプル過去の値を更新
00247 
00248     // ＰＩ制御
00249     vl.iq_ref = wKp*e + wKi*vl.eI + wKd*ed; // ＰＩＤ制御器の出力を計算
00250 }
00251 
00252 void velocity_loop(){   // 速度制御メインループ(w_ref&beta_ref to idq_ref)
00253     float  tmp, idq_ref[2];
00254 
00255     // 速度ωを求めるために、位置θをオイラー微分して、一次ローパスフィルタに通す。
00256     tmp = p.th[0]-p.th[1];
00257     while( tmp> PI ){ tmp -= 2*PI;}
00258     while( tmp<-PI ){ tmp += 2*PI;}
00259     vl.w_lpf = iLPF*vl.w_lpf + tmp/TS0 *(1-iLPF);
00260 tmp=vl.w_lpf/(2*PI) /20; if(tmp>1) tmp=1;else if(tmp<0) tmp=0;
00261 analog_out=tmp;//tmp;//koko
00262 
00263     // 速度制御：速度偏差が入力され、q軸電流指令を出力。
00264 //  [iq_ref,eI] = vel_control(w_ref,w_act,eI,ts);
00265     vel_control();
00266 
00267     // ｑ軸電流指令のMAX制限（異常に大きい指令値を制限する）
00268     // anti-windup: if u=i_ref is saturated, then reduce eI. 
00269     if( vl.iq_ref > iqMAX ){
00270         vl.eI -= (vl.iq_ref - iqMAX)/wKi;    if( vl.eI<0 ){   vl.eI=0;}
00271         vl.iq_ref = iqMAX;
00272     }else if( vl.iq_ref < -iqMAX ){
00273         vl.eI -= (vl.iq_ref + iqMAX)/wKi;    if( vl.eI>0 ){   vl.eI=0;}
00274         vl.iq_ref = -iqMAX;
00275     }
00276 
00277     // 電流ベクトル制御
00278     if( vl.iq_ref>=0 ){ tmp = vl.tan_beta_ref;}     // 負のトルクを発生させるときはidは負のままでiqを正から負にする
00279     else{           tmp = -vl.tan_beta_ref;}// Tm = p((phi+(Ld-Lq)id) iqより
00280     //idq_ref = {{-tmp, 1}}*iq_ref;
00281     idq_ref[0] = -tmp*vl.iq_ref;    idq_ref[1] = vl.iq_ref;
00282 
00283     // ｄｑ軸電流の目標値を速度制御メインループから電流制御マイナーループへ渡す。
00284     il.idq_ref[0] = idq_ref[0];
00285     il.idq_ref[1] = idq_ref[1];
00286   if( f_find_origin==1 ){
00287     il.idq_ref[0] = iqMAX/1.0;  // idをプラス、iqをゼロにして、
00288     il.idq_ref[1] = 0;          // 無負荷のときにθ=0とさせる。
00289   }
00290 }
00291 
00292 void    vuvw2pwm(){ // vu, vv, vwより、UVW相の上アームPWMを発生
00293     float   duty_u, duty_v, duty_w;
00294 
00295     duty_u = p.vuvw[0]/vdqMAX+0.5; // dutyを計算
00296     duty_v = p.vuvw[1]/vdqMAX+0.5; // dutyを計算
00297     duty_w = p.vuvw[2]/vdqMAX+0.5; // dutyを計算
00298     uvw[0].duty = duty_u;  // dutyをPWM発生関数に渡す
00299     uvw[1].duty = duty_v;  // dutyをPWM発生関数に渡す
00300     uvw[2].duty = duty_w;  // dutyをPWM発生関数に渡す
00301 }
00302 
00303 #ifdef SIMULATION
00304 void    sim_motor(){
00305 //  モータシミュレータ
00306 //      入力信号：UVW相電圧p.vuvw [V]、負荷トルクp.TL [Nm]
00307 //      出力信号：モータ角度p.th[0] [rad], モータ速度p.w [rad/s], モータUVW相電流p.iuvw [A]
00308 //      p = motor(p, ts);   // IPM, ｄｑ座標
00309     float c, s, Cdq[2][2], idq_dot[2], id,iq, vdq[2], idq[2], Tall,TL, Cdq_inv[2][2];
00310 analog_out=p.vuvw[0]/100.+0.5;//debug
00311     // vu, vv, vwからvα, vβを計算
00312     p.vab[0] = p.Cuvw[0][0]*p.vuvw[0] + p.Cuvw[0][1]*p.vuvw[1] + p.Cuvw[0][2]*p.vuvw[2];
00313     p.vab[1] = p.Cuvw[1][0]*p.vuvw[0] + p.Cuvw[1][1]*p.vuvw[1] + p.Cuvw[1][2]*p.vuvw[2];
00314 //printf("vab=%f, %f ",p.vab[0],p.vab[1]);scanf("%f",&c);
00315 
00316     // αβ座標からｄｑ座標への変換行列Cdqの設定
00317     c = cos(p.th[0]);
00318     s = sin(p.th[0]);
00319     // Cdq =[ c s; ...
00320     //       -s c];
00321     Cdq[0][0] = c;  Cdq[0][1] = s;
00322     Cdq[1][0] =-s;  Cdq[1][1] = c;
00323 
00324     // ｖα, ｖβからｖｄ, ｖｑを計算
00325     //  vd = c*p.va + s*p.vb;
00326     //  vq =-s*p.va + c*p.vb;
00327     // vdq = Cdq * p.vab;
00328     vdq[0] = Cdq[0][0]*p.vab[0] + Cdq[0][1]*p.vab[1];
00329     vdq[1] = Cdq[1][0]*p.vab[0] + Cdq[1][1]*p.vab[1];
00330 
00331     // iα, iβからid, iqを計算
00332     //  id = c*p.ia + s*p.ib;
00333     //  iq =-s*p.ia + c*p.ib;
00334     // idq = Cdq * p.iab;
00335     idq[0] = Cdq[0][0]*p.iab[0] + Cdq[0][1]*p.iab[1];
00336     idq[1] = Cdq[1][0]*p.iab[0] + Cdq[1][1]*p.iab[1];
00337 
00338     // get id,iq
00339     //  id_dot = (1.0/p.Ld) * ( vd - (p.R*id - p.w*p.Lq*iq) );
00340     //  iq_dot = (1.0/p.Lq) * ( vq - (p.R*iq + p.w*p.Ld*id + p.w*p.phi) );
00341     // 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]);
00342     idq_dot[0] = (1.0/p.Ld) * ( vdq[0] - (p.R*idq[0] - p.w*p.Lq*idq[1]) );
00343     idq_dot[1] = (1.0/p.Lq) * ( vdq[1] - (p.R*idq[1] + p.w*p.Ld*idq[0] + p.w*p.phi) );
00344     //  id = id + ts * id_dot;
00345     //  iq = iq + ts * iq_dot;
00346     p.idq[0] = idq[0] + TS0*idq_dot[0];
00347     p.idq[1] = idq[1] + TS0*idq_dot[1];
00348     id = p.idq[0];
00349     iq = p.idq[1];
00350 
00351     // 磁気飽和を考慮したLqの計算
00352     p.Lq = p.Lq0 + p.Lq1*abs(iq);
00353     if( p.Lq < p.Ld )
00354         p.Lq = p.Ld;
00355 
00356     // モータトルクの計算
00357     p.Tm = p.p * (p.phi + (p.Ld-p.Lq)*id) * iq;
00358 
00359     // モータ速度ωの計算
00360     if( abs(p.w) > 5*2*PI )
00361         if( p.w>=0 )    TL= p.TL;
00362         else            TL=-p.TL;
00363     else
00364         TL = p.w/(5*2*PI)*p.TL;
00365 
00366     Tall = p.Tm - TL;
00367     p.w = p.w + TS0 * (1.0 / p.Jm) * Tall;
00368 
00369     // モータ角度θの計算
00370     p.th[0] = p.th[0] + TS0 * p.w;
00371     if( p.th[0]>4*PI)
00372         p.th[0] = p.th[0] - 4*PI;
00373 
00374     if( p.th[0]<0 )
00375         p.th[0] = p.th[0] + 4*PI;
00376 
00377     // ｄｑ座標からαβ座標への変換行列Cdq_invの設定
00378     c = cos(p.th[0]);
00379     s = sin(p.th[0]);
00380     // Cdq_inv =[ c -s; ...
00381     //            s c];
00382     Cdq_inv[0][0] = c;  Cdq_inv[0][1] =-s;
00383     Cdq_inv[1][0] = s;  Cdq_inv[1][1] = c;
00384 
00385     // id, iqからiα, iβを計算
00386     //p.iab = Cdq_inv * p.idq;
00387     p.iab[0] = Cdq_inv[0][0]*p.idq[0] + Cdq_inv[0][1]*p.idq[1];
00388     p.iab[1] = Cdq_inv[1][0]*p.idq[0] + Cdq_inv[1][1]*p.idq[1];
00389 
00390     // αβ座標からUVW座標への変換行列Cuvw_inv=Cuvw'
00391     // iα, iβからiu, iv, iwを計算
00392     //  iu = r2/r3*ia;
00393     //  iv = -1/r2/r3*ia + 1/r2*ib;
00394     //  iw = -1/r2/r3*ia - 1/r2*ib;
00395     //p.iuvw = p.Cuvw' * p.iab;
00396     p.iuvw[0] = p.Cuvw[0][0]*p.iab[0] + p.Cuvw[1][0]*p.iab[1];
00397     p.iuvw[1] = p.Cuvw[0][1]*p.iab[0] + p.Cuvw[1][1]*p.iab[1];
00398     p.iuvw[2] = p.Cuvw[0][2]*p.iab[0] + p.Cuvw[1][2]*p.iab[1];
00399 }
00400 #endif
00401 
00402 void data2mbedUSB(){    // save data to mbed USB drive 
00403     if( _count_data<1000 ){
00404         data[_count_data][0]=p.th[0]/*vl.w_ref*/; data[_count_data][1]=p.vuvw[0];
00405         data[_count_data][2]=vl.w_lpf; data[_count_data][3]=_count*TS0; data[_count_data][4]=il.vdq_ref[1];
00406         _count_data++;
00407     }
00408 #if 0
00409   if( _count_data<500 ){
00410     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];
00411     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];
00412     debug[10]=p.iuvw[1]; debug[11]=p.iuvw[2]; debug[12]=p.idq[0]; debug[13]=p.idq[1]; debug[14]=p.TL;
00413     debug[15]=p.Tm; debug[16]=p.w; debug[17]=vl.w_lpf; debug[18]=p.th[0]; debug[19]=_count*TS0;//_time;
00414 //BUG    for(j=0;j<19;j++){  fprintf( fp, "%f, ",debug[j]);} fprintf( fp, "%f\n",debug[19]);
00415     for(j=0;j<19;j++){  printf("%f, ",debug[j]);} printf("%f\n",debug[19]);
00416 //    for(j=0;j<19;j++){  pc.printf("%f, ",debug[j]);} pc.printf("%f\n",debug[19]);
00417   }
00418 #endif
00419 }
00420 void timerTS0(){    // timer called every TS0[s].
00421 //    debug_p26 = 1;
00422     _count++;
00423     _time += TS0;
00424     
00425     p.th[1] = p.th[0];  // thを更新
00426  #ifdef SIMULATION
00427     // モータシミュレータ
00428     sim_motor();    // IPM, ｄｑ座標
00429  #else
00430 #ifdef DOUKI
00431 led1=1;
00432 p.th[0] += 2*PI*TS0 * 1; if(p.th[0]>4*PI){   p.th[0]-=4*PI;}
00433 //debug[0]=p.th[0]/PI*180;
00434 analog_out=debug[0]/180*PI/4/PI;
00435 led1=0;
00436 #else
00437     // 位置θをセンサで検出
00438     p.th[0] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI;   // get angle [rad] from encoder
00439 debug[0]=p.th[0]/PI*180;
00440 debug[1]=p.th[0]/(2*PI);    debug[1]=debug[1]-(int)debug[1];    if(debug[1]<0)  debug[1]+=1;
00441 debug[0]=debug[1]*360;
00442 //analog_out=debug[1];
00443 #endif
00444  #endif
00445     current_loop(); // 電流制御マイナーループ(idq_ref to vuvw)
00446     vuvw2pwm();     // vuvw to pwm
00447 //    debug_p26 = 0;
00448 }
00449 void timerTS1(void const *argument){
00450 //    debug_p25 = 1;
00451     velocity_loop();    // 速度制御メインループ(w_ref&beta_ref to idq_ref)
00452 //    debug_p25 = 0;
00453 }
00454 
00455 void display2PC(){  // display to tera term on PC
00456     pc.printf("%8.1f[s]\t%8.5f[V]\t%8.2f [Hz]\t%8.2f\t%8.2f\r\n", 
00457         _time, il.vdq_ref[1], vl.w_lpf/(2*PI), vl.w_ref/(2*PI), debug[0]);  // print to tera term
00458 //    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
00459 }
00460 void timerTS2(){
00461 }
00462 void timerTS3(){
00463     data2mbedUSB();  // data2mbedUSB() is called every TS3[s].
00464 }
00465 void timerTS4(){
00466     display2PC();  // display to tera term on PC. display2PC() is called every TS4[s].
00467 }