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
Diff: main.cpp
- Revision:
- 13:ba71733c11d7
- Parent:
- 12:459af534d1ee
- Child:
- 14:02411880ffb9
--- a/main.cpp Fri Jan 04 12:00:48 2013 +0000 +++ b/main.cpp Fri Mar 01 02:10:59 2013 +0000 @@ -1,65 +1,75 @@ -// DC motor control program using H-bridge driver (ex. TA7291P) and 360 resolution rotary encoder with A, B phase. -// ver. 121224 by Kosaka lab. -#include "mbed.h" -#include "rtos.h" +// DC motor control program using H-bridge driver (ex. TA7291P) and incremental rotary encoder with A, B phase. +// ブラシ付DCモータ制御マイコンプログラム・・・Hブリッジ(例えばTA7291P)+インクリメンタル型AB2相エンコーダ(例えば1回転24パルス) +// ver. 130214 by Kosaka lab. +// +// main.cpp: 優先度の異なるつぎのタイマー処理を生成。 +// 優先度| コールされる関数名| サンプル時間[s]| 割込み方法| 用途 +// --------------------------------------------------------- +// 最高 | timerTS0()| TS0| ハードウェアタイマー | 電流制御 +// 2位 | timerTS1()| TS1| RTOSタイマー | 位置制御または速度制御 +// 3位 | timerTS2()| TS2| RTOSスレッド | 不使用 (main()と同じ優先度) +// 4位 | timerTS3()| TS3| RTOSスレッド | データセーブ +// 最低 | timerTS4()| TS4| RTOSスレッド | PCモニタ表示 +#include "mbed.h" // mbedマイコンではstdio.hに相当 +#include "rtos.h" // リアルタイムOS用 -#include "controller.h" -#include "Hbridge.h" +#include "controller.h" // ブラシ付DCモータの位置制御器と電流制御器用 +#include "Hbridge.h" // Hブリッジ用 -Serial pc2(USBTX, USBRX); // Display on tera term in PC -LocalFileSystem local("mbedUSBdrive"); // save data to mbed USB disk drive in PC -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. +Serial pc2(USBTX, USBRX); // PCのモニタ上のtera termに文字を表示する宣言 +LocalFileSystem local("mbedUSBdrive"); // PCのmbed USB ディスク上にデータをセーブする宣言 +Ticker TickerTimerTS0; // タイマー割込みを宣言。1ms以下もOK。RTOSよりも優先度が高い +unsigned char fTimerTS2ON=0, fTimerTS3ON=0, fTimerTS4ON=0; // timerTS2, TS3, TS4のスタート・ストップ指定フラグ //DigitalOut debug_p24(p24); // p17 for debug -extern "C" void mbed_reset(); +//extern "C" void mbed_reset(); // mbedマイコンをリセットする関数の宣言 -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]. +void CallTimerTS2(void const *argument) { // タイマーtimerTS2()をTS2ごとにコール make sampling time TS3 timer (priority 3: precision 4ms) + int ms; // [ms], 処理時間 + unsigned long c; // カウンタ + while (true) { // 永遠に繰り返す + c = _countTS0; // timerTS0()によるカウント数cを記憶 + if( fTimerTS2ON ){ // タイマースタートフラグがオンのとき + timerTS2(); // timerTS2()をTS2[s]ごとにコールする is called every TS2[s]. } - if( (ms=(int)(TS2*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;} - Thread::wait(ms); + if( (ms=(int)(TS2*1000-(_countTS0-c)*TS0*1000))<=0 ){ ms=1;} // c記憶時点から今まで時間を計算してTS2から引く。それが負なら1msに設定 + Thread::wait(ms); // while()内の処理がTS2[s]ごとに実行されるようにms[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]. +void CallTimerTS3(void const *argument) { // タイマーtimerTS3()をTS3ごとにコール make sampling time TS3 timer (priority 3: precision 4ms) + int ms; // [ms], 処理時間 + unsigned long c; // カウンタ + while (true) { // 永遠に繰り返す + c = _countTS0; // timerTS0()によるカウント数cを記憶 + if( fTimerTS3ON ){ // タイマースタートフラグがオンのとき + timerTS3(); // timerTS3()をTS3[s]ごとにコールする is called every TS3[s]. } - if( (ms=(int)(TS3*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;} - Thread::wait(ms); + if( (ms=(int)(TS3*1000-(_countTS0-c)*TS0*1000))<=0 ){ ms=1;} // c記憶時点から今まで時間を計算してTS3から引く。それが負なら1msに設定 + Thread::wait(ms); // while()内の処理がTS3[s]ごとに実行されるようにms[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]. +void CallTimerTS4(void const *argument) { // タイマーtimerTS4()をTS4ごとにコール make sampling time TS4 timer (priority 4: precision 4ms) + int ms; // [ms], 処理時間 + unsigned long c; // カウンタ + while (true) { // 永遠に繰り返す + c = _countTS0; // timerTS0()によるカウント数cを記憶 + if( fTimerTS4ON ){ // タイマースタートフラグがオンのとき + timerTS4(); // timerTS4()をTS4[s]ごとにコールする is called every TS4[s]. } - if( (ms=(int)(TS4*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;} - Thread::wait(ms); + if( (ms=(int)(TS4*1000-(_countTS0-c)*TS0*1000))<=0 ){ ms=1;} // c記憶時点から今まで時間を計算してTS4から引く。それが負なら1msに設定 + Thread::wait(ms); // while()内の処理がTS4[s]ごとに実行されるようにms[ms]待つ } } -//#define OLD -int main(){ +//#define OLD // タイマーを使わないシミュレーションをするときコメント外す +int main(){ // モータ制御プログラム本体:マイコン起動時に最初にコールされる unsigned short i; - FILE *fp = fopen("/mbedUSBdrive/data.csv", "w"); // save data to PC - RtosTimer RtosTimerTS1(timerTS1); // RtosTimer priority is osPriorityAboveNormal, just one above main() - Thread ThreadTimerTS3(CallTimerTS3,NULL,osPriorityBelowNormal); - Thread ThreadTimerTS4(CallTimerTS4,NULL,osPriorityLow); + FILE *fp = fopen("/mbedUSBdrive/data.csv", "w"); // PC上のmbed USB ディスクにデータをセーブするためにディスクをオープン + RtosTimer RtosTimerTS1(timerTS1); // timerTS1のためのRTOSタイマーを宣言 + Thread ThreadTimerTS3(CallTimerTS3, NULL, osPriorityBelowNormal); // timerTS3のためのRTOSスレッドを宣言 + Thread ThreadTimerTS4(CallTimerTS4, NULL, osPriorityLow); // timerTS4のためのRTOSスレッドを宣言 // Priority of Thread (RtosTimer is osPriorityAboveNormal) // osPriorityIdle = -3, ///< priority: idle (lowest)--> then, mbed ERROR!! // osPriorityLow = -2, ///< priority: low @@ -69,46 +79,43 @@ // 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] = {2* 2*PI, 4* 2*PI}; // [rad/s], 指令速度(第2要素は指令速度急変後の指令速度) + float t; // [s], 現在の時間 - // 指令速度 - float w_ref_req[2] = {2* 2*PI, 4* 2*PI}; // [rad/s](第2要素は指令速度急変後の指令速度) - float t; // current time - - // IPMSMの機器定数等の設定, 制御器の初期化 - init_parameters(); + init_parameters(); // モータの機器定数等の設定, 制御器の初期化 // シミュレーション開始 - pc2.printf("Simulation start!!\r\n"); + pc2.printf("Simulation start!!\r\n"); // PCのモニタ上のtera termに文字を表示 #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 + // PWMとすべてのタイマーをスタートする + start_pwm(); // PWMスタート + TickerTimerTS0.attach(&timerTS0, TS0 ); // timerTS0スタート + RtosTimerTS1.start((unsigned int)(TS1*1000.)); // timerTS1スタート + fTimerTS3ON = 1; // timerTS3スタート + fTimerTS4ON = 1; // timerTS4スタート #endif - while( (t = _count*TS0) < TMAX ){ + while( (t = _countTS0*TS0) < TMAX ){ // 現在の時間tを見て、目標速度等を設定し、TMAX[s]に終了 // debug_p24 = 1; // 速度急変 - if( 0.26<=t && t<2.3 ){ - vl.w_ref=w_ref_req[1]; // 目標速度をメインルーチンから速度制御メインループへ渡す。 - }else{ - vl.w_ref=w_ref_req[0]; + if( 0.26<=t && t<2.3 ){ // 0.26<t<2.3[s] のとき + vl.w_ref=w_ref_req[1]; // 目標速度を速度制御メインループへ渡す。 + }else{ // 0<t0.26, 2.3<t[s] のとき + vl.w_ref=w_ref_req[0]; // 目標速度を速度制御メインループへ渡す。 } #ifdef SIMULATION // 負荷トルク急変 - if( t<3.4 ){ - p.TL = 1; - }else{ - p.TL = 2; + if( t<3.4 ){ // 0<t<3.4[s]のとき + p.TL = 1; // 負荷トルクは1 + }else{ // 3.4<t[s]のとき + p.TL = 2; // 負荷トルクは1 } #endif #ifdef OLD if( (++i2)>=(int)(TS1/TS0) ){ i2=0; - timerTS1(&j); //velocity_loop(); // 速度制御メインループ(w_ref&beta_ref to idq_ref) + timerTS1(&j); //velocity_loop(); // 速度制御メインループ(w_ref&beta_ref to idq_ref) } #endif @@ -117,22 +124,22 @@ #endif // debug_p24 = 0; - Thread::wait(1); + Thread::wait(1); // 1ms待つ(待つ間にtimerTS3とtimerTS4が実行される) } - // stop timers (OFF) - stop_pwm(); - TickerTimerTS0.detach(); // timer interrupt stop - RtosTimerTS1.stop(); // rtos timer stop - fTimerTS3ON=0;//ThreadTimerTS3.terminate(); // - fTimerTS4ON=0;//ThreadTimerTS4.terminate(); // + // PWMとすべてのタイマーをストップする + stop_pwm(); // PWMストップ + TickerTimerTS0.detach();// timerTS0ストップ + RtosTimerTS1.stop(); // timerTS1ストップ + fTimerTS3ON=0; // timerTS3ストップ + fTimerTS4ON=0; // timerTS4ストップ - // save data to mbed USB drive - for(i=0;i<_count_data;i++){ + // PC上のmbed USB ディスクにデータをセーブする + for(i=0;i<N_DATA;i++){ 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) + data[i][0],data[i][1],data[i][2],data[i][3],data[i][4]); // PCのmbed USB ディスク上にデータをセーブする } - fclose( fp ); // release mbed USB drive - - Thread::wait(100); - pc2.printf("Control completed!!\r\n\r\n"); + fclose( fp ); // PC上のmbed USB ディスクをリリース + Thread::wait(100); // セーブ完了まで待つ + + pc2.printf("Control completed!!\r\n\r\n"); // 制御実験終了をPCモニタ上に表示 }