controller.cpp

00001 //  controller.cpp: 制御器
00002 #include "mbed.h"   // mbedマイコンではstdio.hに相当
00003 //#include "QEI.h"    // エンコーダ用ライブラリを使用
00004 
00005 #include "controller.h" //  controller.cpp: モータ制御器（位置制御、電流制御）
00006 PwmOut pwm0p(p21);   // IN1 of TA7291P for right wheel
00007 PwmOut pwm0m(p22);   // IN2 of TA7291P for right wheel
00008 PwmOut pwm1p(p23);   // IN1 of TA7291P for left wheel
00009 PwmOut pwm1m(p24);   // IN2 of TA7291P for left wheel
00010 unsigned char   fReverse[2];  // モータ逆回転フラグ：回転方向が順方向のとき0、逆方向のとき1。[0]が現在の値、[1]はその前の値    reverse direction?
00011 
00012 Serial pc(USBTX, USBRX);    // PCのモニタ上のtera termに文字を表示する宣言
00013 
00014 controller_parameters    K[2];      // 速度制御メインループの定数、変数
00015 float                    volt[2];   // モータへの入力電圧
00016 AnalogOut   analog_out(DA_PORT);    // デバッグ用DA(アナログ信号をDA_PORTに出力)
00017 
00018 unsigned long _countTS0;   // TS0[s]ごとのカウント数
00019 float   _time;          // [s], プログラム開始時からの経過時間
00020 float   debug[20];      // デバッグ用変数
00021 DigitalOut  led1(LED1); // mbedマイコンのLED1を点灯
00022 DigitalOut  led2(LED2); // mbedマイコンのLED2を点灯
00023 DigitalOut  led3(LED3); // mbedマイコンのLED3を点灯
00024 DigitalOut  led4(LED4); // mbedマイコンのLED4を点灯
00025 
00026 
00027 float data[1000][5];    // PC上のmbed USB ディスクにセーブするデータ   memory to save data offline instead of "online fprintf".
00028 unsigned short _countTS3=0;
00029 
00030 DigitalOut  debug_p26(p26); // p17 for debug
00031 DigitalOut  debug_p25(p25); // p17 for debug
00032 
00033 // PIDゲイン
00034 float   Kp[2], Ki[2], Kd[2];
00035 #define vMAX   3.3       // [V], モータ入力電圧の最大値（超えるとこの値に制限する）
00036 
00037 void init_parameters(){
00038 // 制御器の初期値の設定
00039 //  親関数： main()
00040 //  子関数： なし
00041     // 制御器0（人の距離）のPIDゲイン
00042     Kp[0] = 1;
00043     Ki[0] = 0;
00044     Kd[0] = 0;
00045     // 制御器1（人の向き）のPIDゲイン
00046     Kp[1] = 1;
00047     Ki[1] = 0;
00048     Kd[1] = 0;
00049 
00050     K[0].y = K[1].y = 0;        // K[1].yは人の距離[m]、K[1].yは人の向き[deg]
00051     K[0].r = 1;                 // [m],   人の距離の目標値
00052     K[0].r = 0;                 // [deg], 人の向きの目標値
00053     K[0].u = K[1].u =0;         // 制御入力
00054     K[0].eI= K[1].eI=0;         // eの積分値
00055     K[0].e_old = K[1].e_old =0; // 1サンプル過去の偏差e
00056     K[0].eI_old = K[1].eI_old =0; // 1サンプル過去の偏差の積分値eI
00057     volt[0] = volt[1] = 0;      // [V], モータ入力電圧
00058 }
00059 
00060 
00061 void controller(){
00062 // 速度制御メインループ、サンプル時間TS1秒
00063 //      親関数： timerTS0()
00064 //      子関数： PID(), v2Hbridge0()
00065     void PID(int i), v2Hbridge(int i);
00066     int i, f_aw;
00067 
00068     K[0].y = 1; // [m],   人の距離koko
00069     K[1].y = 0; // [deg], 人の向きkoko
00070 
00071     PID(0);    // 人の距離の制御器
00072     PID(1);    // 人の向きの制御器
00073 
00074     volt[0] = K[0].u + K[1].u;  // 右車輪のモータへの電圧
00075     volt[1] = K[0].u - K[1].u;  // 左車輪のモータへの電圧
00076 
00077     // アンチワインドアップ：制御入力が飽和したとき積分項eIを減衰させる   anti-windup: if u=v_ref is saturated, then reduce eI. 
00078     f_aw = 0;
00079     for(i=0;i<2;i++){
00080         if( volt[i] > vMAX ){  // モータ入力電圧がvMAXを超えたとき
00081             volt[i] = vMAX;        // 電圧をvMAXにする
00082             f_aw = 1;
00083         }else if( volt[i] < -vMAX ){  // モータ入力電圧が-vMAXを超えたとき
00084             volt[1] = -vMAX;       // 電圧を-vMAXにする
00085             f_aw = 1;
00086         }
00087     }
00088     if( f_aw ){
00089         K[0].eI = K[0].eI_old; // 積分しなかったことにする
00090         K[1].eI = K[1].eI_old;
00091     }
00092 
00093     v2Hbridge(0);     // モータ０への入力電圧をPWMにしてHbridgeに出力 volt. to Hbridge
00094     v2Hbridge(1);     // モータ１への入力電圧をPWMにしてHbridgeに出力 volt. to Hbridge
00095 }
00096 
00097 
00098 void PID(int i){
00099 //  制御器：偏差eが入力され、制御入力（モータ電圧）uを出力
00100 //      入力：出力 K[i].y, 目標値 K[i].r, PID制御積分項 K[i].eI, サンプル時間 TS1 [s]
00101 //      出力：制御入力（モータ電圧） K[i].u [V]
00102     float  e, ed;
00103 
00104     e = K[i].r - K[i].y;        // 偏差 e の計算
00105 
00106     K[i].eI = K[i].eI + TS1*e;  // e の積分値の計算
00107     K[i].eI_old = K[i].eI;      // eIの1サンプル過去の値を更新
00108 
00109     ed = (e-K[i].e_old)/TS1;    // e の微分値の計算
00110     K[i].e_old = e;             // e の1サンプル過去の値を更新
00111 
00112     K[i].u = Kp[i]*e + Ki[i]*K[i].eI + Kd[i]*ed; // ＰＩＤ制御器の出力を計算
00113 }
00114 
00115 void    v2Hbridge(int i){
00116 // 指令電圧vより、PWM関数pwm_out()のパラメータ（dutyとフラグ）をセット。
00117 //      親関数： timerTS0()
00118 //      子関数： なし
00119 //      入力：電圧指令 p.v [V]
00120 //      出力：フルブリッジのfwdIN, rvsIN用duty,
00121 //            デッドタイムフラグfDeadtime, モータ逆回転フラグfReverse[i]
00122     float           duty;      // 0-1, PWMデューティ    duty of H bridge
00123 
00124     duty = volt[i]/vMAX;    // 指令電圧p.vの値を最大電圧vMAXで割って-1～1にしてdutyに代入
00125     if( duty>=0 ){      // dutyがプラスでモータが順回転のとき
00126         if( fReverse[i]==1 ){       // モータが逆回転から順回転に切り替ったとき
00127             if( i==0 ){  pwm0p = 0;   pwm0m = 0;    // デッドタイム作成
00128             }else{       pwm1p = 0;   pwm1m = 0;
00129             }
00130             wait(TS1/10);           // make デッドタイム
00131         }
00132         fReverse[i] = 0;            // 逆回転フラグをオフにする
00133         if( i==0 ){    pwm0p = duty;    pwm0m = 0;   // dutyをPWM関数pwm_out()に渡す
00134         }else{         pwm1p = duty;    pwm1m = 0;
00135         }
00136     }else{             // dutyがマイナスでモータが逆回転のとき
00137         if( fReverse[i]==0 ){       // モータが順回転から逆回転に切り替ったとき
00138             if( i==0 ){  pwm0p = 0;   pwm0m = 0;    // デッドタイム作成
00139             }else{       pwm1p = 0;   pwm1m = 0;
00140             }
00141             wait(TS1/10);           // make デッドタイム
00142         }
00143         fReverse[i] = 1;            // 逆回転フラグをオンにする
00144         if( i==0 ){    pwm0p = 0;    pwm0m = -duty;   // dutyをPWM関数pwm_out()に渡す
00145         }else{         pwm1p = 0;    pwm1m = -duty;   // dutyをPWM関数pwm_out()に渡す
00146         }
00147     }
00148 }
00149 
00150 
00151 void data2mbedUSB(){    // PC上のmbed USB ディスクにセーブするためのデータをTS3[s]ごとに代入    save data to mbed USB drive 
00152     if( _countTS3<1000 ){   // データ数が1,000の5種類のデータをメモリーに貯める
00153         data[_countTS3][0]= debug[0];
00154         data[_countTS3][1]= debug[1];
00155         data[_countTS3][2]= K[0].y;
00156         data[_countTS3][3]= K[1].y;
00157         data[_countTS3][4]= _countTS0*TS0;
00158         _countTS3++;
00159     }
00160 }
00161 void timerTS0(){    // タイマーtimerTS0()はTS0[s]ごとにコールされる   timer called every TS0[s].
00162 //  debug_p26 = 1;
00163     _countTS0++;    // カウンターに1足す
00164     _time += TS0;   // 現在の時間にTS0[s]足す
00165 
00166     controller();   // 制御器
00167 //  debug_p26 = 0;
00168 }
00169 
00170 void timerTS1(void const *argument){    // タイマーtimerTS1()はTS1[s]ごとにコールされる   
00171 }
00172 
00173 void display2PC(){  //  PCのモニタ上のtera termに諸量を表示 display to tera term on PC
00174     pc.printf("%8.1f[s]\t%8.2f[V]\t%8.2f [Hz]\t%8.2f\t%8.2f\r\n",
00175         _time, K[0].y, K[1].y, K[0].u, debug[0]);
00176 //    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
00177 }
00178 void timerTS2(){    // タイマーtimerTS2()はTS2[s]ごとにコールされる   
00179 }
00180 void timerTS3(){    // タイマーtimerTS3()はTS3[s]ごとにコールされる   
00181     data2mbedUSB();  // PC上のmbed USB ディスクにセーブするためのデータをTS3[s]ごとに代入   data2mbedUSB() is called every TS3[s].
00182 }
00183 void timerTS4(){    // タイマーtimerTS4()はTS4[s]ごとにコールされる   
00184     display2PC();   //  PCのモニタ上のtera termに文字を表示 display to tera term on PC. display2PC() is called every TS4[s].
00185 }