main.cpp

00001 //  DC motor control program using H-bridge driver (ex. TA7291P) and incremental rotary encoder with A, B phase.
00002 //  ブラシ付DCモータ制御マイコンプログラム・・・Hブリッジ(例えばTA7291P)＋インクリメンタル型AB２相エンコーダ（例えば1回転24パルス）
00003 //      ver. 130214 by Kosaka lab.
00004 //
00005 //  main.cpp: 優先度の異なるつぎのタイマー処理を生成。
00006 //  優先度|    コールされる関数名|  サンプル時間[s]| 割込み方法| 用途
00007 //  ---------------------------------------------------------
00008 //  最高  |   timerTS0()| TS0| ハードウェアタイマー | 電流制御
00009 //  ２位  |   timerTS1()| TS1| RTOSタイマー   | 位置制御または速度制御
00010 //  ３位  |   timerTS2()| TS2| RTOSスレッド   | 不使用 (main()と同じ優先度)
00011 //  ４位  |   timerTS3()| TS3| RTOSスレッド   | データセーブ
00012 //  最低  |   timerTS4()| TS4| RTOSスレッド   | PCモニタ表示
00013 #include "mbed.h"   // mbedマイコンではstdio.hに相当
00014 #include "rtos.h"   // リアルタイムOS用
00015 
00016 #include "controller.h" // ブラシ付DCモータの位置制御器と電流制御器用
00017 #include "Hbridge.h"    // Hブリッジ用
00018 
00019 
00020 //Serial pc2(USBTX, USBRX);               // PCのモニタ上のtera termに文字を表示する宣言
00021 LocalFileSystem local("mbedUSBdrive");  // PCのmbed USB ディスク上にデータをセーブする宣言
00022 Ticker TickerTimerTS0;          // タイマー割込みを宣言。1ms以下もOK。RTOSよりも優先度が高い
00023 unsigned char   fTimerTS2ON=0, fTimerTS3ON=0, fTimerTS4ON=0;  // timerTS2, TS3, TS4のスタート・ストップ指定フラグ
00024 //DigitalOut  debug_p24(p24); // p17 for debug
00025 
00026 //extern "C" void mbed_reset(); // mbedマイコンをリセットする関数の宣言
00027 
00028 void CallTimerTS2(void const *argument) {   // タイマーtimerTS2()をTS2ごとにコール make sampling time TS3 timer (priority 3: precision 4ms)
00029     int ms;             // [ms], 処理時間
00030     unsigned long c;    // カウンタ
00031     while (true) {      // 永遠に繰り返す
00032         c = _countTS0;          // timerTS0()によるカウント数cを記憶
00033         if( fTimerTS2ON ){  // タイマースタートフラグがオンのとき
00034             timerTS2();         // timerTS2()をTS2[s]ごとにコールする is called every TS2[s].
00035         }
00036         if( (ms=(int)(TS2*1000-(_countTS0-c)*TS0*1000))<=0 ){    ms=1;} // c記憶時点から今まで時間を計算してTS2から引く。それが負なら1msに設定
00037         Thread::wait(ms);   // while()内の処理がTS2[s]ごとに実行されるようにms[ms]待つ
00038     }
00039 }
00040 void CallTimerTS3(void const *argument) {   // タイマーtimerTS3()をTS3ごとにコール make sampling time TS3 timer (priority 3: precision 4ms)
00041     int ms;             // [ms], 処理時間
00042     unsigned long c;    // カウンタ
00043     while (true) {      // 永遠に繰り返す
00044         c = _countTS0;          // timerTS0()によるカウント数cを記憶
00045         if( fTimerTS3ON ){  // タイマースタートフラグがオンのとき
00046             timerTS3();         // timerTS3()をTS3[s]ごとにコールする is called every TS3[s].
00047         }
00048         if( (ms=(int)(TS3*1000-(_countTS0-c)*TS0*1000))<=0 ){    ms=1;} // c記憶時点から今まで時間を計算してTS3から引く。それが負なら1msに設定
00049         Thread::wait(ms);   // while()内の処理がTS3[s]ごとに実行されるようにms[ms]待つ
00050     }
00051 }
00052 
00053 void CallTimerTS4(void const *argument) {   // タイマーtimerTS4()をTS4ごとにコール make sampling time TS4 timer (priority 4: precision 4ms)
00054     int ms;             // [ms], 処理時間
00055     unsigned long c;    // カウンタ
00056     while (true) {      // 永遠に繰り返す
00057         c = _countTS0;          // timerTS0()によるカウント数cを記憶
00058         if( fTimerTS4ON ){  // タイマースタートフラグがオンのとき
00059             timerTS4();         // timerTS4()をTS4[s]ごとにコールする is called every TS4[s].
00060         }
00061         if( (ms=(int)(TS4*1000-(_countTS0-c)*TS0*1000))<=0 ){    ms=1;} // c記憶時点から今まで時間を計算してTS4から引く。それが負なら1msに設定
00062         Thread::wait(ms);   // while()内の処理がTS4[s]ごとに実行されるようにms[ms]待つ
00063     }
00064 }
00065 
00066 //#define OLD   // タイマーを使わないシミュレーションをするときコメント外す
00067 int main(){ // モータ制御プログラム本体：マイコン起動時に最初にコールされる
00068     unsigned short  i;
00069     FILE *fp = fopen("/mbedUSBdrive/data.csv", "w");    // PC上のmbed USB ディスクにデータをセーブするためにディスクをオープン
00070     RtosTimer RtosTimerTS1(timerTS1);                                 // timerTS1のためのRTOSタイマーを宣言
00071     Thread ThreadTimerTS3(CallTimerTS3, NULL, osPriorityBelowNormal); // timerTS3のためのRTOSスレッドを宣言
00072     Thread ThreadTimerTS4(CallTimerTS4, NULL, osPriorityLow);         // timerTS4のためのRTOSスレッドを宣言
00073 // Priority of Thread (RtosTimer is osPriorityAboveNormal)
00074 //  osPriorityIdle          = -3,          ///< priority: idle (lowest)--> then, mbed ERROR!!
00075 //  osPriorityLow           = -2,          ///< priority: low
00076 //  osPriorityBelowNormal   = -1,          ///< priority: below normal
00077 //  osPriorityNormal        =  0,          ///< priority: normal (default)
00078 //  osPriorityAboveNormal   = +1,          ///< priority: above normal
00079 //  osPriorityHigh          = +2,          ///< priority: high 
00080 //  osPriorityRealtime      = +3,          ///< priority: realtime (highest)
00081 //  osPriorityError         =  0x84        ///< system cannot determine priority or thread has illegal priority
00082     float  w_ref_req[2] = {2* 2*PI, 10* 2*PI};   // [rad/s], 指令速度（第2要素は指令速度急変後の指令速度）
00083     float  t;   // [s], 現在の時間
00084 
00085     init_parameters();  // モータの機器定数等の設定, 制御器の初期化
00086 
00087     // シミュレーション開始
00088     printf("Simulation start!!\r\n");   // PCのモニタ上のtera termに文字を表示
00089 #ifndef OLD
00090     // PWMとすべてのタイマーをスタートする
00091     start_pwm();                                    // PWMスタート
00092     TickerTimerTS0.attach(&timerTS0, TS0 );         // timerTS0スタート
00093     RtosTimerTS1.start((unsigned int)(TS1*1000.));  // timerTS1スタート
00094     fTimerTS3ON = 1;                                // timerTS3スタート
00095     fTimerTS4ON = 1;                                // timerTS4スタート
00096 #endif
00097 
00098     while( (t = _countTS0*TS0) < TMAX ){    // 現在の時間tを見て、目標速度等を設定し、TMAX[s]に終了
00099 //        debug_p24 = 1;
00100 
00101         // 速度急変
00102         if( 0.26<=t && t<2.3 ){ // 0.26<t<2.3[s] のとき
00103             vl.w_ref=w_ref_req[1];   // 目標速度を速度制御メインループへ渡す。
00104         }else{                  // 0<t0.26, 2.3<t[s] のとき
00105             vl.w_ref=w_ref_req[0];   // 目標速度を速度制御メインループへ渡す。
00106         }
00107 #ifdef SIMULATION
00108         // 負荷トルク急変
00109         if( t<3.4 ){    // 0<t<3.4[s]のとき
00110             p.TL = 1;       // 負荷トルクは１
00111         }else{          // 3.4<t[s]のとき
00112             p.TL = 2;       // 負荷トルクは１
00113         }
00114 #endif
00115 
00116 #ifdef OLD
00117         if( (++i2)>=(int)(TS1/TS0) ){  i2=0;
00118             timerTS1(&j);   //velocity_loop();  // 速度制御メインループ(w_ref&beta_ref to idq_ref)
00119         }
00120 #endif
00121 
00122 #ifdef OLD
00123         timerTS0();
00124 #endif
00125 
00126 //        debug_p24 = 0;
00127         Thread::wait(1);    // 1ms待つ（待つ間にtimerTS3とtimerTS4が実行される）
00128     }
00129     // PWMとすべてのタイマーをストップする
00130     stop_pwm();             // PWMストップ
00131     TickerTimerTS0.detach();// timerTS0ストップ
00132     RtosTimerTS1.stop();    // timerTS1ストップ
00133     fTimerTS3ON=0;          // timerTS3ストップ
00134     fTimerTS4ON=0;          // timerTS4ストップ
00135 
00136     // PC上のmbed USB ディスクにデータをセーブする
00137     for(i=0;i<N_DATA;i++){
00138         fprintf( fp, "%f, %f, %f, %f, %f\r\n", 
00139         data[i][0],data[i][1],data[i][2],data[i][3],data[i][4]);    // PCのmbed USB ディスク上にデータをセーブする
00140     }
00141     fclose( fp );       // PC上のmbed USB ディスクをリリース
00142     Thread::wait(100);  // セーブ完了まで待つ
00143 
00144     printf("Control completed!!\r\n\r\n");  // 制御実験終了をPCモニタ上に表示
00145 }