Kosaka Lab / Mbed 2 deprecated auto_tracking_car

auto tracking car

Dependencies:   mbed-rtos mbed

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?

UserRevisionLine numberNew 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 // ２位 | timerTS1()| TS1| RTOSタイマー | 位置制御または速度制御
kosakaLab 0:2af3980d8cc8 9 // ３位 | timerTS2()| TS2| RTOSスレッド | 不使用 (main()と同じ優先度)
kosakaLab 0:2af3980d8cc8 10 // ４位 | 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 }