Vector_math.cpp

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