UVW 3 phases Brushless DC motor control
Dependencies: QEI mbed-rtos mbed
Fork of BLDCmotor by
controller.cpp
- Committer:
- kosakaLab
- Date:
- 2013-09-07
- Revision:
- 17:1ac855d69c78
- Parent:
- 16:d05404eef8ee
File content as of revision 17:1ac855d69c78:
// controller.cpp: 各種3相同期モータに対するセンサあり運転のシミュレーション // 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; // q軸電流指令[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]){ // dq座標電流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]; // dq電流偏差の計算 e[0] = il.idq_ref[0] - idq_act[0]; e[1] = il.idq_ref[1] - idq_act[1]; // dq電流偏差の積分項の計算 il.eI_idq[0] = il.eI_idq[0] + TS0*e[0]; il.eI_idq[1] = il.eI_idq[1] + TS0*e[1]; // dq電流偏差の微分値の計算 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]; } // αβ座標からdq座標への変換行列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); // αβ座標電流をdq座標電流に変換 //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 // dq軸電圧指令ベクトルの大きさを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 // dq座標指令電圧 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サンプル過去の値を更新 // PI制御 vl.iq_ref = wKp*e + wKi*vl.eI + wKd*ed; // PID制御器の出力を計算 } 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(); // q軸電流指令の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; // dq軸電流の目標値を速度制御メインループから電流制御マイナーループへ渡す。 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, dq座標 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); // αβ座標からdq座標への変換行列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; // vα, vβからvd, vqを計算 // 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; // dq座標からαβ座標への変換行列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, dq座標 #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]. }