Kosaka Lab
/
auto_tracking_car
auto tracking car
controller.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 | // controller.cpp: 制御器 |
kosakaLab | 0:2af3980d8cc8 | 2 | #include "mbed.h" // mbedマイコンではstdio.hに相当 |
kosakaLab | 0:2af3980d8cc8 | 3 | //#include "QEI.h" // エンコーダ用ライブラリを使用 |
kosakaLab | 0:2af3980d8cc8 | 4 | |
kosakaLab | 0:2af3980d8cc8 | 5 | #include "controller.h" // controller.cpp: モータ制御器(位置制御、電流制御) |
kosakaLab | 0:2af3980d8cc8 | 6 | PwmOut pwm0p(p21); // IN1 of TA7291P for right wheel |
kosakaLab | 0:2af3980d8cc8 | 7 | PwmOut pwm0m(p22); // IN2 of TA7291P for right wheel |
kosakaLab | 0:2af3980d8cc8 | 8 | PwmOut pwm1p(p23); // IN1 of TA7291P for left wheel |
kosakaLab | 0:2af3980d8cc8 | 9 | PwmOut pwm1m(p24); // IN2 of TA7291P for left wheel |
kosakaLab | 0:2af3980d8cc8 | 10 | unsigned char fReverse[2]; // モータ逆回転フラグ:回転方向が順方向のとき0、逆方向のとき1。[0]が現在の値、[1]はその前の値 reverse direction? |
kosakaLab | 0:2af3980d8cc8 | 11 | |
kosakaLab | 0:2af3980d8cc8 | 12 | Serial pc(USBTX, USBRX); // PCのモニタ上のtera termに文字を表示する宣言 |
kosakaLab | 0:2af3980d8cc8 | 13 | |
kosakaLab | 0:2af3980d8cc8 | 14 | controller_parameters K[2]; // 速度制御メインループの定数、変数 |
kosakaLab | 0:2af3980d8cc8 | 15 | float volt[2]; // モータへの入力電圧 |
kosakaLab | 0:2af3980d8cc8 | 16 | AnalogOut analog_out(DA_PORT); // デバッグ用DA(アナログ信号をDA_PORTに出力) |
kosakaLab | 0:2af3980d8cc8 | 17 | |
kosakaLab | 0:2af3980d8cc8 | 18 | unsigned long _countTS0; // TS0[s]ごとのカウント数 |
kosakaLab | 0:2af3980d8cc8 | 19 | float _time; // [s], プログラム開始時からの経過時間 |
kosakaLab | 0:2af3980d8cc8 | 20 | float debug[20]; // デバッグ用変数 |
kosakaLab | 0:2af3980d8cc8 | 21 | DigitalOut led1(LED1); // mbedマイコンのLED1を点灯 |
kosakaLab | 0:2af3980d8cc8 | 22 | DigitalOut led2(LED2); // mbedマイコンのLED2を点灯 |
kosakaLab | 0:2af3980d8cc8 | 23 | DigitalOut led3(LED3); // mbedマイコンのLED3を点灯 |
kosakaLab | 0:2af3980d8cc8 | 24 | DigitalOut led4(LED4); // mbedマイコンのLED4を点灯 |
kosakaLab | 0:2af3980d8cc8 | 25 | |
kosakaLab | 0:2af3980d8cc8 | 26 | |
kosakaLab | 0:2af3980d8cc8 | 27 | float data[1000][5]; // PC上のmbed USB ディスクにセーブするデータ memory to save data offline instead of "online fprintf". |
kosakaLab | 0:2af3980d8cc8 | 28 | unsigned short _countTS3=0; |
kosakaLab | 0:2af3980d8cc8 | 29 | |
kosakaLab | 0:2af3980d8cc8 | 30 | DigitalOut debug_p26(p26); // p17 for debug |
kosakaLab | 0:2af3980d8cc8 | 31 | DigitalOut debug_p25(p25); // p17 for debug |
kosakaLab | 0:2af3980d8cc8 | 32 | |
kosakaLab | 0:2af3980d8cc8 | 33 | // PIDゲイン |
kosakaLab | 0:2af3980d8cc8 | 34 | float Kp[2], Ki[2], Kd[2]; |
kosakaLab | 0:2af3980d8cc8 | 35 | #define vMAX 3.3 // [V], モータ入力電圧の最大値(超えるとこの値に制限する) |
kosakaLab | 0:2af3980d8cc8 | 36 | |
kosakaLab | 0:2af3980d8cc8 | 37 | void init_parameters(){ |
kosakaLab | 0:2af3980d8cc8 | 38 | // 制御器の初期値の設定 |
kosakaLab | 0:2af3980d8cc8 | 39 | // 親関数: main() |
kosakaLab | 0:2af3980d8cc8 | 40 | // 子関数: なし |
kosakaLab | 0:2af3980d8cc8 | 41 | // 制御器0(人の距離)のPIDゲイン |
kosakaLab | 0:2af3980d8cc8 | 42 | Kp[0] = 1; |
kosakaLab | 0:2af3980d8cc8 | 43 | Ki[0] = 0; |
kosakaLab | 0:2af3980d8cc8 | 44 | Kd[0] = 0; |
kosakaLab | 0:2af3980d8cc8 | 45 | // 制御器1(人の向き)のPIDゲイン |
kosakaLab | 0:2af3980d8cc8 | 46 | Kp[1] = 1; |
kosakaLab | 0:2af3980d8cc8 | 47 | Ki[1] = 0; |
kosakaLab | 0:2af3980d8cc8 | 48 | Kd[1] = 0; |
kosakaLab | 0:2af3980d8cc8 | 49 | |
kosakaLab | 0:2af3980d8cc8 | 50 | K[0].y = K[1].y = 0; // K[1].yは人の距離[m]、K[1].yは人の向き[deg] |
kosakaLab | 0:2af3980d8cc8 | 51 | K[0].r = 1; // [m], 人の距離の目標値 |
kosakaLab | 0:2af3980d8cc8 | 52 | K[0].r = 0; // [deg], 人の向きの目標値 |
kosakaLab | 0:2af3980d8cc8 | 53 | K[0].u = K[1].u =0; // 制御入力 |
kosakaLab | 0:2af3980d8cc8 | 54 | K[0].eI= K[1].eI=0; // eの積分値 |
kosakaLab | 0:2af3980d8cc8 | 55 | K[0].e_old = K[1].e_old =0; // 1サンプル過去の偏差e |
kosakaLab | 0:2af3980d8cc8 | 56 | K[0].eI_old = K[1].eI_old =0; // 1サンプル過去の偏差の積分値eI |
kosakaLab | 0:2af3980d8cc8 | 57 | volt[0] = volt[1] = 0; // [V], モータ入力電圧 |
kosakaLab | 0:2af3980d8cc8 | 58 | } |
kosakaLab | 0:2af3980d8cc8 | 59 | |
kosakaLab | 0:2af3980d8cc8 | 60 | |
kosakaLab | 0:2af3980d8cc8 | 61 | void controller(){ |
kosakaLab | 0:2af3980d8cc8 | 62 | // 速度制御メインループ、サンプル時間TS1秒 |
kosakaLab | 0:2af3980d8cc8 | 63 | // 親関数: timerTS0() |
kosakaLab | 0:2af3980d8cc8 | 64 | // 子関数: PID(), v2Hbridge0() |
kosakaLab | 0:2af3980d8cc8 | 65 | void PID(int i), v2Hbridge(int i); |
kosakaLab | 0:2af3980d8cc8 | 66 | int i, f_aw; |
kosakaLab | 0:2af3980d8cc8 | 67 | |
kosakaLab | 0:2af3980d8cc8 | 68 | K[0].y = 1; // [m], 人の距離koko |
kosakaLab | 0:2af3980d8cc8 | 69 | K[1].y = 0; // [deg], 人の向きkoko |
kosakaLab | 0:2af3980d8cc8 | 70 | |
kosakaLab | 0:2af3980d8cc8 | 71 | PID(0); // 人の距離の制御器 |
kosakaLab | 0:2af3980d8cc8 | 72 | PID(1); // 人の向きの制御器 |
kosakaLab | 0:2af3980d8cc8 | 73 | |
kosakaLab | 0:2af3980d8cc8 | 74 | volt[0] = K[0].u + K[1].u; // 右車輪のモータへの電圧 |
kosakaLab | 0:2af3980d8cc8 | 75 | volt[1] = K[0].u - K[1].u; // 左車輪のモータへの電圧 |
kosakaLab | 0:2af3980d8cc8 | 76 | |
kosakaLab | 0:2af3980d8cc8 | 77 | // アンチワインドアップ:制御入力が飽和したとき積分項eIを減衰させる anti-windup: if u=v_ref is saturated, then reduce eI. |
kosakaLab | 0:2af3980d8cc8 | 78 | f_aw = 0; |
kosakaLab | 0:2af3980d8cc8 | 79 | for(i=0;i<2;i++){ |
kosakaLab | 0:2af3980d8cc8 | 80 | if( volt[i] > vMAX ){ // モータ入力電圧がvMAXを超えたとき |
kosakaLab | 0:2af3980d8cc8 | 81 | volt[i] = vMAX; // 電圧をvMAXにする |
kosakaLab | 0:2af3980d8cc8 | 82 | f_aw = 1; |
kosakaLab | 0:2af3980d8cc8 | 83 | }else if( volt[i] < -vMAX ){ // モータ入力電圧が-vMAXを超えたとき |
kosakaLab | 0:2af3980d8cc8 | 84 | volt[1] = -vMAX; // 電圧を-vMAXにする |
kosakaLab | 0:2af3980d8cc8 | 85 | f_aw = 1; |
kosakaLab | 0:2af3980d8cc8 | 86 | } |
kosakaLab | 0:2af3980d8cc8 | 87 | } |
kosakaLab | 0:2af3980d8cc8 | 88 | if( f_aw ){ |
kosakaLab | 0:2af3980d8cc8 | 89 | K[0].eI = K[0].eI_old; // 積分しなかったことにする |
kosakaLab | 0:2af3980d8cc8 | 90 | K[1].eI = K[1].eI_old; |
kosakaLab | 0:2af3980d8cc8 | 91 | } |
kosakaLab | 0:2af3980d8cc8 | 92 | |
kosakaLab | 0:2af3980d8cc8 | 93 | v2Hbridge(0); // モータ0への入力電圧をPWMにしてHbridgeに出力 volt. to Hbridge |
kosakaLab | 0:2af3980d8cc8 | 94 | v2Hbridge(1); // モータ1への入力電圧をPWMにしてHbridgeに出力 volt. to Hbridge |
kosakaLab | 0:2af3980d8cc8 | 95 | } |
kosakaLab | 0:2af3980d8cc8 | 96 | |
kosakaLab | 0:2af3980d8cc8 | 97 | |
kosakaLab | 0:2af3980d8cc8 | 98 | void PID(int i){ |
kosakaLab | 0:2af3980d8cc8 | 99 | // 制御器:偏差eが入力され、制御入力(モータ電圧)uを出力 |
kosakaLab | 0:2af3980d8cc8 | 100 | // 入力:出力 K[i].y, 目標値 K[i].r, PID制御積分項 K[i].eI, サンプル時間 TS1 [s] |
kosakaLab | 0:2af3980d8cc8 | 101 | // 出力:制御入力(モータ電圧) K[i].u [V] |
kosakaLab | 0:2af3980d8cc8 | 102 | float e, ed; |
kosakaLab | 0:2af3980d8cc8 | 103 | |
kosakaLab | 0:2af3980d8cc8 | 104 | e = K[i].r - K[i].y; // 偏差 e の計算 |
kosakaLab | 0:2af3980d8cc8 | 105 | |
kosakaLab | 0:2af3980d8cc8 | 106 | K[i].eI = K[i].eI + TS1*e; // e の積分値の計算 |
kosakaLab | 0:2af3980d8cc8 | 107 | K[i].eI_old = K[i].eI; // eIの1サンプル過去の値を更新 |
kosakaLab | 0:2af3980d8cc8 | 108 | |
kosakaLab | 0:2af3980d8cc8 | 109 | ed = (e-K[i].e_old)/TS1; // e の微分値の計算 |
kosakaLab | 0:2af3980d8cc8 | 110 | K[i].e_old = e; // e の1サンプル過去の値を更新 |
kosakaLab | 0:2af3980d8cc8 | 111 | |
kosakaLab | 0:2af3980d8cc8 | 112 | K[i].u = Kp[i]*e + Ki[i]*K[i].eI + Kd[i]*ed; // PID制御器の出力を計算 |
kosakaLab | 0:2af3980d8cc8 | 113 | } |
kosakaLab | 0:2af3980d8cc8 | 114 | |
kosakaLab | 0:2af3980d8cc8 | 115 | void v2Hbridge(int i){ |
kosakaLab | 0:2af3980d8cc8 | 116 | // 指令電圧vより、PWM関数pwm_out()のパラメータ(dutyとフラグ)をセット。 |
kosakaLab | 0:2af3980d8cc8 | 117 | // 親関数: timerTS0() |
kosakaLab | 0:2af3980d8cc8 | 118 | // 子関数: なし |
kosakaLab | 0:2af3980d8cc8 | 119 | // 入力:電圧指令 p.v [V] |
kosakaLab | 0:2af3980d8cc8 | 120 | // 出力:フルブリッジのfwdIN, rvsIN用duty, |
kosakaLab | 0:2af3980d8cc8 | 121 | // デッドタイムフラグfDeadtime, モータ逆回転フラグfReverse[i] |
kosakaLab | 0:2af3980d8cc8 | 122 | float duty; // 0-1, PWMデューティ duty of H bridge |
kosakaLab | 0:2af3980d8cc8 | 123 | |
kosakaLab | 0:2af3980d8cc8 | 124 | duty = volt[i]/vMAX; // 指令電圧p.vの値を最大電圧vMAXで割って-1~1にしてdutyに代入 |
kosakaLab | 0:2af3980d8cc8 | 125 | if( duty>=0 ){ // dutyがプラスでモータが順回転のとき |
kosakaLab | 0:2af3980d8cc8 | 126 | if( fReverse[i]==1 ){ // モータが逆回転から順回転に切り替ったとき |
kosakaLab | 0:2af3980d8cc8 | 127 | if( i==0 ){ pwm0p = 0; pwm0m = 0; // デッドタイム作成 |
kosakaLab | 0:2af3980d8cc8 | 128 | }else{ pwm1p = 0; pwm1m = 0; |
kosakaLab | 0:2af3980d8cc8 | 129 | } |
kosakaLab | 0:2af3980d8cc8 | 130 | wait(TS1/10); // make デッドタイム |
kosakaLab | 0:2af3980d8cc8 | 131 | } |
kosakaLab | 0:2af3980d8cc8 | 132 | fReverse[i] = 0; // 逆回転フラグをオフにする |
kosakaLab | 0:2af3980d8cc8 | 133 | if( i==0 ){ pwm0p = duty; pwm0m = 0; // dutyをPWM関数pwm_out()に渡す |
kosakaLab | 0:2af3980d8cc8 | 134 | }else{ pwm1p = duty; pwm1m = 0; |
kosakaLab | 0:2af3980d8cc8 | 135 | } |
kosakaLab | 0:2af3980d8cc8 | 136 | }else{ // dutyがマイナスでモータが逆回転のとき |
kosakaLab | 0:2af3980d8cc8 | 137 | if( fReverse[i]==0 ){ // モータが順回転から逆回転に切り替ったとき |
kosakaLab | 0:2af3980d8cc8 | 138 | if( i==0 ){ pwm0p = 0; pwm0m = 0; // デッドタイム作成 |
kosakaLab | 0:2af3980d8cc8 | 139 | }else{ pwm1p = 0; pwm1m = 0; |
kosakaLab | 0:2af3980d8cc8 | 140 | } |
kosakaLab | 0:2af3980d8cc8 | 141 | wait(TS1/10); // make デッドタイム |
kosakaLab | 0:2af3980d8cc8 | 142 | } |
kosakaLab | 0:2af3980d8cc8 | 143 | fReverse[i] = 1; // 逆回転フラグをオンにする |
kosakaLab | 0:2af3980d8cc8 | 144 | if( i==0 ){ pwm0p = 0; pwm0m = -duty; // dutyをPWM関数pwm_out()に渡す |
kosakaLab | 0:2af3980d8cc8 | 145 | }else{ pwm1p = 0; pwm1m = -duty; // dutyをPWM関数pwm_out()に渡す |
kosakaLab | 0:2af3980d8cc8 | 146 | } |
kosakaLab | 0:2af3980d8cc8 | 147 | } |
kosakaLab | 0:2af3980d8cc8 | 148 | } |
kosakaLab | 0:2af3980d8cc8 | 149 | |
kosakaLab | 0:2af3980d8cc8 | 150 | |
kosakaLab | 0:2af3980d8cc8 | 151 | void data2mbedUSB(){ // PC上のmbed USB ディスクにセーブするためのデータをTS3[s]ごとに代入 save data to mbed USB drive |
kosakaLab | 0:2af3980d8cc8 | 152 | if( _countTS3<1000 ){ // データ数が1,000の5種類のデータをメモリーに貯める |
kosakaLab | 0:2af3980d8cc8 | 153 | data[_countTS3][0]= debug[0]; |
kosakaLab | 0:2af3980d8cc8 | 154 | data[_countTS3][1]= debug[1]; |
kosakaLab | 0:2af3980d8cc8 | 155 | data[_countTS3][2]= K[0].y; |
kosakaLab | 0:2af3980d8cc8 | 156 | data[_countTS3][3]= K[1].y; |
kosakaLab | 0:2af3980d8cc8 | 157 | data[_countTS3][4]= _countTS0*TS0; |
kosakaLab | 0:2af3980d8cc8 | 158 | _countTS3++; |
kosakaLab | 0:2af3980d8cc8 | 159 | } |
kosakaLab | 0:2af3980d8cc8 | 160 | } |
kosakaLab | 0:2af3980d8cc8 | 161 | void timerTS0(){ // タイマーtimerTS0()はTS0[s]ごとにコールされる timer called every TS0[s]. |
kosakaLab | 0:2af3980d8cc8 | 162 | // debug_p26 = 1; |
kosakaLab | 0:2af3980d8cc8 | 163 | _countTS0++; // カウンターに1足す |
kosakaLab | 0:2af3980d8cc8 | 164 | _time += TS0; // 現在の時間にTS0[s]足す |
kosakaLab | 0:2af3980d8cc8 | 165 | |
kosakaLab | 0:2af3980d8cc8 | 166 | controller(); // 制御器 |
kosakaLab | 0:2af3980d8cc8 | 167 | // debug_p26 = 0; |
kosakaLab | 0:2af3980d8cc8 | 168 | } |
kosakaLab | 0:2af3980d8cc8 | 169 | |
kosakaLab | 0:2af3980d8cc8 | 170 | void timerTS1(void const *argument){ // タイマーtimerTS1()はTS1[s]ごとにコールされる |
kosakaLab | 0:2af3980d8cc8 | 171 | } |
kosakaLab | 0:2af3980d8cc8 | 172 | |
kosakaLab | 0:2af3980d8cc8 | 173 | void display2PC(){ // PCのモニタ上のtera termに諸量を表示 display to tera term on PC |
kosakaLab | 0:2af3980d8cc8 | 174 | pc.printf("%8.1f[s]\t%8.2f[V]\t%8.2f [Hz]\t%8.2f\t%8.2f\r\n", |
kosakaLab | 0:2af3980d8cc8 | 175 | _time, K[0].y, K[1].y, K[0].u, debug[0]); |
kosakaLab | 0:2af3980d8cc8 | 176 | // 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 |
kosakaLab | 0:2af3980d8cc8 | 177 | } |
kosakaLab | 0:2af3980d8cc8 | 178 | void timerTS2(){ // タイマーtimerTS2()はTS2[s]ごとにコールされる |
kosakaLab | 0:2af3980d8cc8 | 179 | } |
kosakaLab | 0:2af3980d8cc8 | 180 | void timerTS3(){ // タイマーtimerTS3()はTS3[s]ごとにコールされる |
kosakaLab | 0:2af3980d8cc8 | 181 | data2mbedUSB(); // PC上のmbed USB ディスクにセーブするためのデータをTS3[s]ごとに代入 data2mbedUSB() is called every TS3[s]. |
kosakaLab | 0:2af3980d8cc8 | 182 | } |
kosakaLab | 0:2af3980d8cc8 | 183 | void timerTS4(){ // タイマーtimerTS4()はTS4[s]ごとにコールされる |
kosakaLab | 0:2af3980d8cc8 | 184 | display2PC(); // PCのモニタ上のtera termに文字を表示 display to tera term on PC. display2PC() is called every TS4[s]. |
kosakaLab | 0:2af3980d8cc8 | 185 | } |