Kosaka Lab
UVW 3 phases Brushless DC motor control
Dependencies: QEI mbed-rtos mbed
Fork of
BLDCmotor
by 
manabu kosaka
Diff: controller.cpp
- Revision:
- 12:a4b17bb682eb
- Child:
- 13:791e20f1af43
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/controller.cpp Fri Dec 21 22:06:56 2012 +0000
@@ -0,0 +1,451 @@
+// BLDCmotor.cpp: 各種３相同期モータに対するセンサあり運転のシミュレーション
+// Kosaka Lab. 121215
+#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);
+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 _r; // reference signal
+float _th=0; // [rad], motor angle, control output of angle controller
+float _i=0; // [A], motor current, control output of current controller
+float _e=0; // e=r-y for PID controller
+float _eI=0; // integral of e for PID controller
+float _iref; // reference current iref [A], output of angle th_contorller
+float _u; // control input[V], motor input volt.
+float _ei=0; // e=r-y for current PID controller
+float _eiI=0; // integral of e for current PID controller
+unsigned char _f_u_plus=1;// sign(u)
+unsigned char _f_umax=0;// flag showing u is max or not
+unsigned char _f_imax=0;// flag showing i is max or not
+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の機器定数等の設定, 制御器の初期化
+ 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
+#endif
+ p.th[0] = 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β];
+ p.p = 2; // 極対数
+ // 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_w = 0; // 速度制御用偏差の積分値（積分項）
+ il.eI_idq[0] = 0; // 電流制御用偏差の積分値（積分項）
+ il.eI_idq[1] = 0; // 電流制御用偏差の積分値（積分項）
+}
+
+void idq_control(float idq_act[2]){
+// ｄｑ座標電流PID制御器（電流マイナーループのフィードバック制御）
+// 入力：指令dq座標電流 idq_ref [A], 実dq座標電流 idq_act [A], PI制御積分項 eI, サンプル時間 ts [s]
+// 出力：αβ軸電圧指令 vdq_ref [A]
+// [vdq_ref,eI_idq] = idq_control(idq_ref,idq_act,eI_idq,ts);
+ float Kp_d, Kp_q, Ki_d, Ki_q, e[2];
+ // 電流制御ゲイン
+ Kp_d = iKPd; // P gain (d-axis)
+ Ki_d = iKId; // I gain (d-axis)
+ Kp_q = iKPq; // P gain (q-axis)
+ Ki_q = iKIq; // I gain (q-axis)
+
+ // ｄｑ電流偏差の計算
+ 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];
+
+ // ＰＩ制御
+ // vdq_ref = [Kp_d 0;0 Kp_q]*e + [Ki_d 0;0 Ki_q]*eI;
+ il.vdq_ref[0] = Kp_d*e[0] + Ki_d*il.eI_idq[0];
+ il.vdq_ref[1] = Kp_q*e[1] + Ki_q*il.eI_idq[1];
+
+// koko anti-windup
+}
+
+void current_loop(){ // 電流制御マイナーループ
+ float th, c, s, Cdq[2][2], iu, iv, iab[2], idq_act[2], vab_ref[2],tmp;
+ if( f_find_origin==1 ){
+ th = 0;
+ }else{
+ // 位置θをセンサで検出
+#ifndef SIMULATION
+ p.th[0] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI; // get angle [rad] from encoder
+#endif
+ 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, iv を検出
+#ifndef SIMULATION
+ p.iuvw[0] = (VshuntR_Uplus - VshuntR_Uminus) /R_SHUNT; // get iu [A] from shunt resistance;
+ p.iuvw[1] = (VshuntR_Vplus - VshuntR_Vminus) /R_SHUNT; // get iv [A] from shunt resistance;
+#endif
+ 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];
+ il.vdq_ref[1] = il.idq_ref[1];
+#endif
+ // ｄｑ軸電圧指令ベクトルの大きさをMAX制限（コンバータ出力電圧値に設定）
+ // if( norm(vdq_ref) > vdqmax ){ vdq_ref= vdqmax/norm(vdq_ref)*vdq_ref}
+ if( (tmp=il.vdq_ref[0]*il.vdq_ref[0]+il.vdq_ref[1]*il.vdq_ref[1])>SQRvdqMAX ){
+ tmp = sqrt2(SQRvdqMAX/tmp);
+ il.vdq_ref[0] = tmp*il.vdq_ref[0]; //= vdqmax/norm(vdq_ref)*vdq_ref
+ il.vdq_ref[1] = tmp*il.vdq_ref[1];
+// koko anti-windup
+ }
+
+ // ｄｑ座標指令電圧 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];
+
+ // モータに印加する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];
+
+ p.th[1] = p.th[0]; // thを更新
+}
+
+
+void vel_control(){
+// 速度制御器：速度偏差が入力され、q軸電流指令を出力。
+// 入力：指令速度 w_ref [rad/s], 実速度 w_lpf [rad/s], PI制御積分項 eI, サンプル時間 TS1 [s]
+// 出力：q軸電流指令 iq_ref [A]
+// [iq_ref,eI_w] = vel_control(w_ref,w_lpf,eI_w,ts);
+ float Kp, Ki, e;
+ // 速度制御PIDゲイン
+ Kp = wKp; // 速度制御PIDのPゲイン
+ Ki = wKi; // 速度制御PIDのIゲイン
+
+ // 速度偏差の計算
+ e = vl.w_ref - vl.w_lpf;
+
+ // 速度偏差の積分値の計算
+ vl.eI_w = vl.eI_w + TS1*e;
+
+ // ＰＩ制御
+ vl.iq_ref = Kp*e + Ki*vl.eI_w;
+// koko anti-windup
+}
+
+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 = (1-iLPF)*vl.w_lpf + tmp/TS0 *iLPF;
+
+ // 速度制御：速度偏差が入力され、q軸電流指令を出力。
+// [iq_ref,eI_w] = vel_control(w_ref,w_act,eI_w,ts);
+ vel_control();
+
+ // ｑ軸電流指令のMAX制限（異常に大きい指令値を制限する）
+ if( vl.iq_ref > iqMAX ){
+ vl.iq_ref = iqMAX;
+ }else if( vl.iq_ref < -iqMAX ){
+ 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];
+}
+
+void vuvw2pwm(){ // vu, vv, vwより、UVW相の上アームPWMを発生
+ float duty_u, duty_v, duty_w;
+
+ duty_u = (p.vuvw[0]/vdqMAX+1)*0.5;
+ duty_v = (p.vuvw[1]/vdqMAX+1)*0.5;
+ duty_w = (p.vuvw[2]/vdqMAX+1)*0.5;
+ uvw[0].duty = duty_u;
+ uvw[1].duty = duty_v;
+ uvw[2].duty = duty_w;
+}
+
+#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;
+
+ current_loop(); // 電流制御マイナーループ(idq_ref to vuvw)
+ vuvw2pwm(); // vuvw to pwm
+ #ifdef SIMULATION
+ // モータシミュレータ
+ sim_motor(); // IPM, ｄｑ座標
+ #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.5f[V]\t%4d [Hz]\t%d\r\n", _time, il.vdq_ref[0], (int)(vl.w_lpf/(2*PI)+0.5), (int)(vl.w_ref/(2*PI)+0.5)); // 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].
+}