controller.cpp

00001 //  controller.cpp: モータ制御器（位置制御、電流制御）
00002 #include "mbed.h"   // mbedマイコンではstdio.hに相当
00003 #include "QEI.h"    // エンコーダ用ライブラリを使用
00004 
00005 #include "controller.h" //  controller.cpp: モータ制御器（位置制御、電流制御）
00006 #include "Hbridge.h"    //  Hbridge.cpp: モータ駆動用ドライバのフルブリッジ（Ｈブリッジ）をＰＷＭ駆動する。
00007 //Serial pc(USBTX, USBRX);    // PCのモニタ上のtera termに文字を表示する宣言
00008 
00009 motor_parameters            p;  // モータの定数、信号など
00010 current_loop_parameters     il; // 電流制御マイナーループの定数、変数
00011 velocity_loop_parameters    vl; // 速度制御メインループの定数、変数
00012 
00013 QEI encoder (CH_A, CH_B, NC, N_ENC, QEI::X4_ENCODING);  // エンコーダ用ライブラリを使用
00014 //  QEI(PinName     channelA, mbed pin for channel A input.
00015 //      PinName     channelB, mbed pin for channel B input.
00016 //      PinName     index,    mbed pin for channel Z input. (index channel input Z phase th=0), (pass NC if not needed).
00017 //      int         pulsesPerRev, Number of pulses in one revolution(=360 deg).
00018 //      Encoding    encoding = X2_ENCODING, X2 is default. X2 uses interrupts on the rising and falling edges of only channel A where as 
00019 //                    X4 uses them on both channels.
00020 //  )
00021 //  void     reset (void)
00022 //     Reset the encoder. 
00023 //  int      getCurrentState (void)
00024 //     Read the state of the encoder. 
00025 //  int      getPulses (void)
00026 //     Read the number of pulses recorded by the encoder. 
00027 //  int      getRevolutions (void)
00028 //     Read the number of revolutions recorded by the encoder on the index channel. 
00029 /*********** User setting for control parameters (end) ***************/
00030 
00031 AnalogOut   analog_out(DA_PORT);    // デバッグ用DA(アナログ信号をDA_PORTに出力)
00032 
00033 unsigned long _countTS0;   // TS0[s]ごとのカウント数
00034 float   _time;          // [s], プログラム開始時からの経過時間
00035 float   debug[20];      // デバッグ用変数
00036 DigitalOut  led1(LED1); // mbedマイコンのLED1を点灯
00037 DigitalOut  led2(LED2); // mbedマイコンのLED2を点灯
00038 DigitalOut  led3(LED3); // mbedマイコンのLED3を点灯
00039 DigitalOut  led4(LED4); // mbedマイコンのLED4を点灯
00040 
00041 
00042 float data[1000][5];    // PC上のmbed USB ディスクにセーブするデータ   memory to save data offline instead of "online fprintf".
00043 //unsigned int    count3; // 
00044 //unsigned int    count2=(int)(TS2/TS0); // 
00045 unsigned short _countTS3=0;
00046 
00047 DigitalOut  debug_p26(p26); // p17 for debug
00048 DigitalOut  debug_p25(p25); // p17 for debug
00049 //DigitalOut  debug_p24(p24); // p17 for debug
00050 //AnalogIn VCC(p19);          // *3.3 [V], Volt of VCC for motor
00051 //AnalogIn VCC2(p20);         // *3.3 [V], Volt of (VCC-R i), R=2.5[Ohm]. R is for preventing too much i when deadtime is failed.
00052 
00053 
00054 void init_parameters(){
00055 // モータ機器定数等、モータ・制御器の初期値の設定
00056 //  親関数： main()
00057 //  子関数： なし
00058 #ifdef SIMULATION
00059     // モータ機器定数の設定
00060     p.L = 0.0063;   // [H], インダクタンス
00061     p.R = 0.143;    // [Ω], モータ巻線抵抗
00062     p.phi = 0.176;  // [V s], 永久磁石の鎖交磁束
00063     p.Jm = 0.00018; // [Nms^2], イナーシャ
00064     p.p = 2;        // 極対数
00065 #endif
00066     // モータの初期値の設定
00067 #ifdef SIMULATION
00068     p.th[0] = p.th[1] = 0;  // [rad], ロータの回転角, th[0]は現在の値, th[1]はTS0[s]だけ過去の値
00069 #else
00070     encoder.reset();    // 現在の回転角を原点に設定（エンコーダのカウント数をゼロに設定）  set encoder counter zero
00071     p.th[0] = p.th[1] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI;   // [rad], ロータの回転角をエンコーダ検出値に設定, th[0]は現在の値, th[1]はTS0[s]だけ過去の値 get angle [rad] from encoder
00072 #endif
00073     p.w = 0;    // [rad/s], モータの回転速度
00074     p.i = 0;    // [A], モータ電流
00075     p.v =0;     // [V], モータ電圧
00076 
00077     // 制御器の初期値の設定
00078     vl.i_ref=0;         // 電流指令[A]
00079     vl.w_lpf = 0;       // 検出した速度[rad/s]
00080     vl.eI = 0;          // 速度制御用偏差の積分値（積分項）
00081     vl.e_old = 0;       // 速度制御用偏差の1サンプル過去の値
00082     il.eI = 0;          // 電流制御用偏差の積分値（積分項）
00083     il.e_old = 0;       // 電流制御用偏差の1サンプル過去の値
00084 }
00085 
00086 void i_control(){
00087 //  電流PID制御器（電流マイナーループのフィードバック制御）
00088 //      親関数： current_loop()
00089 //      子関数： なし
00090 //      入力：指令電流 il.i_ref [A], 実電流 p.i [A], PID制御積分項 il.eI, サンプル時間 TS0 [s]
00091 //      出力：電圧指令 il.v_ref [A]
00092     float  e, ed;
00093 
00094     e = il.i_ref - p.i;     // 電流偏差の計算
00095 
00096     il.eI = il.eI + TS0*e;  // 電流偏差の積分項の計算
00097 
00098     ed = (e-il.e_old)/TS0;  // 電流偏差の微分値の計算
00099     il.e_old = e;           // 電流偏差の1サンプル過去の値を更新
00100 
00101     il.v_ref = iKP*e + iKI*il.eI + iKD*ed;  // ＰＩＤ制御器の出力を計算
00102 }
00103 
00104 void current_loop(){
00105 // 電流制御マイナーループ、サンプル時間TS0秒
00106 //      親関数： timerTS0()
00107 //      子関数： i_control()
00108 //      入力：指令電流 il.i_ref [A], 実電流 p.i [A]
00109 //      出力：電圧指令 p.v [V]
00110 
00111 #ifdef USE_CURRENT_CONTROL
00112     i_control();    // 電流制御（電流フィードバック制御）
00113 #else
00114     il.v_ref = il.i_ref/iMAX*vMAX;  // 速度制御の出力をそのまま電圧指令にする
00115 #endif
00116     // 電圧指令の大きさをMAX制限
00117     // アンチワインゴアップ：制御入力v_refが飽和したとき積分項eIを減衰させる   anti-windup: if u=v_ref is saturated, then reduce eI. 
00118     if( il.v_ref > vMAX ){  // 電圧指令がプラス側に大きすぎるとき
00119         il.eI -= (il.v_ref - vMAX)/iKI; // 電圧指令がvMAXと等しくなる積分項を計算
00120         if( il.eI<0 ){   il.eI=0;}      // 積分項が負になるとゼロにする
00121         il.v_ref = vMAX;                // 電圧指令をvMAXにする
00122     }else if( il.v_ref < -vMAX ){   // 電圧指令がマイナス側に大きすぎるとき
00123         il.eI -= (il.v_ref + vMAX)/iKI; // 電圧指令が-vMAXと等しくなる積分項を計算
00124         if( il.eI>0 ){   il.eI=0;}      // 積分項が正になるとゼロにする
00125         il.v_ref = -vMAX;               // 電圧指令を-vMAXにする
00126     }
00127     p.v = il.v_ref;     // 指令電圧をp.vに格納してv2Hbridge()に渡す
00128 }
00129 
00130 
00131 void vel_control(){
00132 //  速度制御器：速度偏差が入力され、q軸電流指令を出力。
00133 //      入力：指令速度 vl.w_ref [rad/s], 実速度 vl.w_lpf [rad/s], PI制御積分項 vl.eI, サンプル時間 TS1 [s]
00134 //      出力：電流指令 vl.i_ref [A]
00135     float  e, ed;
00136 
00137     e = vl.w_ref - vl.w_lpf;        // 速度偏差の計算
00138 debug[1]=vl.w_ref/2/PI;//[Hz], for debug
00139 
00140     vl.eI = vl.eI + TS1*e;          // 速度偏差の積分値の計算
00141 
00142     ed = (e-vl.e_old)/TS1;          // 速度偏差の微分値の計算
00143     vl.e_old = e;                   // 速度偏差の1サンプル過去の値を更新
00144 
00145     vl.i_ref = wKp*e + wKi*vl.eI + wKd*ed; // ＰＩＤ制御器の出力を計算
00146 }
00147 
00148 void velocity_loop(){
00149 // 速度制御メインループ、サンプル時間TS1秒
00150 //      親関数： timerTS1()
00151 //      子関数： vel_control()
00152 //      入力：指令速度 vl.w_ref [rad/s], 実速度 vl.w_lpf [rad/s]
00153 //      出力：電圧指令 il.i_ref [A]
00154     float  tmp;
00155 
00156 #if 1
00157     // 速度ωを求めるために、位置θをオイラー微分して、一次ローパスフィルタに通す。
00158     tmp = p.th[0]-p.th[1];          // 回転角の差分を取る
00159     while( tmp> PI ){ tmp -= 2*PI;} // 差分の値域を-π～+πに設定
00160     while( tmp<-PI ){ tmp += 2*PI;} //  〃
00161     vl.w_lpf = iLPF*vl.w_lpf + tmp/TS0 *(1-iLPF);   // オイラー微分近似＋１次ＬＰＦで現在速度を計算
00162 #else
00163     vl.w_lpf = p.th[0];
00164 #endif
00165 
00166     vel_control();    // 速度制御：速度偏差が入力され、電流指令を出力。
00167 
00168     // 電流指令の大きさをMAX制限
00169     // アンチワインドアップ：制御入力i_refが飽和したとき積分項eIを減衰させる   anti-windup: if u=v_ref is saturated, then reduce eI. 
00170     if( vl.i_ref > iMAX ){  // 電流指令がプラス側に大きすぎるとき
00171         vl.eI -= (vl.i_ref - iMAX)/wKi; // 電流指令がvMAXと等しくなる積分項を計算
00172         if( vl.eI<0 ){   vl.eI=0;}      // 積分項が負になるとゼロにする
00173         vl.i_ref = iMAX;                // 電流指令をvMAXにする
00174     }else if( vl.i_ref < -iMAX ){   // 電流指令がマイナス側に大きすぎるとき
00175         vl.eI -= (vl.i_ref + iMAX)/wKi; // 電流指令が-vMAXと等しくなる積分項を計算
00176         if( vl.eI>0 ){   vl.eI=0;}      // 積分項が正になるとゼロにする
00177         vl.i_ref = -iMAX;               // 電流指令を-vMAXにする
00178     }
00179 
00180     il.i_ref = vl.i_ref;    // 電流の目標値を速度制御メインループから電流制御マイナーループへ渡す。
00181 }
00182 
00183 void    v2Hbridge(){
00184 // 指令電圧vより、PWM関数pwm_out()のパラメータ（dutyとフラグ）をセット。
00185 //      親関数： timerTS0()
00186 //      子関数： なし
00187 //      入力：電圧指令 p.v [V]
00188 //      出力：フルブリッジのfwdIN, rvsIN用duty,
00189 //            デッドタイムフラグfDeadtime, モータ逆回転フラグfReverse
00190     float   duty;
00191 
00192     duty = p.v/vMAX;    // 指令電圧p.vの値を最大電圧vMAXで割って-1～1にしてdutyに代入
00193     if( duty>=0 ){      // dutyがプラスでモータが順回転のとき
00194         IN.duty = duty;     // dutyをPWM関数pwm_out()に渡す
00195         if( IN.fReverse==1 ){       // モータが逆回転から順回転に切り替ったとき
00196             IN.fDeadtime = 1;       // デッドタイム要求フラグをオンにする
00197         }
00198         IN.fReverse = 0;            // 逆回転フラグをオフにする
00199     }else{             // dutyがマイナスでモータが逆回転のとき
00200         IN.duty = -duty;    // dutyの絶対値をPWM関数pwm_out()に渡す
00201         if( IN.fReverse==0 ){       // モータが順回転から逆回転に切り替ったとき
00202             IN.fDeadtime = 1;       // デッドタイム要求フラグをオンにする
00203         }
00204         IN.fReverse = 1;            // 逆回転フラグをオンにする
00205     }
00206 }
00207 
00208 #ifdef SIMULATION
00209 void    sim_motor(){
00210 //  ブラシ付きDCモータシミュレータ
00211 //      入力信号：電圧p.v [V]、負荷トルクp.TL [Nm]
00212 //      出力信号：モータ角度p.th[0] [rad], モータ速度p.w [rad/s], モータ電流p.i [A]
00213     float i_dot, Tall, TL;
00214 analog_out=p.v/100.+0.5;//debug
00215 //debug[0]=p.v;
00216     // 電圧方程式より、指令電圧から電流を計算
00217     i_dot = (1.0/p.L) * ( p.v - (p.R*p.i + p.w*p.phi) );    // 電圧方程式より電流の微分値を計算
00218     p.i = p.i + TS0*i_dot;                                  // オイラー近似微分で電流の微分値を積分して電流を求める
00219 
00220     // トルク方程式より、電流からモータトルクを計算
00221     p.Tm = p.p * p.phi * p.i;
00222 
00223     // モータ速度ωの計算
00224     if( abs(p.w) > 5*2*PI ){    // 速度が5rps以上のとき、減速するように負荷トルクTLをセット
00225         if( p.w>=0 ){   TL= p.TL;}  // 速度が正なら負荷トルクを+TLにセット
00226         else{           TL=-p.TL;}  // 速度が負なら負荷トルクを-TLにセット
00227     }else{                      // 速度が5rps以下のとき、速度に比例した負荷トルクをセット
00228         TL = p.w/(5*2*PI)*p.TL;
00229     }
00230 
00231     Tall = p.Tm - TL;   // モータのシャフトにかかるトルクを計算
00232     p.w = p.w + TS0 * (1.0 / p.Jm) * Tall;  // モータの機械的特性をシミュレートしてトルクから速度を計算
00233 
00234     // モータ角度θの計算
00235     p.th[0] = p.th[0] + TS0 * p.w;  // オイラー近似微分で速度を積分して回転角を計算
00236 
00237     // 回転角の値域を０～２πにする
00238     if( p.th[0]>2*PI)   // 回転角が２π以上のとき
00239         p.th[0] = p.th[0] - 2*PI;   // ２π引く 
00240     if( p.th[0]<0 )     // 回転角が負のとき
00241         p.th[0] = p.th[0] + 2*PI;   // ２π足す
00242 }
00243 #endif
00244 
00245 void data2mbedUSB(){    // PC上のmbed USB ディスクにセーブするためのデータをTS3[s]ごとに代入    save data to mbed USB drive 
00246     if( _countTS3<1000 ){   // データ数が1,000の5種類のデータをメモリーに貯める
00247         data[_countTS3][0]=debug[0]; data[_countTS3][1]=debug[1];
00248         data[_countTS3][2]=vl.w_lpf; data[_countTS3][3]=_countTS0*TS0; data[_countTS3][4]=il.v_ref;
00249         _countTS3++;
00250     }
00251 #if 0
00252   if( _countTS3<500 ){
00253     debug[0]=p.vab[0]; debug[1]=p.vab[1]; debug[2]=il.vdq_ref[0]; debug[3]=il.vdq_ref[1]; debug[4]=p.iab[0];
00254     debug[5]=p.iab[1]; debug[6]=p.vuvw[0]; debug[7]=p.vuvw[1]; debug[8]=p.vuvw[2]; debug[9]=p.iuvw[0];
00255     debug[10]=p.iuvw[1]; debug[11]=p.iuvw[2]; debug[12]=p.idq[0]; debug[13]=p.idq[1]; debug[14]=p.TL;
00256     debug[15]=p.Tm; debug[16]=p.w; debug[17]=vl.w_lpf; debug[18]=p.th[0]; debug[19]=_countTS0*TS0;//_time;
00257 //BUG    for(j=0;j<19;j++){  fprintf( fp, "%f, ",debug[j]);} fprintf( fp, "%f\n",debug[19]);
00258     for(j=0;j<19;j++){  printf("%f, ",debug[j]);} printf("%f\n",debug[19]);
00259 //    for(j=0;j<19;j++){  pc.printf("%f, ",debug[j]);} pc.printf("%f\n",debug[19]);
00260   }
00261 #endif
00262 }
00263 void timerTS0(){    // タイマーtimerTS0()はTS0[s]ごとにコールされる   timer called every TS0[s].
00264 //  debug_p26 = 1;
00265     _countTS0++;    // カウンターに1足す
00266     _time += TS0;   // 現在の時間にTS0[s]足す
00267     
00268     current_loop();  // 電流制御マイナーループ(指令電流i_refからPID制御で指令電圧を計算)
00269     v2Hbridge();     // 指令電圧を見てPWM関数pwm_out()のパラメータを決める volt. to Hbridge
00270 
00271     // モータ回転角の検出
00272     p.th[1] = p.th[0];  // // 1サンプル前の回転角p.th[1]を更新
00273 #ifdef SIMULATION
00274     sim_motor();    // モータシミュレータ
00275 #else
00276     p.th[0] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI;   // エンコーダで回転角を検出 get angle [rad] from encoder
00277 #endif
00278 //  debug_p26 = 0;
00279 }
00280 
00281 void timerTS1(void const *argument){    // タイマーtimerTS1()はTS1[s]ごとにコールされる   
00282 //  debug_p25 = 1;
00283     velocity_loop();    // 速度制御メインループ(指令速度w_refから指令電流i_refを求める)
00284 //  debug_p25 = 0;
00285 }
00286 
00287 void display2PC(){  //  PCのモニタ上のtera termに諸量を表示 display to tera term on PC
00288     printf("%8.1f[s]\t%8.2f[V]\t%8.2f [Hz]\t%8.2f\t%8.2f\r\n",
00289         _time, il.v_ref, vl.w_lpf/(2*PI), vl.w_ref/(2*PI), debug[0]);
00290 //    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
00291 }
00292 void timerTS2(){    // タイマーtimerTS2()はTS2[s]ごとにコールされる   
00293 }
00294 void timerTS3(){    // タイマーtimerTS3()はTS3[s]ごとにコールされる   
00295     data2mbedUSB();  // PC上のmbed USB ディスクにセーブするためのデータをTS3[s]ごとに代入   data2mbedUSB() is called every TS3[s].
00296 }
00297 void timerTS4(){    // タイマーtimerTS4()はTS4[s]ごとにコールされる   
00298     display2PC();   //  PCのモニタ上のtera termに文字を表示 display to tera term on PC. display2PC() is called every TS4[s].
00299 }