main.cpp

00001 //  UVW three phases Blushless DC motor control program using 3 H-bridge driver (ex. TA7291P) and incremental rotary encoder with A, B phase.
00002 //      ver. 130903 by Kosaka lab.
00003 #include "mbed.h"
00004 #include "rtos.h"
00005 
00006 #include "fast_math.h"
00007 #include "controller.h"
00008 #include "UVWpwm.h"
00009 
00010 
00011 Serial pc2(USBTX, USBRX);        // Display on tera term in PC 
00012 LocalFileSystem local("mbedUSBdrive"); // save data to mbed USB disk drive in PC
00013 //Semaphore semaphore1(1);      // wait and release to protect memories and so on
00014 //Mutex stdio_mutex;            // wait and release to protect memories and so on
00015 Ticker TickerTimerTS0;          // Timer interrupt using TIMER3, TS<0.001 is OK. Priority is higher than rtosTimer.
00016 unsigned char   fTimerTS2ON=0, fTimerTS3ON=0, fTimerTS4ON=0;  // ON/OFF flag for timerTS2, 3, 4.
00017 
00018 extern "C" void mbed_reset();
00019 
00020 void CallTimerTS2(void const *argument) {   // make sampling time TS3 timer (priority 3: precision 4ms)
00021     int ms;
00022     unsigned long c;
00023     while (true) {
00024         c = _count;
00025         if( fTimerTS2ON ){
00026             timerTS2();  // timerTS2() is called every TS2[s].
00027         }
00028         if( (ms=(int)(TS2*1000-(_count-c)*TS0*1000))<=0 ){    ms=1;}
00029         Thread::wait(ms);
00030     }
00031 }
00032 void CallTimerTS3(void const *argument) {   // make sampling time TS3 timer (priority 3: precision 4ms)
00033     int ms;
00034     unsigned long c;
00035     while (true) {
00036         c = _count;
00037         if( fTimerTS3ON ){
00038             timerTS3();  // timerTS3() is called every TS3[s].
00039         }
00040         if( (ms=(int)(TS3*1000-(_count-c)*TS0*1000))<=0 ){    ms=1;}
00041         Thread::wait(ms);
00042     }
00043 }
00044 
00045 void CallTimerTS4(void const *argument) {   // make sampling time TS4 timer (priority 4: precision 4ms)
00046     int ms;
00047     unsigned long c;
00048     while (true) {
00049         c = _count;
00050         if( fTimerTS4ON ){
00051             timerTS4();  // timerTS4() is called every TS4[s].
00052         }
00053         if( (ms=(int)(TS4*1000-(_count-c)*TS0*1000))<=0 ){    ms=1;}
00054         Thread::wait(ms);
00055     }
00056 }
00057 
00058 //void stop_fprintf(){ // emergencyStop if fprintf keep busy for 3 secons
00059 //    fclose(fp);
00060 //    pc2.printf("error: fprintf was in hung-up!");
00061 //}
00062 
00063 //#define OLD
00064 int main(){
00065     unsigned short  i, i2=0;
00066     FILE *fp = fopen("/mbedUSBdrive/data.csv", "w");    // save data to PC
00067 //    char chr[100];
00068     RtosTimer RtosTimerTS1(timerTS1);  // RtosTimer priority is osPriorityAboveNormal, just one above main()
00069     Thread ThreadTimerTS3(CallTimerTS3,NULL,osPriorityBelowNormal);
00070     Thread ThreadTimerTS4(CallTimerTS4,NULL,osPriorityLow);
00071 // Priority of Thread (RtosTimer is osPriorityAboveNormal)
00072 //  osPriorityIdle          = -3,          ///< priority: idle (lowest)--> then, mbed ERROR!!
00073 //  osPriorityLow           = -2,          ///< priority: low
00074 //  osPriorityBelowNormal   = -1,          ///< priority: below normal
00075 //  osPriorityNormal        =  0,          ///< priority: normal (default)
00076 //  osPriorityAboveNormal   = +1,          ///< priority: above normal
00077 //  osPriorityHigh          = +2,          ///< priority: high 
00078 //  osPriorityRealtime      = +3,          ///< priority: realtime (highest)
00079 //  osPriorityError         =  0x84        ///< system cannot determine priority or thread has illegal priority
00080 
00081     // 指令速度
00082 //    float  w_ref_req[2] = {20* 2*PI, 40* 2*PI};        // [rad/s]（第2要素は指令速度急変後の指令速度）
00083     float  w_ref_req[2] = {5* 2*PI, 10* 2*PI};        // [rad/s]（第2要素は指令速度急変後の指令速度）
00084     // 指令ｄｑ電流位相
00085 //    float  beta_ref = 30*PI/180;   // [rad], 電流位相指令βを30度に
00086     float  beta_ref = 35*PI/180;   // [rad], 電流位相指令βを30度に
00087     float  tan_beta_ref1;
00088     float  tan_beta_ref2,tan_beta_ref;
00089     float  t;   // current time
00090     extern void    velocity_loop();
00091 
00092 //    start_timers(1); // start multi-timers, sampling times are TS0, TS1, TS2, TS3, TS4.
00093 
00094     // IPMSMの機器定数等の設定, 制御器の初期化
00095     init_parameters();
00096     p.th[0] = 2*PI/3;   //θの初期値
00097 
00098 //  p.Lq0 = p.Ld;   // SPMSMの場合
00099 //  p.phi = 0;      // SynRMの場合
00100 
00101 //    w_ref=p.w;          // 速度指令[rad/s]
00102     tan_beta_ref1 = tan(beta_ref);           // tan30°を計算
00103     tan_beta_ref2 = tan(beta_ref-30*PI/180); // tan0°を計算
00104     tan_beta_ref = tan_beta_ref1;            // βの指令値をtan30°に
00105     // シミュレーション開始
00106     pc2.printf("Simulation start!!\r\n");
00107 #ifndef OLD
00108     // start control (ON)
00109     start_pwm();
00110     TickerTimerTS0.attach(&timerTS0, TS0 ); // Sampling period[s] of i_controller
00111     RtosTimerTS1.start((unsigned int)(TS1*1000.));   // Sampling period[ms] of th controller
00112     fTimerTS3ON = 1;    // timerTS3 start
00113     fTimerTS4ON = 1;    // timerTS3 start
00114 #endif
00115     
00116     // set th by moving rotor to th=zero.
00117     f_find_origin=1;    // 回転角原点復帰フラグをセット
00118     while( (t = _count*TS0) < TMAX_FIND_ORIGIN ){  // TMAX秒経過するまで制御実行
00119         if     (t<1)    p.th_const =  0*2*PI*t*1;       // Set theta with constant angular velosity
00120         else if(t<3)    p.th_const =  9*2*PI*t*1;       // Set theta with constant angular velosity
00121         else if(t<5)    p.th_const = 15*2*PI*t*1;       // Set theta with constant angular velosity
00122         else if(t<TMAX_FIND_ORIGIN-2)
00123                         p.th_const =  9*2*PI*t*1;       // Set theta with constant angular velosity
00124         else            p.th_const =  0*2*PI*t*1;       // Set theta with constant angular velosity
00125 #ifdef OLD
00126         timerTS0();
00127 #endif
00128         Thread::wait(1);
00129     }
00130     // IPMSMの機器定数等の設定, 制御器の初期化
00131     init_parameters();
00132     f_find_origin=0;
00133 
00134 #ifndef OLD
00135     TickerTimerTS0.detach(); // timer interrupt stop
00136     // start control (ON)
00137     TickerTimerTS0.attach(&timerTS0, TS0 ); // Sampling period[s] of i_controller
00138     RtosTimerTS1.start((unsigned int)(TS1*1000.));   // Sampling period[ms] of th controller
00139 #endif
00140 
00141     while( (t = _count*TS0-TMAX_FIND_ORIGIN) < TMAX ){
00142 //        debug_p24 = 1;
00143 
00144         // 電流位相（ｄｑ軸電流）を変化させるベクトル制御
00145         if( t>=0.15 && t<0.19 ){  // 0.15～0.19秒の間に
00146             if( tan_beta_ref>tan_beta_ref2 ){ // βの指令値が0度以上のとき
00147                 tan_beta_ref=tan_beta_ref-0.002;  // 指令値を小さくする
00148             }
00149         }else{                   // 0.15～0.19秒の間でないときに
00150             if( tan_beta_ref<tan_beta_ref1){ // βの指令値が30度以下のとき
00151                 tan_beta_ref=tan_beta_ref+0.002;  // 指令値を大きくする
00152             }
00153         }
00154         // 目標電流位相をメインルーチンから速度制御メインループへ渡す。
00155         vl.tan_beta_ref = tan_beta_ref;
00156 
00157 #if 0
00158         // 速度急変
00159         if( 0.26<=t && t<0.3 ){
00160             vl.w_ref=w_ref_req[1];   // 目標速度をメインルーチンから速度制御メインループへ渡す。
00161         }else{
00162             vl.w_ref=w_ref_req[0];
00163         }
00164 #else
00165         vl.w_ref=0.999*vl.w_ref + (1-0.999)*w_ref_req[0];
00166 #endif
00167 #ifdef SIMULATION
00168         // 負荷トルク急変
00169         if( t<0.4 ){
00170             p.TL = 1;
00171         }else{
00172             p.TL = 2;
00173         }
00174 #endif
00175 
00176 #ifdef OLD
00177         if( (++i2)>=(int)(TS1/TS0) ){  i2=0;
00178             timerTS1(&j);   //velocity_loop();    // 速度制御メインループ(w_ref&beta_ref to idq_ref)
00179         }
00180 #endif
00181 
00182 #ifdef OLD
00183         timerTS0();
00184         //current_loop(); // 電流制御マイナーループ(idq_ref to vuvw)
00185         //vuvw2pwm();     // vuvw to pwm
00186         //sim_motor();    // IPM, ｄｑ座標
00187 #endif
00188 
00189 //        if( (ms=(int)(TS1*1000-(_count-c)*TS0*1000))<=0 ){    ms=1;}// ms=TS0
00190 //        debug_p24 = 0;
00191         Thread::wait(1);         // 1ms待つ
00192     }
00193 //pc2.printf("\r\n^0^ 2\r\n");
00194     // stop timers (OFF)
00195     stop_pwm();
00196     TickerTimerTS0.detach(); // timer interrupt stop
00197     RtosTimerTS1.stop();    // rtos timer stop
00198 //    Thread::wait(1000); // wait till timerTS3 completed
00199     fTimerTS3ON=0;//ThreadTimerTS3.terminate();   // 
00200     fTimerTS4ON=0;//ThreadTimerTS4.terminate();   // 
00201 //pc2.printf("\r\n^0^ 0\r\n\r\n");
00202 
00203     // save data to mbed USB drive
00204 //    if ( NULL == (fp = fopen( "/mbedUSBdrive/data.csv", "w" )) ){   error( "" );} // save data to PC
00205 //pc2.printf("\r\n^0^ %d\r\n\r\n",_count_data);
00206 //    emergencyStop.attach(&stop_fprintf, 0.001); // emergencyStop if fprintf keep busy for 3 secons
00207     for(i=0;i<_count_data;i++){
00208 //pc2.printf("%d: ",i);
00209 //pc2.printf("%f, %f, %f, %f, %f\r\n", 
00210 //        data[i][0],data[i][1],data[i][2],data[i][3],data[i][4]);  // save data to PC (para, y, time, u)
00211 //        sprintf(&chr[0],"Temperature: f ºC\r\n");//,data[i][0]);
00212 //        sprintf(&chr[0],"%d, ", data[i][1]);
00213 //        fprintf(fp,&chr[0]); 
00214 //               fprintf( fp, ", ");
00215 //       fprintf( fp, "%d, ", data[i][1]*10000);
00216 //       fprintf( fp, "\r\n");
00217 //
00218 //        pc2.printf("%f, %f, %f, %f, %f\r\n", 
00219 //        fprintf( fp, "%f, %f, %f, %f, %f\r\n", 
00220 //        data[i][0],data[i][1],data[i][2],data[i][3],data[i][4]);  // save data to PC (para, y, time, u)
00221 //
00222 //        wait(0.1);
00223 //        Thread::wait(100);
00224     }
00225 //pc2.printf("\r\n^0^ 2\r\n\r\n");
00226     fclose( fp );               // release mbed USB drive
00227     
00228     Thread::wait(100);
00229     pc2.printf("Control completed!!\r\n\r\n");
00230 }