Kosaka Lab
auto tracking car
Revision 0:2af3980d8cc8, committed 2016-06-14
- Comitter:
- kosakaLab
- Date:
- Tue Jun 14 03:12:50 2016 +0000
- Commit message:
- ver 0.1;
Changed in this revision
diff -r 000000000000 -r 2af3980d8cc8 controller.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/controller.cpp Tue Jun 14 03:12:50 2016 +0000
@@ -0,0 +1,185 @@
+// controller.cpp: 制御器
+#include "mbed.h" // mbedマイコンではstdio.hに相当
+//#include "QEI.h" // エンコーダ用ライブラリを使用
+
+#include "controller.h" // controller.cpp: モータ制御器（位置制御、電流制御）
+PwmOut pwm0p(p21); // IN1 of TA7291P for right wheel
+PwmOut pwm0m(p22); // IN2 of TA7291P for right wheel
+PwmOut pwm1p(p23); // IN1 of TA7291P for left wheel
+PwmOut pwm1m(p24); // IN2 of TA7291P for left wheel
+unsigned char fReverse[2]; // モータ逆回転フラグ：回転方向が順方向のとき0、逆方向のとき1。[0]が現在の値、[1]はその前の値 reverse direction?
+
+Serial pc(USBTX, USBRX); // PCのモニタ上のtera termに文字を表示する宣言
+
+controller_parameters K[2]; // 速度制御メインループの定数、変数
+float volt[2]; // モータへの入力電圧
+AnalogOut analog_out(DA_PORT); // デバッグ用DA(アナログ信号をDA_PORTに出力)
+
+unsigned long _countTS0; // TS0[s]ごとのカウント数
+float _time; // [s], プログラム開始時からの経過時間
+float debug[20]; // デバッグ用変数
+DigitalOut led1(LED1); // mbedマイコンのLED1を点灯
+DigitalOut led2(LED2); // mbedマイコンのLED2を点灯
+DigitalOut led3(LED3); // mbedマイコンのLED3を点灯
+DigitalOut led4(LED4); // mbedマイコンのLED4を点灯
+
+
+float data[1000][5]; // PC上のmbed USB ディスクにセーブするデータ memory to save data offline instead of "online fprintf".
+unsigned short _countTS3=0;
+
+DigitalOut debug_p26(p26); // p17 for debug
+DigitalOut debug_p25(p25); // p17 for debug
+
+// PIDゲイン
+float Kp[2], Ki[2], Kd[2];
+#define vMAX 3.3 // [V], モータ入力電圧の最大値（超えるとこの値に制限する）
+
+void init_parameters(){
+// 制御器の初期値の設定
+// 親関数： main()
+// 子関数： なし
+ // 制御器0（人の距離）のPIDゲイン
+ Kp[0] = 1;
+ Ki[0] = 0;
+ Kd[0] = 0;
+ // 制御器1（人の向き）のPIDゲイン
+ Kp[1] = 1;
+ Ki[1] = 0;
+ Kd[1] = 0;
+
+ K[0].y = K[1].y = 0; // K[1].yは人の距離[m]、K[1].yは人の向き[deg]
+ K[0].r = 1; // [m], 人の距離の目標値
+ K[0].r = 0; // [deg], 人の向きの目標値
+ K[0].u = K[1].u =0; // 制御入力
+ K[0].eI= K[1].eI=0; // eの積分値
+ K[0].e_old = K[1].e_old =0; // 1サンプル過去の偏差e
+ K[0].eI_old = K[1].eI_old =0; // 1サンプル過去の偏差の積分値eI
+ volt[0] = volt[1] = 0; // [V], モータ入力電圧
+}
+
+
+void controller(){
+// 速度制御メインループ、サンプル時間TS1秒
+// 親関数： timerTS0()
+// 子関数： PID(), v2Hbridge0()
+ void PID(int i), v2Hbridge(int i);
+ int i, f_aw;
+
+ K[0].y = 1; // [m], 人の距離koko
+ K[1].y = 0; // [deg], 人の向きkoko
+
+ PID(0); // 人の距離の制御器
+ PID(1); // 人の向きの制御器
+
+ volt[0] = K[0].u + K[1].u; // 右車輪のモータへの電圧
+ volt[1] = K[0].u - K[1].u; // 左車輪のモータへの電圧
+
+ // アンチワインドアップ：制御入力が飽和したとき積分項eIを減衰させる anti-windup: if u=v_ref is saturated, then reduce eI.
+ f_aw = 0;
+ for(i=0;i<2;i++){
+ if( volt[i] > vMAX ){ // モータ入力電圧がvMAXを超えたとき
+ volt[i] = vMAX; // 電圧をvMAXにする
+ f_aw = 1;
+ }else if( volt[i] < -vMAX ){ // モータ入力電圧が-vMAXを超えたとき
+ volt[1] = -vMAX; // 電圧を-vMAXにする
+ f_aw = 1;
+ }
+ }
+ if( f_aw ){
+ K[0].eI = K[0].eI_old; // 積分しなかったことにする
+ K[1].eI = K[1].eI_old;
+ }
+
+ v2Hbridge(0); // モータ０への入力電圧をPWMにしてHbridgeに出力 volt. to Hbridge
+ v2Hbridge(1); // モータ１への入力電圧をPWMにしてHbridgeに出力 volt. to Hbridge
+}
+
+
+void PID(int i){
+// 制御器：偏差eが入力され、制御入力（モータ電圧）uを出力
+// 入力：出力 K[i].y, 目標値 K[i].r, PID制御積分項 K[i].eI, サンプル時間 TS1 [s]
+// 出力：制御入力（モータ電圧） K[i].u [V]
+ float e, ed;
+
+ e = K[i].r - K[i].y; // 偏差 e の計算
+
+ K[i].eI = K[i].eI + TS1*e; // e の積分値の計算
+ K[i].eI_old = K[i].eI; // eIの1サンプル過去の値を更新
+
+ ed = (e-K[i].e_old)/TS1; // e の微分値の計算
+ K[i].e_old = e; // e の1サンプル過去の値を更新
+
+ K[i].u = Kp[i]*e + Ki[i]*K[i].eI + Kd[i]*ed; // ＰＩＤ制御器の出力を計算
+}
+
+void v2Hbridge(int i){
+// 指令電圧vより、PWM関数pwm_out()のパラメータ（dutyとフラグ）をセット。
+// 親関数： timerTS0()
+// 子関数： なし
+// 入力：電圧指令 p.v [V]
+// 出力：フルブリッジのfwdIN, rvsIN用duty,
+// デッドタイムフラグfDeadtime, モータ逆回転フラグfReverse[i]
+ float duty; // 0-1, PWMデューティ duty of H bridge
+
+ duty = volt[i]/vMAX; // 指令電圧p.vの値を最大電圧vMAXで割って-1～1にしてdutyに代入
+ if( duty>=0 ){ // dutyがプラスでモータが順回転のとき
+ if( fReverse[i]==1 ){ // モータが逆回転から順回転に切り替ったとき
+ if( i==0 ){ pwm0p = 0; pwm0m = 0; // デッドタイム作成
+ }else{ pwm1p = 0; pwm1m = 0;
+ }
+ wait(TS1/10); // make デッドタイム
+ }
+ fReverse[i] = 0; // 逆回転フラグをオフにする
+ if( i==0 ){ pwm0p = duty; pwm0m = 0; // dutyをPWM関数pwm_out()に渡す
+ }else{ pwm1p = duty; pwm1m = 0;
+ }
+ }else{ // dutyがマイナスでモータが逆回転のとき
+ if( fReverse[i]==0 ){ // モータが順回転から逆回転に切り替ったとき
+ if( i==0 ){ pwm0p = 0; pwm0m = 0; // デッドタイム作成
+ }else{ pwm1p = 0; pwm1m = 0;
+ }
+ wait(TS1/10); // make デッドタイム
+ }
+ fReverse[i] = 1; // 逆回転フラグをオンにする
+ if( i==0 ){ pwm0p = 0; pwm0m = -duty; // dutyをPWM関数pwm_out()に渡す
+ }else{ pwm1p = 0; pwm1m = -duty; // dutyをPWM関数pwm_out()に渡す
+ }
+ }
+}
+
+
+void data2mbedUSB(){ // PC上のmbed USB ディスクにセーブするためのデータをTS3[s]ごとに代入 save data to mbed USB drive
+ if( _countTS3<1000 ){ // データ数が1,000の5種類のデータをメモリーに貯める
+ data[_countTS3][0]= debug[0];
+ data[_countTS3][1]= debug[1];
+ data[_countTS3][2]= K[0].y;
+ data[_countTS3][3]= K[1].y;
+ data[_countTS3][4]= _countTS0*TS0;
+ _countTS3++;
+ }
+}
+void timerTS0(){ // タイマーtimerTS0()はTS0[s]ごとにコールされる timer called every TS0[s].
+// debug_p26 = 1;
+ _countTS0++; // カウンターに1足す
+ _time += TS0; // 現在の時間にTS0[s]足す
+
+ controller(); // 制御器
+// debug_p26 = 0;
+}
+
+void timerTS1(void const *argument){ // タイマーtimerTS1()はTS1[s]ごとにコールされる
+}
+
+void display2PC(){ // PCのモニタ上のtera termに諸量を表示 display to tera term on PC
+ pc.printf("%8.1f[s]\t%8.2f[V]\t%8.2f [Hz]\t%8.2f\t%8.2f\r\n",
+ _time, K[0].y, K[1].y, K[0].u, debug[0]);
+// pc.printf("%8.1f[s]\t%8.5f[V]\t%4d [deg]\t%8.2f\r\n", _time, _u, (int)(_th/(2*PI)*360.0), _r);//debug[0]*3.3/R_SHUNT); // print to tera term
+}
+void timerTS2(){ // タイマーtimerTS2()はTS2[s]ごとにコールされる
+}
+void timerTS3(){ // タイマーtimerTS3()はTS3[s]ごとにコールされる
+ data2mbedUSB(); // PC上のmbed USB ディスクにセーブするためのデータをTS3[s]ごとに代入 data2mbedUSB() is called every TS3[s].
+}
+void timerTS4(){ // タイマーtimerTS4()はTS4[s]ごとにコールされる
+ display2PC(); // PCのモニタ上のtera termに文字を表示 display to tera term on PC. display2PC() is called every TS4[s].
+}
diff -r 000000000000 -r 2af3980d8cc8 controller.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/controller.h Tue Jun 14 03:12:50 2016 +0000
@@ -0,0 +1,46 @@
+#ifndef __controller_h
+#define __controller_h
+
+#define PI 3.14159265358979 // 円周率πの定義
+
+/*********** 使用するポート、サンプル時間、制御ゲインなどの設定 (ここから) ***************/
+ // タイマーのサンプル周期等
+#define TS0 0.002 // [s], timerTS0のサンプル時間（電流制御用） sampling time (priority highest: Ticker IRQ) of motor current i control PID using timer interrupt
+#define TS1 0.002 // [s], timerTS1のサンプル時間（位置制御用） sampling time (priority high: RtosTimer) of motor angle th PID using rtos-timer
+#define TS2 0.2 // [s], timerTS2のサンプル時間（不使用） sampling time (priority =main(): precision 4ms) to save data to PC using thread. But, max data length is 1000.
+#define TS3 0.002 // [s], timerTS3のサンプル時間（データセーブ用） sampling time (priority low: precision 4ms)
+#define TS4 0.2 // [s], timerTS4のサンプル時間（モニタ表示用） sampling time (priority lowest: precision 4ms) to display data to PC tera term
+#define TMAX 5.0 // [s], プログラム開始から終了までの時間 experiment starts from 0[s] to TMAX[s]
+#define N_DATA 1000 // PC上のmbed USB ディスクにセーブするデータの数
+
+#define DA_PORT p18 // デバッグ用DAポート analog out (DA) port of mbed
+/*********** 使用するポートやサンプル時間、制御ゲインなどの設定 (ここから) ***************/
+
+ // 制御器 K(s) の定数、変数の宣言
+typedef struct struct_controller_parameters{
+ float y; // 出力（制御量）
+ float r; // 目標値
+ float u; // 制御入力
+ float eI; // 偏差eの積分値（積分項）
+ float e_old; // 偏差eの1サンプル過去の値
+ float eI_old; // 偏差eの積分値の1サンプル過去の値
+}controller_parameters;
+
+ // タイマー宣言
+extern void timerTS0(); // TS0[s]ごとにコールされるタイマー timer called every TS0[s].
+extern void timerTS1(void const *argument); // TS1[s]ごとにコールされるタイマー timer called every TS1[s].
+extern void timerTS2(); // TS2[s]ごとにコールされるタイマー timer called every TS2[s].
+extern void timerTS3(); // TS3[s]ごとにコールされるタイマー timer called every TS3[s].
+extern void timerTS4(); // TS4[s]ごとにコールされるタイマー timer called every TS4[s].
+
+extern void init_parameters(); // モータの機器定数等の設定, 制御器の初期化する関数の宣言
+
+extern unsigned long _countTS0; // TS0[s]ごとのカウント数
+extern float _time; // [s], プログラム開始時からの経過時間
+
+extern controller_parameters K[2]; // 制御器の定数、変数
+
+extern float data[][5]; // PC上のmbed USB ディスクにセーブするデータ // memory to save data offline instead of "online fprintf".
+extern unsigned short _countTS3_data; // data[i][5]のカウンタ
+
+#endif
\ No newline at end of file
diff -r 000000000000 -r 2af3980d8cc8 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Jun 14 03:12:50 2016 +0000
@@ -0,0 +1,106 @@
+// Auto tracking car
+// ver. 160610 by Kosaka lab.
+//
+// main.cpp: 優先度の異なるつぎのタイマー処理を生成。
+// 優先度| コールされる関数名| サンプル時間[s]| 割込み方法| 用途
+// ---------------------------------------------------------
+// 最高 | timerTS0()| TS0| ハードウェアタイマー | 電流制御
+// ２位 | timerTS1()| TS1| RTOSタイマー | 位置制御または速度制御
+// ３位 | timerTS2()| TS2| RTOSスレッド | 不使用 (main()と同じ優先度)
+// ４位 | timerTS3()| TS3| RTOSスレッド | データセーブ
+// 最低 | timerTS4()| TS4| RTOSスレッド | PCモニタ表示
+#include "mbed.h" // mbedマイコンではstdio.hに相当
+#include "rtos.h" // リアルタイムOS用
+
+#include "controller.h" // ブラシ付DCモータの位置制御器と電流制御器用
+
+
+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のスタート・ストップ指定フラグ
+
+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-(_countTS0-c)*TS0*1000))<=0 ){ ms=1;} // c記憶時点から今まで時間を計算してTS2から引く。それが負なら1msに設定
+ Thread::wait(ms); // while()内の処理がTS2[s]ごとに実行されるようにms[ms]待つ
+ }
+}
+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-(_countTS0-c)*TS0*1000))<=0 ){ ms=1;} // c記憶時点から今まで時間を計算してTS3から引く。それが負なら1msに設定
+ Thread::wait(ms); // while()内の処理がTS3[s]ごとに実行されるようにms[ms]待つ
+ }
+}
+
+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-(_countTS0-c)*TS0*1000))<=0 ){ ms=1;} // c記憶時点から今まで時間を計算してTS4から引く。それが負なら1msに設定
+ Thread::wait(ms); // while()内の処理がTS4[s]ごとに実行されるようにms[ms]待つ
+ }
+}
+
+int main(){ // モータ制御プログラム本体：マイコン起動時に最初にコールされる
+ unsigned short i;
+ 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
+// osPriorityBelowNormal = -1, ///< priority: below normal
+// osPriorityNormal = 0, ///< priority: normal (default)
+// osPriorityAboveNormal = +1, ///< priority: above normal
+// osPriorityHigh = +2, ///< priority: high
+// osPriorityRealtime = +3, ///< priority: realtime (highest)
+// osPriorityError = 0x84 ///< system cannot determine priority or thread has illegal priority
+
+ init_parameters(); // モータの機器定数等の設定, 制御器の初期化
+
+ // シミュレーション開始
+ pc2.printf("Simulation start!!\r\n"); // PCのモニタ上のtera termに文字を表示
+ // PWMとすべてのタイマーをスタートする
+ TickerTimerTS0.attach(&timerTS0, TS0 ); // timerTS0スタート
+ RtosTimerTS1.start((unsigned int)(TS1*1000.)); // timerTS1スタート
+ fTimerTS3ON = 1; // timerTS3スタート
+ fTimerTS4ON = 1; // timerTS4スタート
+
+ while( (_countTS0*TS0) < TMAX ){ // 現在の時間tを見て、目標速度等を設定し、TMAX[s]に終了
+ Thread::wait(1); // 1ms待つ（待つ間にtimerTS3とtimerTS4が実行される）
+ }
+ // PWMとすべてのタイマーをストップする
+// stop_pwm(); // PWMストップ
+ TickerTimerTS0.detach();// timerTS0ストップ
+ RtosTimerTS1.stop(); // timerTS1ストップ
+ fTimerTS3ON=0; // timerTS3ストップ
+ fTimerTS4ON=0; // timerTS4ストップ
+
+ // 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]); // PCのmbed USB ディスク上にデータをセーブする
+ }
+ fclose( fp ); // PC上のmbed USB ディスクをリリース
+ Thread::wait(100); // セーブ完了まで待つ
+
+ pc2.printf("Control completed!!\r\n\r\n"); // 制御実験終了をPCモニタ上に表示
+}
diff -r 000000000000 -r 2af3980d8cc8 mbed-rtos.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Tue Jun 14 03:12:50 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed-rtos/#4c105b8d7cae
diff -r 000000000000 -r 2af3980d8cc8 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Jun 14 03:12:50 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/6c34061e7c34 \ No newline at end of file