manabu kosaka
UVW 3 phases Brushless DC motor control
Dependencies: QEI mbed-rtos mbed
Fork of
DCmotor
by 
manabu kosaka
Diff: controller.cpp
- Revision:
- 13:791e20f1af43
- Parent:
- 12:a4b17bb682eb
--- a/controller.cpp Fri Dec 21 22:06:56 2012 +0000
+++ b/controller.cpp Sun Mar 03 09:09:34 2013 +0000
@@ -1,5 +1,5 @@
-// BLDCmotor.cpp: 各種３相同期モータに対するセンサあり運転のシミュレーション
-// Kosaka Lab. 121215
+// controller.cpp: 各種３相同期モータに対するセンサあり運転のシミュレーション
+// Kosaka Lab. 130214
#include "mbed.h"
#include "QEI.h"
@@ -31,25 +31,9 @@
/*********** 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);
@@ -57,15 +41,14 @@
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
+
+//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.
@@ -101,13 +84,12 @@
p.R = 0.143; // Ω
p.phi = 0.176; // V s
p.Jm = 0.00018; // Nms^2
+ p.p = 2; // 極対数
#endif
- p.th[0] = 0;
- p.th[1] = 0;
+ 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β];
- p.p = 2; // 極対数
// UVW座標からαβ座標への変換行列Cuvwの設定
r2 = sqrt(2.);//1.414213562373095;//2^(1/2);
r3 = sqrt(3.);//1.732050807568877;//3^(1/2);
@@ -121,22 +103,23 @@
// 制御器の初期化
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; // 電流制御用偏差の積分値（積分項）
+ 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, サンプル時間 ts [s]
-// 出力：αβ軸電圧指令 vdq_ref [A]
+// 入力：指令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 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)
+ float e[2], ed[2];
// ｄｑ電流偏差の計算
e[0] = il.idq_ref[0] - idq_act[0];
@@ -146,23 +129,24 @@
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] = 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
+ 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;
+ 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 = 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];
}
@@ -177,11 +161,6 @@
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 を計算
@@ -204,18 +183,30 @@
#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];
+ il.vdq_ref[0] = il.idq_ref[0]/iqMAX*vdqMAX;
+ il.vdq_ref[1] = il.idq_ref[1]/iqMAX*vdqMAX;
#endif
- // ｄｑ軸電圧指令ベクトルの大きさをMAX制限（コンバータ出力電圧値に設定）
+ // ｄｑ軸電圧指令ベクトルの大きさを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
+ // 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
// ｄｑ座標指令電圧 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];
@@ -233,8 +224,6 @@
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を更新
}
@@ -242,21 +231,20 @@
// 速度制御器：速度偏差が入力され、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ゲイン
+// [iq_ref,eI] = vel_control(w_ref,w_lpf,eI,ts);
+ float e, ed;
// 速度偏差の計算
e = vl.w_ref - vl.w_lpf;
// 速度偏差の積分値の計算
- vl.eI_w = vl.eI_w + TS1*e;
+ vl.eI = vl.eI + TS1*e;
+
+ ed = (e-vl.e_old)/TS1; // 速度偏差の微分値の計算
+ vl.e_old = e; // 速度偏差の1サンプル過去の値を更新
// ＰＩ制御
- vl.iq_ref = Kp*e + Ki*vl.eI_w;
-// koko anti-windup
+ vl.iq_ref = wKp*e + wKi*vl.eI + wKd*ed; // ＰＩＤ制御器の出力を計算
}
void velocity_loop(){ // 速度制御メインループ(w_ref&beta_ref to idq_ref)
@@ -266,16 +254,19 @@
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;
+ vl.w_lpf = iLPF*vl.w_lpf + tmp/TS0 *(1-iLPF);
// 速度制御：速度偏差が入力され、q軸電流指令を出力。
-// [iq_ref,eI_w] = vel_control(w_ref,w_act,eI_w,ts);
+// [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;
}
@@ -293,12 +284,12 @@
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;
+ 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
@@ -419,26 +410,38 @@
#endif
}
void timerTS0(){ // timer called every TS0[s].
- debug_p26 = 1;
+// debug_p26 = 1;
_count++;
_time += TS0;
- current_loop(); // 電流制御マイナーループ(idq_ref to vuvw)
- vuvw2pwm(); // vuvw to pwm
+ p.th[1] = p.th[0]; // thを更新
#ifdef SIMULATION
// モータシミュレータ
sim_motor(); // IPM, ｄｑ座標
+ #else
+//koko p.th[0] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI; // get angle [rad] from encoder
+ // 位置θをセンサで検出
+#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;
+#endif
#endif
- debug_p26 = 0;
+ current_loop(); // 電流制御マイナーループ(idq_ref to vuvw)
+ vuvw2pwm(); // vuvw to pwm
+// debug_p26 = 0;
}
void timerTS1(void const *argument){
- debug_p25 = 1;
+// debug_p25 = 1;
velocity_loop(); // 速度制御メインループ(w_ref&beta_ref to idq_ref)
- debug_p25 = 0;
+// 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%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(){