Kosaka Lab / Mbed 2 deprecated auto_tracking_car

auto tracking car

Dependencies:   mbed-rtos mbed

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?

UserRevisionLine numberNew 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); // モータ０への入力電圧をPWMにしてHbridgeに出力 volt. to Hbridge
kosakaLab 0:2af3980d8cc8 94 v2Hbridge(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; // ＰＩＤ制御器の出力を計算
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 }