Kosaka Lab
/
auto_tracking_car
auto tracking car
main.cpp@0:2af3980d8cc8, 2016-06-14 (annotated)
- Committer:
- kosakaLab
- Date:
- Tue Jun 14 03:12:50 2016 +0000
- Revision:
- 0:2af3980d8cc8
ver 0.1;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kosakaLab | 0:2af3980d8cc8 | 1 | // Auto tracking car |
kosakaLab | 0:2af3980d8cc8 | 2 | // ver. 160610 by Kosaka lab. |
kosakaLab | 0:2af3980d8cc8 | 3 | // |
kosakaLab | 0:2af3980d8cc8 | 4 | // main.cpp: 優先度の異なるつぎのタイマー処理を生成。 |
kosakaLab | 0:2af3980d8cc8 | 5 | // 優先度| コールされる関数名| サンプル時間[s]| 割込み方法| 用途 |
kosakaLab | 0:2af3980d8cc8 | 6 | // --------------------------------------------------------- |
kosakaLab | 0:2af3980d8cc8 | 7 | // 最高 | timerTS0()| TS0| ハードウェアタイマー | 電流制御 |
kosakaLab | 0:2af3980d8cc8 | 8 | // 2位 | timerTS1()| TS1| RTOSタイマー | 位置制御または速度制御 |
kosakaLab | 0:2af3980d8cc8 | 9 | // 3位 | timerTS2()| TS2| RTOSスレッド | 不使用 (main()と同じ優先度) |
kosakaLab | 0:2af3980d8cc8 | 10 | // 4位 | timerTS3()| TS3| RTOSスレッド | データセーブ |
kosakaLab | 0:2af3980d8cc8 | 11 | // 最低 | timerTS4()| TS4| RTOSスレッド | PCモニタ表示 |
kosakaLab | 0:2af3980d8cc8 | 12 | #include "mbed.h" // mbedマイコンではstdio.hに相当 |
kosakaLab | 0:2af3980d8cc8 | 13 | #include "rtos.h" // リアルタイムOS用 |
kosakaLab | 0:2af3980d8cc8 | 14 | |
kosakaLab | 0:2af3980d8cc8 | 15 | #include "controller.h" // ブラシ付DCモータの位置制御器と電流制御器用 |
kosakaLab | 0:2af3980d8cc8 | 16 | |
kosakaLab | 0:2af3980d8cc8 | 17 | |
kosakaLab | 0:2af3980d8cc8 | 18 | Serial pc2(USBTX, USBRX); // PCのモニタ上のtera termに文字を表示する宣言 |
kosakaLab | 0:2af3980d8cc8 | 19 | LocalFileSystem local("mbedUSBdrive"); // PCのmbed USB ディスク上にデータをセーブする宣言 |
kosakaLab | 0:2af3980d8cc8 | 20 | Ticker TickerTimerTS0; // タイマー割込みを宣言。1ms以下もOK。RTOSよりも優先度が高い |
kosakaLab | 0:2af3980d8cc8 | 21 | unsigned char fTimerTS2ON=0, fTimerTS3ON=0, fTimerTS4ON=0; // timerTS2, TS3, TS4のスタート・ストップ指定フラグ |
kosakaLab | 0:2af3980d8cc8 | 22 | |
kosakaLab | 0:2af3980d8cc8 | 23 | void CallTimerTS2(void const *argument) { // タイマーtimerTS2()をTS2ごとにコール make sampling time TS3 timer (priority 3: precision 4ms) |
kosakaLab | 0:2af3980d8cc8 | 24 | int ms; // [ms], 処理時間 |
kosakaLab | 0:2af3980d8cc8 | 25 | unsigned long c; // カウンタ |
kosakaLab | 0:2af3980d8cc8 | 26 | while (true) { // 永遠に繰り返す |
kosakaLab | 0:2af3980d8cc8 | 27 | c = _countTS0; // timerTS0()によるカウント数cを記憶 |
kosakaLab | 0:2af3980d8cc8 | 28 | if( fTimerTS2ON ){ // タイマースタートフラグがオンのとき |
kosakaLab | 0:2af3980d8cc8 | 29 | timerTS2(); // timerTS2()をTS2[s]ごとにコールする is called every TS2[s]. |
kosakaLab | 0:2af3980d8cc8 | 30 | } |
kosakaLab | 0:2af3980d8cc8 | 31 | if( (ms=(int)(TS2*1000-(_countTS0-c)*TS0*1000))<=0 ){ ms=1;} // c記憶時点から今まで時間を計算してTS2から引く。それが負なら1msに設定 |
kosakaLab | 0:2af3980d8cc8 | 32 | Thread::wait(ms); // while()内の処理がTS2[s]ごとに実行されるようにms[ms]待つ |
kosakaLab | 0:2af3980d8cc8 | 33 | } |
kosakaLab | 0:2af3980d8cc8 | 34 | } |
kosakaLab | 0:2af3980d8cc8 | 35 | void CallTimerTS3(void const *argument) { // タイマーtimerTS3()をTS3ごとにコール make sampling time TS3 timer (priority 3: precision 4ms) |
kosakaLab | 0:2af3980d8cc8 | 36 | int ms; // [ms], 処理時間 |
kosakaLab | 0:2af3980d8cc8 | 37 | unsigned long c; // カウンタ |
kosakaLab | 0:2af3980d8cc8 | 38 | while (true) { // 永遠に繰り返す |
kosakaLab | 0:2af3980d8cc8 | 39 | c = _countTS0; // timerTS0()によるカウント数cを記憶 |
kosakaLab | 0:2af3980d8cc8 | 40 | if( fTimerTS3ON ){ // タイマースタートフラグがオンのとき |
kosakaLab | 0:2af3980d8cc8 | 41 | timerTS3(); // timerTS3()をTS3[s]ごとにコールする is called every TS3[s]. |
kosakaLab | 0:2af3980d8cc8 | 42 | } |
kosakaLab | 0:2af3980d8cc8 | 43 | if( (ms=(int)(TS3*1000-(_countTS0-c)*TS0*1000))<=0 ){ ms=1;} // c記憶時点から今まで時間を計算してTS3から引く。それが負なら1msに設定 |
kosakaLab | 0:2af3980d8cc8 | 44 | Thread::wait(ms); // while()内の処理がTS3[s]ごとに実行されるようにms[ms]待つ |
kosakaLab | 0:2af3980d8cc8 | 45 | } |
kosakaLab | 0:2af3980d8cc8 | 46 | } |
kosakaLab | 0:2af3980d8cc8 | 47 | |
kosakaLab | 0:2af3980d8cc8 | 48 | void CallTimerTS4(void const *argument) { // タイマーtimerTS4()をTS4ごとにコール make sampling time TS4 timer (priority 4: precision 4ms) |
kosakaLab | 0:2af3980d8cc8 | 49 | int ms; // [ms], 処理時間 |
kosakaLab | 0:2af3980d8cc8 | 50 | unsigned long c; // カウンタ |
kosakaLab | 0:2af3980d8cc8 | 51 | while (true) { // 永遠に繰り返す |
kosakaLab | 0:2af3980d8cc8 | 52 | c = _countTS0; // timerTS0()によるカウント数cを記憶 |
kosakaLab | 0:2af3980d8cc8 | 53 | if( fTimerTS4ON ){ // タイマースタートフラグがオンのとき |
kosakaLab | 0:2af3980d8cc8 | 54 | timerTS4(); // timerTS4()をTS4[s]ごとにコールする is called every TS4[s]. |
kosakaLab | 0:2af3980d8cc8 | 55 | } |
kosakaLab | 0:2af3980d8cc8 | 56 | if( (ms=(int)(TS4*1000-(_countTS0-c)*TS0*1000))<=0 ){ ms=1;} // c記憶時点から今まで時間を計算してTS4から引く。それが負なら1msに設定 |
kosakaLab | 0:2af3980d8cc8 | 57 | Thread::wait(ms); // while()内の処理がTS4[s]ごとに実行されるようにms[ms]待つ |
kosakaLab | 0:2af3980d8cc8 | 58 | } |
kosakaLab | 0:2af3980d8cc8 | 59 | } |
kosakaLab | 0:2af3980d8cc8 | 60 | |
kosakaLab | 0:2af3980d8cc8 | 61 | int main(){ // モータ制御プログラム本体:マイコン起動時に最初にコールされる |
kosakaLab | 0:2af3980d8cc8 | 62 | unsigned short i; |
kosakaLab | 0:2af3980d8cc8 | 63 | FILE *fp = fopen("/mbedUSBdrive/data.csv", "w"); // PC上のmbed USB ディスクにデータをセーブするためにディスクをオープン |
kosakaLab | 0:2af3980d8cc8 | 64 | RtosTimer RtosTimerTS1(timerTS1); // timerTS1のためのRTOSタイマーを宣言 |
kosakaLab | 0:2af3980d8cc8 | 65 | Thread ThreadTimerTS3(CallTimerTS3, NULL, osPriorityBelowNormal); // timerTS3のためのRTOSスレッドを宣言 |
kosakaLab | 0:2af3980d8cc8 | 66 | Thread ThreadTimerTS4(CallTimerTS4, NULL, osPriorityLow); // timerTS4のためのRTOSスレッドを宣言 |
kosakaLab | 0:2af3980d8cc8 | 67 | // Priority of Thread (RtosTimer is osPriorityAboveNormal) |
kosakaLab | 0:2af3980d8cc8 | 68 | // osPriorityIdle = -3, ///< priority: idle (lowest)--> then, mbed ERROR!! |
kosakaLab | 0:2af3980d8cc8 | 69 | // osPriorityLow = -2, ///< priority: low |
kosakaLab | 0:2af3980d8cc8 | 70 | // osPriorityBelowNormal = -1, ///< priority: below normal |
kosakaLab | 0:2af3980d8cc8 | 71 | // osPriorityNormal = 0, ///< priority: normal (default) |
kosakaLab | 0:2af3980d8cc8 | 72 | // osPriorityAboveNormal = +1, ///< priority: above normal |
kosakaLab | 0:2af3980d8cc8 | 73 | // osPriorityHigh = +2, ///< priority: high |
kosakaLab | 0:2af3980d8cc8 | 74 | // osPriorityRealtime = +3, ///< priority: realtime (highest) |
kosakaLab | 0:2af3980d8cc8 | 75 | // osPriorityError = 0x84 ///< system cannot determine priority or thread has illegal priority |
kosakaLab | 0:2af3980d8cc8 | 76 | |
kosakaLab | 0:2af3980d8cc8 | 77 | init_parameters(); // モータの機器定数等の設定, 制御器の初期化 |
kosakaLab | 0:2af3980d8cc8 | 78 | |
kosakaLab | 0:2af3980d8cc8 | 79 | // シミュレーション開始 |
kosakaLab | 0:2af3980d8cc8 | 80 | pc2.printf("Simulation start!!\r\n"); // PCのモニタ上のtera termに文字を表示 |
kosakaLab | 0:2af3980d8cc8 | 81 | // PWMとすべてのタイマーをスタートする |
kosakaLab | 0:2af3980d8cc8 | 82 | TickerTimerTS0.attach(&timerTS0, TS0 ); // timerTS0スタート |
kosakaLab | 0:2af3980d8cc8 | 83 | RtosTimerTS1.start((unsigned int)(TS1*1000.)); // timerTS1スタート |
kosakaLab | 0:2af3980d8cc8 | 84 | fTimerTS3ON = 1; // timerTS3スタート |
kosakaLab | 0:2af3980d8cc8 | 85 | fTimerTS4ON = 1; // timerTS4スタート |
kosakaLab | 0:2af3980d8cc8 | 86 | |
kosakaLab | 0:2af3980d8cc8 | 87 | while( (_countTS0*TS0) < TMAX ){ // 現在の時間tを見て、目標速度等を設定し、TMAX[s]に終了 |
kosakaLab | 0:2af3980d8cc8 | 88 | Thread::wait(1); // 1ms待つ(待つ間にtimerTS3とtimerTS4が実行される) |
kosakaLab | 0:2af3980d8cc8 | 89 | } |
kosakaLab | 0:2af3980d8cc8 | 90 | // PWMとすべてのタイマーをストップする |
kosakaLab | 0:2af3980d8cc8 | 91 | // stop_pwm(); // PWMストップ |
kosakaLab | 0:2af3980d8cc8 | 92 | TickerTimerTS0.detach();// timerTS0ストップ |
kosakaLab | 0:2af3980d8cc8 | 93 | RtosTimerTS1.stop(); // timerTS1ストップ |
kosakaLab | 0:2af3980d8cc8 | 94 | fTimerTS3ON=0; // timerTS3ストップ |
kosakaLab | 0:2af3980d8cc8 | 95 | fTimerTS4ON=0; // timerTS4ストップ |
kosakaLab | 0:2af3980d8cc8 | 96 | |
kosakaLab | 0:2af3980d8cc8 | 97 | // PC上のmbed USB ディスクにデータをセーブする |
kosakaLab | 0:2af3980d8cc8 | 98 | for(i=0;i<N_DATA;i++){ |
kosakaLab | 0:2af3980d8cc8 | 99 | fprintf( fp, "%f, %f, %f, %f, %f\r\n", |
kosakaLab | 0:2af3980d8cc8 | 100 | data[i][0],data[i][1],data[i][2],data[i][3],data[i][4]); // PCのmbed USB ディスク上にデータをセーブする |
kosakaLab | 0:2af3980d8cc8 | 101 | } |
kosakaLab | 0:2af3980d8cc8 | 102 | fclose( fp ); // PC上のmbed USB ディスクをリリース |
kosakaLab | 0:2af3980d8cc8 | 103 | Thread::wait(100); // セーブ完了まで待つ |
kosakaLab | 0:2af3980d8cc8 | 104 | |
kosakaLab | 0:2af3980d8cc8 | 105 | pc2.printf("Control completed!!\r\n\r\n"); // 制御実験終了をPCモニタ上に表示 |
kosakaLab | 0:2af3980d8cc8 | 106 | } |