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要素は指令速度急変後の指令速度）
    // 指令ｄｑ電流位相
//    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;

        // 電流位相（ｄｑ軸電流）を変化させるベクトル制御
        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, ｄｑ座標
#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");
}