manabu kosaka / Mbed 2 deprecated DCmotor2

DC motor control program using TA7291P type H bridge driver and rotary encoder with A, B phase.

Dependencies:   QEI mbed-rtos mbed

Fork of DCmotor by manabu kosaka

main.cpp@14:02411880ffb9, 2013-03-12 (annotated)

Committer:
kosaka
Date:
Tue Mar 12 04:32:22 2013 +0000
Revision:
14:02411880ffb9
Parent:
13:ba71733c11d7 
130303;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kosaka 13:ba71733c11d7 1 // DC motor control program using H-bridge driver (ex. TA7291P) and incremental rotary encoder with A, B phase.
kosaka 13:ba71733c11d7 2 // ブラシ付DCモータ制御マイコンプログラム・・・Hブリッジ(例えばTA7291P)＋インクリメンタル型AB２相エンコーダ（例えば1回転24パルス）
kosaka 13:ba71733c11d7 3 // ver. 130214 by Kosaka lab.
kosaka 13:ba71733c11d7 4 //
kosaka 13:ba71733c11d7 5 // main.cpp: 優先度の異なるつぎのタイマー処理を生成。
kosaka 13:ba71733c11d7 6 // 優先度| コールされる関数名| サンプル時間[s]| 割込み方法| 用途
kosaka 13:ba71733c11d7 7 // ---------------------------------------------------------
kosaka 13:ba71733c11d7 8 // 最高 | timerTS0()| TS0| ハードウェアタイマー | 電流制御
kosaka 13:ba71733c11d7 9 // ２位 | timerTS1()| TS1| RTOSタイマー | 位置制御または速度制御
kosaka 13:ba71733c11d7 10 // ３位 | timerTS2()| TS2| RTOSスレッド | 不使用 (main()と同じ優先度)
kosaka 13:ba71733c11d7 11 // ４位 | timerTS3()| TS3| RTOSスレッド | データセーブ
kosaka 13:ba71733c11d7 12 // 最低 | timerTS4()| TS4| RTOSスレッド | PCモニタ表示
kosaka 13:ba71733c11d7 13 #include "mbed.h" // mbedマイコンではstdio.hに相当
kosaka 13:ba71733c11d7 14 #include "rtos.h" // リアルタイムOS用
kosaka 0:fe068497f773 15
kosaka 13:ba71733c11d7 16 #include "controller.h" // ブラシ付DCモータの位置制御器と電流制御器用
kosaka 13:ba71733c11d7 17 #include "Hbridge.h" // Hブリッジ用
kosaka 0:fe068497f773 18
kosaka 0:fe068497f773 19
kosaka 13:ba71733c11d7 20 Serial pc2(USBTX, USBRX); // PCのモニタ上のtera termに文字を表示する宣言
kosaka 13:ba71733c11d7 21 LocalFileSystem local("mbedUSBdrive"); // PCのmbed USB ディスク上にデータをセーブする宣言
kosaka 13:ba71733c11d7 22 Ticker TickerTimerTS0; // タイマー割込みを宣言。1ms以下もOK。RTOSよりも優先度が高い
kosaka 13:ba71733c11d7 23 unsigned char fTimerTS2ON=0, fTimerTS3ON=0, fTimerTS4ON=0; // timerTS2, TS3, TS4のスタート・ストップ指定フラグ
kosaka 12:459af534d1ee 24 //DigitalOut debug_p24(p24); // p17 for debug
kosaka 0:fe068497f773 25
kosaka 13:ba71733c11d7 26 //extern "C" void mbed_reset(); // mbedマイコンをリセットする関数の宣言
kosaka 3:b6b9b8c7dce6 27
kosaka 13:ba71733c11d7 28 void CallTimerTS2(void const *argument) { // タイマーtimerTS2()をTS2ごとにコール make sampling time TS3 timer (priority 3: precision 4ms)
kosaka 13:ba71733c11d7 29 int ms; // [ms], 処理時間
kosaka 13:ba71733c11d7 30 unsigned long c; // カウンタ
kosaka 13:ba71733c11d7 31 while (true) { // 永遠に繰り返す
kosaka 13:ba71733c11d7 32 c = _countTS0; // timerTS0()によるカウント数cを記憶
kosaka 13:ba71733c11d7 33 if( fTimerTS2ON ){ // タイマースタートフラグがオンのとき
kosaka 13:ba71733c11d7 34 timerTS2(); // timerTS2()をTS2[s]ごとにコールする is called every TS2[s].
kosaka 3:b6b9b8c7dce6 35 }
kosaka 13:ba71733c11d7 36 if( (ms=(int)(TS2*1000-(_countTS0-c)*TS0*1000))<=0 ){ ms=1;} // c記憶時点から今まで時間を計算してTS2から引く。それが負なら1msに設定
kosaka 13:ba71733c11d7 37 Thread::wait(ms); // while()内の処理がTS2[s]ごとに実行されるようにms[ms]待つ
kosaka 3:b6b9b8c7dce6 38 }
kosaka 8:b8b31e9b60c2 39 }
kosaka 13:ba71733c11d7 40 void CallTimerTS3(void const *argument) { // タイマーtimerTS3()をTS3ごとにコール make sampling time TS3 timer (priority 3: precision 4ms)
kosaka 13:ba71733c11d7 41 int ms; // [ms], 処理時間
kosaka 13:ba71733c11d7 42 unsigned long c; // カウンタ
kosaka 13:ba71733c11d7 43 while (true) { // 永遠に繰り返す
kosaka 13:ba71733c11d7 44 c = _countTS0; // timerTS0()によるカウント数cを記憶
kosaka 13:ba71733c11d7 45 if( fTimerTS3ON ){ // タイマースタートフラグがオンのとき
kosaka 13:ba71733c11d7 46 timerTS3(); // timerTS3()をTS3[s]ごとにコールする is called every TS3[s].
kosaka 2:e056793d6fc5 47 }
kosaka 13:ba71733c11d7 48 if( (ms=(int)(TS3*1000-(_countTS0-c)*TS0*1000))<=0 ){ ms=1;} // c記憶時点から今まで時間を計算してTS3から引く。それが負なら1msに設定
kosaka 13:ba71733c11d7 49 Thread::wait(ms); // while()内の処理がTS3[s]ごとに実行されるようにms[ms]待つ
kosaka 0:fe068497f773 50 }
kosaka 0:fe068497f773 51 }
kosaka 0:fe068497f773 52
kosaka 13:ba71733c11d7 53 void CallTimerTS4(void const *argument) { // タイマーtimerTS4()をTS4ごとにコール make sampling time TS4 timer (priority 4: precision 4ms)
kosaka 13:ba71733c11d7 54 int ms; // [ms], 処理時間
kosaka 13:ba71733c11d7 55 unsigned long c; // カウンタ
kosaka 13:ba71733c11d7 56 while (true) { // 永遠に繰り返す
kosaka 13:ba71733c11d7 57 c = _countTS0; // timerTS0()によるカウント数cを記憶
kosaka 13:ba71733c11d7 58 if( fTimerTS4ON ){ // タイマースタートフラグがオンのとき
kosaka 13:ba71733c11d7 59 timerTS4(); // timerTS4()をTS4[s]ごとにコールする is called every TS4[s].
kosaka 7:613febb8f028 60 }
kosaka 13:ba71733c11d7 61 if( (ms=(int)(TS4*1000-(_countTS0-c)*TS0*1000))<=0 ){ ms=1;} // c記憶時点から今まで時間を計算してTS4から引く。それが負なら1msに設定
kosaka 13:ba71733c11d7 62 Thread::wait(ms); // while()内の処理がTS4[s]ごとに実行されるようにms[ms]待つ
kosaka 12:459af534d1ee 63 }
kosaka 12:459af534d1ee 64 }
kosaka 0:fe068497f773 65
kosaka 13:ba71733c11d7 66 //#define OLD // タイマーを使わないシミュレーションをするときコメント外す
kosaka 13:ba71733c11d7 67 int main(){ // モータ制御プログラム本体：マイコン起動時に最初にコールされる
kosaka 12:459af534d1ee 68 unsigned short i;
kosaka 13:ba71733c11d7 69 FILE *fp = fopen("/mbedUSBdrive/data.csv", "w"); // PC上のmbed USB ディスクにデータをセーブするためにディスクをオープン
kosaka 13:ba71733c11d7 70 RtosTimer RtosTimerTS1(timerTS1); // timerTS1のためのRTOSタイマーを宣言
kosaka 13:ba71733c11d7 71 Thread ThreadTimerTS3(CallTimerTS3, NULL, osPriorityBelowNormal); // timerTS3のためのRTOSスレッドを宣言
kosaka 13:ba71733c11d7 72 Thread ThreadTimerTS4(CallTimerTS4, NULL, osPriorityLow); // timerTS4のためのRTOSスレッドを宣言
kosaka 12:459af534d1ee 73 // Priority of Thread (RtosTimer is osPriorityAboveNormal)
kosaka 0:fe068497f773 74 // osPriorityIdle = -3, ///< priority: idle (lowest)--> then, mbed ERROR!!
kosaka 0:fe068497f773 75 // osPriorityLow = -2, ///< priority: low
kosaka 0:fe068497f773 76 // osPriorityBelowNormal = -1, ///< priority: below normal
kosaka 0:fe068497f773 77 // osPriorityNormal = 0, ///< priority: normal (default)
kosaka 0:fe068497f773 78 // osPriorityAboveNormal = +1, ///< priority: above normal
kosaka 0:fe068497f773 79 // osPriorityHigh = +2, ///< priority: high
kosaka 0:fe068497f773 80 // osPriorityRealtime = +3, ///< priority: realtime (highest)
kosaka 0:fe068497f773 81 // osPriorityError = 0x84 ///< system cannot determine priority or thread has illegal priority
kosaka 14:02411880ffb9 82 float w_ref_req[2] = {2* 2*PI, 10* 2*PI}; // [rad/s], 指令速度（第2要素は指令速度急変後の指令速度）
kosaka 13:ba71733c11d7 83 float t; // [s], 現在の時間
kosaka 12:459af534d1ee 84
kosaka 13:ba71733c11d7 85 init_parameters(); // モータの機器定数等の設定, 制御器の初期化
kosaka 12:459af534d1ee 86
kosaka 12:459af534d1ee 87 // シミュレーション開始
kosaka 13:ba71733c11d7 88 pc2.printf("Simulation start!!\r\n"); // PCのモニタ上のtera termに文字を表示
kosaka 12:459af534d1ee 89 #ifndef OLD
kosaka 13:ba71733c11d7 90 // PWMとすべてのタイマーをスタートする
kosaka 13:ba71733c11d7 91 start_pwm(); // PWMスタート
kosaka 13:ba71733c11d7 92 TickerTimerTS0.attach(&timerTS0, TS0 ); // timerTS0スタート
kosaka 13:ba71733c11d7 93 RtosTimerTS1.start((unsigned int)(TS1*1000.)); // timerTS1スタート
kosaka 13:ba71733c11d7 94 fTimerTS3ON = 1; // timerTS3スタート
kosaka 13:ba71733c11d7 95 fTimerTS4ON = 1; // timerTS4スタート
kosaka 12:459af534d1ee 96 #endif
kosaka 12:459af534d1ee 97
kosaka 13:ba71733c11d7 98 while( (t = _countTS0*TS0) < TMAX ){ // 現在の時間tを見て、目標速度等を設定し、TMAX[s]に終了
kosaka 12:459af534d1ee 99 // debug_p24 = 1;
kosaka 12:459af534d1ee 100
kosaka 12:459af534d1ee 101 // 速度急変
kosaka 13:ba71733c11d7 102 if( 0.26<=t && t<2.3 ){ // 0.26<t<2.3[s] のとき
kosaka 13:ba71733c11d7 103 vl.w_ref=w_ref_req[1]; // 目標速度を速度制御メインループへ渡す。
kosaka 13:ba71733c11d7 104 }else{ // 0<t0.26, 2.3<t[s] のとき
kosaka 13:ba71733c11d7 105 vl.w_ref=w_ref_req[0]; // 目標速度を速度制御メインループへ渡す。
kosaka 12:459af534d1ee 106 }
kosaka 12:459af534d1ee 107 #ifdef SIMULATION
kosaka 12:459af534d1ee 108 // 負荷トルク急変
kosaka 13:ba71733c11d7 109 if( t<3.4 ){ // 0<t<3.4[s]のとき
kosaka 13:ba71733c11d7 110 p.TL = 1; // 負荷トルクは１
kosaka 13:ba71733c11d7 111 }else{ // 3.4<t[s]のとき
kosaka 13:ba71733c11d7 112 p.TL = 2; // 負荷トルクは１
kosaka 12:459af534d1ee 113 }
kosaka 12:459af534d1ee 114 #endif
kosaka 12:459af534d1ee 115
kosaka 12:459af534d1ee 116 #ifdef OLD
kosaka 12:459af534d1ee 117 if( (++i2)>=(int)(TS1/TS0) ){ i2=0;
kosaka 13:ba71733c11d7 118 timerTS1(&j); //velocity_loop(); // 速度制御メインループ(w_ref&beta_ref to idq_ref)
kosaka 12:459af534d1ee 119 }
kosaka 12:459af534d1ee 120 #endif
kosaka 12:459af534d1ee 121
kosaka 12:459af534d1ee 122 #ifdef OLD
kosaka 12:459af534d1ee 123 timerTS0();
kosaka 12:459af534d1ee 124 #endif
kosaka 12:459af534d1ee 125
kosaka 12:459af534d1ee 126 // debug_p24 = 0;
kosaka 13:ba71733c11d7 127 Thread::wait(1); // 1ms待つ（待つ間にtimerTS3とtimerTS4が実行される）
kosaka 12:459af534d1ee 128 }
kosaka 13:ba71733c11d7 129 // PWMとすべてのタイマーをストップする
kosaka 13:ba71733c11d7 130 stop_pwm(); // PWMストップ
kosaka 13:ba71733c11d7 131 TickerTimerTS0.detach();// timerTS0ストップ
kosaka 13:ba71733c11d7 132 RtosTimerTS1.stop(); // timerTS1ストップ
kosaka 13:ba71733c11d7 133 fTimerTS3ON=0; // timerTS3ストップ
kosaka 13:ba71733c11d7 134 fTimerTS4ON=0; // timerTS4ストップ
kosaka 12:459af534d1ee 135
kosaka 13:ba71733c11d7 136 // PC上のmbed USB ディスクにデータをセーブする
kosaka 13:ba71733c11d7 137 for(i=0;i<N_DATA;i++){
kosaka 12:459af534d1ee 138 fprintf( fp, "%f, %f, %f, %f, %f\r\n",
kosaka 13:ba71733c11d7 139 data[i][0],data[i][1],data[i][2],data[i][3],data[i][4]); // PCのmbed USB ディスク上にデータをセーブする
kosaka 12:459af534d1ee 140 }
kosaka 13:ba71733c11d7 141 fclose( fp ); // PC上のmbed USB ディスクをリリース
kosaka 13:ba71733c11d7 142 Thread::wait(100); // セーブ完了まで待つ
kosaka 13:ba71733c11d7 143
kosaka 13:ba71733c11d7 144 pc2.printf("Control completed!!\r\n\r\n"); // 制御実験終了をPCモニタ上に表示
kosaka 0:fe068497f773 145 }