Kosaka Lab
UVW 3 phases Brushless DC motor control
Dependencies: QEI mbed-rtos mbed
Fork of
BLDCmotor
by 
manabu kosaka
Revision 15:427f5ae8e957, committed 2013-09-03
- Comitter:
- kosakaLab
- Date:
- Tue Sep 03 07:40:50 2013 +0000
- Parent:
- 14:8e205264baa8
- Child:
- 16:d05404eef8ee
- Commit message:
- ?????????; Forced synchronous operation is added.
Changed in this revision
--- a/UVWpwm.cpp Thu Jun 13 06:41:16 2013 +0000
+++ b/UVWpwm.cpp Tue Sep 03 07:40:50 2013 +0000
@@ -21,6 +21,7 @@
// 関数配列: NG
//void (*pwmUVWout[])(int) = {pwmout,pwmout,pwmout};
// pwmUVWout[i](i);
+void pwmVout(), pwmWout();
#if PWM_WAVEFORM==0 // 0: saw tooth wave comparison
void pwmUout() { // タイムアウト関数でU相PWMを発生する関数
@@ -39,6 +40,7 @@
uvw[i].lower_us = 1000000/PWM_FREQ -uvw[i].upper_us - 2*DEADTIME_US;
// 時間幅が小さいときはTMINにする
if( uvw[i].lower_us < TMIN ){ uvw[i].lower_us=TMIN;}
+ pwmVout(); pwmWout();
}else if( uvw[i].mode==2 ){ // モードが２のとき、デッドタイムをつくる
// Tdt[μs]経過してからタイムアウトでこの関数自身をコール
pwm[i].attach_us(&pwmUout, DEADTIME_US);
@@ -86,7 +88,7 @@
pwm_upper[i] = 0;
pwm_lower[i] = 1;
}else{// if( u.mode==4 ){
- pwm[i].attach_us(&pwmVout, DEADTIME_US); // setup pwmU to call pwmUout after t [us]
+// pwm[i].attach_us(&pwmVout, DEADTIME_US); // setup pwmU to call pwmUout after t [us]
pwm_upper[i] = 0;
pwm_lower[i] = 0;
uvw[i].mode = 0;
@@ -113,7 +115,7 @@
pwm_upper[i] = 0;
pwm_lower[i] = 1;
}else{// if( u.mode==4 ){
- pwm[i].attach_us(&pwmWout, DEADTIME_US); // setup pwmU to call pwmUout after t [us]
+// pwm[i].attach_us(&pwmWout, DEADTIME_US); // setup pwmU to call pwmUout after t [us]
pwm_upper[i] = 0;
pwm_lower[i] = 0;
uvw[i].mode = 0;
@@ -242,8 +244,10 @@
uvw[i].mode = 0;
}
pwmUout();
+#if PWM_WAVEFORM == 1
pwmVout();
pwmWout();
+#endif
}
void stop_pwm(){
--- a/UVWpwm.h Thu Jun 13 06:41:16 2013 +0000 +++ b/UVWpwm.h Tue Sep 03 07:40:50 2013 +0000 @@ -10,7 +10,7 @@ #define V_LOWER_PORT p24 // V相下アームVd用ポート #define W_UPPER_PORT p25 // W相上アームWu用ポート #define W_LOWER_PORT p26 // W相下アームWd用ポート -#define PWM_WAVEFORM 1 // 0: saw tooth wave comparison, 1: triangler wave comparison +#define PWM_WAVEFORM 1 // 0: saw tooth wave comparison, 1: triangler wave comparison // koko if 0, no round #define TMIN 3 // [us], processing time of pwm_out() #define R_SHUNT_UP_PORT p16 // ポート：U相電流検出用抵抗の+側アナログ入力
--- a/controller.cpp Thu Jun 13 06:41:16 2013 +0000
+++ b/controller.cpp Tue Sep 03 07:40:50 2013 +0000
@@ -1,5 +1,5 @@
// controller.cpp: 各種３相同期モータに対するセンサあり運転のシミュレーション
-// Kosaka Lab. 130214
+// Kosaka Lab. 130903
#include "mbed.h"
#include "QEI.h"
@@ -145,8 +145,7 @@
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 = 0;
- th = debug[2];
+ th = p.th_const;
}else{
th = p.th[0];
}
@@ -208,10 +207,12 @@
il.vdq_ref[0]=0;
il.vdq_ref[1]=vdqMAX;
#endif
+//analog_out=il.vdq_ref[1]/3.3+0.4;//koko
// ｄｑ座標指令電圧 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;
@@ -256,6 +257,8 @@
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);
@@ -280,6 +283,10 @@
// ｄｑ軸電流の目標値を速度制御メインループから電流制御マイナーループへ渡す。
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を発生
@@ -424,12 +431,16 @@
#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;
+//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)
--- a/controller.h Thu Jun 13 06:41:16 2013 +0000
+++ b/controller.h Tue Sep 03 07:40:50 2013 +0000
@@ -3,12 +3,13 @@
//#define PI 3.14159265358979 // def. of PI
/*********** User setting for control parameters (begin) ***************/
-#define SIMULATION // Comment this line if not simulation
+//#define SIMULATION // Comment this line if not simulation
//#define USE_CURRENT_CONTROL // Current control on. Comment if current control off.
#define DEADZONE_PLUS 1. // deadzone of plus side
#define DEADZONE_MINUS -1.5 // deadzone of minus side
// encoder
-#define N_ENC (24*4) // "*4": QEI::X4_ENCODING. Number of pulses in one revolution(=360 deg) of rotary encoder.
+#define N_ENC (360*4) // "*4": QEI::X4_ENCODING. Number of pulses in one revolution(=360 deg) of rotary encoder.
+//#define N_ENC (24*4) // "*4": QEI::X4_ENCODING. Number of pulses in one revolution(=360 deg) of rotary encoder.
#define CH_A p29 // A phase port
#define CH_B p30 // A phase port
@@ -25,8 +26,8 @@
// RtosTimer RtosTimerTS1(timerTS1); // RtosTimer priority is osPriorityAboveNormal, just one above main()
// Thread ThreadTimerTS3(CallTimerTS3,NULL,osPriorityBelowNormal);
// Thread ThreadTimerTS4(CallTimerTS4,NULL,osPriorityLow);
-#define TMAX 3.0 // [s], experiment starts from 0[s] to TMAX[s]
-#define TMAX_FIND_ORIGIN 30//0.1//1.0 // [s], finding th origin starts from 0[s] to TMAX[s]
+#define TMAX 15 // [s], experiment starts from 0[s] to TMAX[s]
+#define TMAX_FIND_ORIGIN 9//0.1//1.0 // [s], finding th origin starts from 0[s] to TMAX[s]
// 電流制御マイナーループ
#define iKPd 10./2 // 電流制御d軸PIDのPゲイン (d-axis)
@@ -45,12 +46,14 @@
#define wKi 2.50 // 速度制御PIDのIゲイン
#define wKd 0 // 速度制御PIDのDゲイン
#else
- #define wKp 0.005 // 速度制御PIDのPゲイン
- #define wKi 0.2 // 速度制御PIDのIゲイン
+ #define wKp 0.05 // 速度制御PIDのPゲイン
+ #define wKi 0.05 // 速度制御PIDのIゲイン
+// #define wKp 0.005 // 速度制御PIDのPゲイン
+// #define wKi 0.2 // 速度制御PIDのIゲイン
#define wKd 0 // 速度制御PIDのDゲイン
#endif
-#define iLPF 0.9 // 0-1, 速度に対する1次LPF; Low Pass Filter, G(z)=(1-a)/(z-a)
+#define iLPF 0.99//0.95//0.9 // 0-1, 速度に対する1次LPF; Low Pass Filter, G(z)=(1-a)/(z-a)
#define iqMAX 4//100 // [A], ｑ軸電流指令のMAX制限（異常に大きい指令値を制限する）
/*********** User setting for control parameters (end) ***************/
@@ -69,7 +72,8 @@
float Tm; // [Nm], モータトルク
float TL; // [Nm], 負荷トルク
#endif
- float th[2]; // [rad]. ロータの位置, th[0]=th(t), th[1]=th(t-TS0)
+ float th[2]; // [rad], ロータの位置, th[0]=th(t), th[1]=th(t-TS0)
+ float th_const;// [rad], ロータの位置 with constant angular velosity
float w; // [rad/s], モータ速度
float w_lpf; // [rad/s], フィルタで高周波ノイズを除去したモータ速度
float iab[2]; // [A], αβ軸電流 iab = [iα;iβ];
--- a/main.cpp Thu Jun 13 06:41:16 2013 +0000
+++ b/main.cpp Tue Sep 03 07:40:50 2013 +0000
@@ -1,5 +1,5 @@
// UVW three phases Blushless DC motor control program using 3 H-bridge driver (ex. TA7291P) and incremental rotary encoder with A, B phase.
-// ver. 130214 by Kosaka lab.
+// ver. 130903 by Kosaka lab.
#include "mbed.h"
#include "rtos.h"
@@ -64,7 +64,7 @@
int main(){
unsigned short i, i2=0;
FILE *fp = fopen("/mbedUSBdrive/data.csv", "w"); // save data to PC
- char chr[100];
+// char chr[100];
RtosTimer RtosTimerTS1(timerTS1); // RtosTimer priority is osPriorityAboveNormal, just one above main()
Thread ThreadTimerTS3(CallTimerTS3,NULL,osPriorityBelowNormal);
Thread ThreadTimerTS4(CallTimerTS4,NULL,osPriorityLow);
@@ -80,12 +80,14 @@
// 指令速度
// float w_ref_req[2] = {20* 2*PI, 40* 2*PI}; // [rad/s]（第2要素は指令速度急変後の指令速度）
- float w_ref_req[2] = {0.5* 2*PI, 0.5* 2*PI}; // [rad/s]（第2要素は指令速度急変後の指令速度）
+ float w_ref_req[2] = {10* 2*PI, 10* 2*PI}; // [rad/s]（第2要素は指令速度急変後の指令速度）
// 指令ｄｑ電流位相
- float beta_ref = 30*PI/180; // [rad], 電流位相指令βを30度に
+// float beta_ref = 30*PI/180; // [rad], 電流位相指令βを30度に
+ float beta_ref = 85*PI/180; // [rad], 電流位相指令βを30度に
float tan_beta_ref1;
float tan_beta_ref2,tan_beta_ref;
float t; // current time
+ extern void velocity_loop();
// start_timers(1); // start multi-timers, sampling times are TS0, TS1, TS2, TS3, TS4.
@@ -106,7 +108,7 @@
// start control (ON)
start_pwm();
TickerTimerTS0.attach(&timerTS0, TS0 ); // Sampling period[s] of i_controller
-// RtosTimerTS1.start((unsigned int)(TS1*1000.)); // Sampling period[ms] of th controller
+ RtosTimerTS1.start((unsigned int)(TS1*1000.)); // Sampling period[ms] of th controller
fTimerTS3ON = 1; // timerTS3 start
fTimerTS4ON = 1; // timerTS3 start
#endif
@@ -114,24 +116,17 @@
// set th by moving rotor to th=zero.
f_find_origin=1; // 回転角原点復帰フラグをセット
while( (t = _count*TS0) < TMAX_FIND_ORIGIN ){ // TMAX秒経過するまで制御実行
-// debug_p24 = 1;
- il.idq_ref[0] = iqMAX/1.0; // idをプラス、iqをゼロにして、
- il.idq_ref[1] = 0; // 無負荷のときにθ=0とさせる。
-
-//pc2.scanf("%f",&debug[2]);
-debug[2]=3*2*PI*t*1;
+ if (t<1) p.th_const = 0*2*PI*t*1; // Set theta with constant angular velosity
+ else if(t<3) p.th_const = 9*2*PI*t*1; // Set theta with constant angular velosity
+ else if(t<5) p.th_const = 15*2*PI*t*1; // Set theta with constant angular velosity
+ else if(t<TMAX_FIND_ORIGIN-2)
+ p.th_const = 9*2*PI*t*1; // Set theta with constant angular velosity
+ else p.th_const = 0*2*PI*t*1; // Set theta with constant angular velosity
#ifdef OLD
timerTS0();
- //current_loop(); // 電流制御マイナーループ(idq_ref to vuvw)
- //vuvw2pwm(); // vuvw to pwm
- //sim_motor(); // IPM, ｄｑ座標
#endif
-
-// if( (ms=(int)(TS1*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;}// ms=TS0
-// debug_p24 = 0;
Thread::wait(1);
}
-//pc2.printf("\r\n^0^ 0\r\n");
// IPMSMの機器定数等の設定, 制御器の初期化
init_parameters();
f_find_origin=0;
@@ -159,12 +154,16 @@
// 目標電流位相をメインルーチンから速度制御メインループへ渡す。
vl.tan_beta_ref = tan_beta_ref;
+#if 0
// 速度急変
if( 0.26<=t && t<0.3 ){
vl.w_ref=w_ref_req[1]; // 目標速度をメインルーチンから速度制御メインループへ渡す。
}else{
vl.w_ref=w_ref_req[0];
}
+#else
+ vl.w_ref=0.999*vl.w_ref + (1-0.999)*w_ref_req[0];
+#endif
#ifdef SIMULATION
// 負荷トルク急変
if( t<0.4 ){