UVW 3 phases Brushless DC motor control
Dependencies: QEI mbed-rtos mbed
Fork of BLDCmotor by
main.cpp
- Committer:
- kosakaLab
- Date:
- 2013-09-07
- Revision:
- 17:1ac855d69c78
- Parent:
- 15:427f5ae8e957
File content as of revision 17:1ac855d69c78:
// UVW three phases Blushless DC motor control program using 3 H-bridge driver (ex. TA7291P) and incremental rotary encoder with A, B phase. // ver. 130903 by Kosaka lab. #include "mbed.h" #include "rtos.h" #include "fast_math.h" #include "controller.h" #include "UVWpwm.h" Serial pc2(USBTX, USBRX); // Display on tera term in PC LocalFileSystem local("mbedUSBdrive"); // save data to mbed USB disk drive in PC //Semaphore semaphore1(1); // wait and release to protect memories and so on //Mutex stdio_mutex; // wait and release to protect memories and so on Ticker TickerTimerTS0; // Timer interrupt using TIMER3, TS<0.001 is OK. Priority is higher than rtosTimer. unsigned char fTimerTS2ON=0, fTimerTS3ON=0, fTimerTS4ON=0; // ON/OFF flag for timerTS2, 3, 4. extern "C" void mbed_reset(); void CallTimerTS2(void const *argument) { // make sampling time TS3 timer (priority 3: precision 4ms) int ms; unsigned long c; while (true) { c = _count; if( fTimerTS2ON ){ timerTS2(); // timerTS2() is called every TS2[s]. } if( (ms=(int)(TS2*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;} Thread::wait(ms); } } void CallTimerTS3(void const *argument) { // make sampling time TS3 timer (priority 3: precision 4ms) int ms; unsigned long c; while (true) { c = _count; if( fTimerTS3ON ){ timerTS3(); // timerTS3() is called every TS3[s]. } if( (ms=(int)(TS3*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;} Thread::wait(ms); } } void CallTimerTS4(void const *argument) { // make sampling time TS4 timer (priority 4: precision 4ms) int ms; unsigned long c; while (true) { c = _count; if( fTimerTS4ON ){ timerTS4(); // timerTS4() is called every TS4[s]. } if( (ms=(int)(TS4*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;} Thread::wait(ms); } } //void stop_fprintf(){ // emergencyStop if fprintf keep busy for 3 secons // fclose(fp); // pc2.printf("error: fprintf was in hung-up!"); //} //#define OLD int main(){ unsigned short i, i2=0; FILE *fp = fopen("/mbedUSBdrive/data.csv", "w"); // save data to PC // char chr[100]; RtosTimer RtosTimerTS1(timerTS1); // RtosTimer priority is osPriorityAboveNormal, just one above main() Thread ThreadTimerTS3(CallTimerTS3,NULL,osPriorityBelowNormal); Thread ThreadTimerTS4(CallTimerTS4,NULL,osPriorityLow); // Priority of Thread (RtosTimer is osPriorityAboveNormal) // osPriorityIdle = -3, ///< priority: idle (lowest)--> then, mbed ERROR!! // osPriorityLow = -2, ///< priority: low // osPriorityBelowNormal = -1, ///< priority: below normal // osPriorityNormal = 0, ///< priority: normal (default) // osPriorityAboveNormal = +1, ///< priority: above normal // osPriorityHigh = +2, ///< priority: high // osPriorityRealtime = +3, ///< priority: realtime (highest) // osPriorityError = 0x84 ///< system cannot determine priority or thread has illegal priority // 指令速度 // float w_ref_req[2] = {20* 2*PI, 40* 2*PI}; // [rad/s](第2要素は指令速度急変後の指令速度) float w_ref_req[2] = {5* 2*PI, 10* 2*PI}; // [rad/s](第2要素は指令速度急変後の指令速度) // 指令dq電流位相 // float beta_ref = 30*PI/180; // [rad], 電流位相指令βを30度に float beta_ref = 35*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. // IPMSMの機器定数等の設定, 制御器の初期化 init_parameters(); p.th[0] = 2*PI/3; //θの初期値 // p.Lq0 = p.Ld; // SPMSMの場合 // p.phi = 0; // SynRMの場合 // w_ref=p.w; // 速度指令[rad/s] tan_beta_ref1 = tan(beta_ref); // tan30°を計算 tan_beta_ref2 = tan(beta_ref-30*PI/180); // tan0°を計算 tan_beta_ref = tan_beta_ref1; // βの指令値をtan30°に // シミュレーション開始 pc2.printf("Simulation start!!\r\n"); #ifndef OLD // 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 fTimerTS3ON = 1; // timerTS3 start fTimerTS4ON = 1; // timerTS3 start #endif // set th by moving rotor to th=zero. f_find_origin=1; // 回転角原点復帰フラグをセット while( (t = _count*TS0) < TMAX_FIND_ORIGIN ){ // TMAX秒経過するまで制御実行 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(); #endif Thread::wait(1); } // IPMSMの機器定数等の設定, 制御器の初期化 init_parameters(); f_find_origin=0; #ifndef OLD TickerTimerTS0.detach(); // timer interrupt stop // start control (ON) TickerTimerTS0.attach(&timerTS0, TS0 ); // Sampling period[s] of i_controller RtosTimerTS1.start((unsigned int)(TS1*1000.)); // Sampling period[ms] of th controller #endif while( (t = _count*TS0-TMAX_FIND_ORIGIN) < TMAX ){ // debug_p24 = 1; // 電流位相(dq軸電流)を変化させるベクトル制御 if( t>=0.15 && t<0.19 ){ // 0.15~0.19秒の間に if( tan_beta_ref>tan_beta_ref2 ){ // βの指令値が0度以上のとき tan_beta_ref=tan_beta_ref-0.002; // 指令値を小さくする } }else{ // 0.15~0.19秒の間でないときに if( tan_beta_ref<tan_beta_ref1){ // βの指令値が30度以下のとき tan_beta_ref=tan_beta_ref+0.002; // 指令値を大きくする } } // 目標電流位相をメインルーチンから速度制御メインループへ渡す。 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 ){ p.TL = 1; }else{ p.TL = 2; } #endif #ifdef OLD if( (++i2)>=(int)(TS1/TS0) ){ i2=0; timerTS1(&j); //velocity_loop(); // 速度制御メインループ(w_ref&beta_ref to idq_ref) } #endif #ifdef OLD timerTS0(); //current_loop(); // 電流制御マイナーループ(idq_ref to vuvw) //vuvw2pwm(); // vuvw to pwm //sim_motor(); // IPM, dq座標 #endif // if( (ms=(int)(TS1*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;}// ms=TS0 // debug_p24 = 0; Thread::wait(1); // 1ms待つ } //pc2.printf("\r\n^0^ 2\r\n"); // stop timers (OFF) stop_pwm(); TickerTimerTS0.detach(); // timer interrupt stop RtosTimerTS1.stop(); // rtos timer stop // Thread::wait(1000); // wait till timerTS3 completed fTimerTS3ON=0;//ThreadTimerTS3.terminate(); // fTimerTS4ON=0;//ThreadTimerTS4.terminate(); // //pc2.printf("\r\n^0^ 0\r\n\r\n"); // save data to mbed USB drive // if ( NULL == (fp = fopen( "/mbedUSBdrive/data.csv", "w" )) ){ error( "" );} // save data to PC //pc2.printf("\r\n^0^ %d\r\n\r\n",_count_data); // emergencyStop.attach(&stop_fprintf, 0.001); // emergencyStop if fprintf keep busy for 3 secons for(i=0;i<_count_data;i++){ //pc2.printf("%d: ",i); //pc2.printf("%f, %f, %f, %f, %f\r\n", // data[i][0],data[i][1],data[i][2],data[i][3],data[i][4]); // save data to PC (para, y, time, u) // sprintf(&chr[0],"Temperature: f ºC\r\n");//,data[i][0]); // sprintf(&chr[0],"%d, ", data[i][1]); // fprintf(fp,&chr[0]); // fprintf( fp, ", "); // fprintf( fp, "%d, ", data[i][1]*10000); // fprintf( fp, "\r\n"); // // pc2.printf("%f, %f, %f, %f, %f\r\n", // fprintf( fp, "%f, %f, %f, %f, %f\r\n", // data[i][0],data[i][1],data[i][2],data[i][3],data[i][4]); // save data to PC (para, y, time, u) // // wait(0.1); // Thread::wait(100); } //pc2.printf("\r\n^0^ 2\r\n\r\n"); fclose( fp ); // release mbed USB drive Thread::wait(100); pc2.printf("Control completed!!\r\n\r\n"); }