Diff: controller.cpp

Revision:

12:459af534d1ee

Child:

13:ba71733c11d7

diff -r 9747752435d1 -r 459af534d1ee controller.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/controller.cpp	Fri Jan 04 12:00:48 2013 +0000
@@ -0,0 +1,296 @@
+//  BLDCmotor.cpp: 各種３相同期モータに対するセンサあり運転のシミュレーション
+//      Kosaka Lab. 121215
+#include "mbed.h"
+#include "QEI.h"
+
+#include "controller.h"
+#include "Hbridge.h"
+Serial pc(USBTX, USBRX);        // Display on tera term in PC 
+
+motor_parameters            p;  // モータの定数、信号など
+current_loop_parameters     il; // 電流制御マイナーループの定数、変数
+velocity_loop_parameters    vl; // 速度制御メインループの定数、変数
+
+QEI encoder (CH_A, CH_B, NC, N_ENC, QEI::X4_ENCODING);
+//  QEI(PinName     channelA, mbed pin for channel A input.
+//      PinName     channelB, mbed pin for channel B input.
+//      PinName     index,    mbed pin for channel Z input. (index channel input Z phase th=0), (pass NC if not needed).
+//      int         pulsesPerRev, Number of pulses in one revolution(=360 deg).
+//      Encoding    encoding = X2_ENCODING, X2 is default. X2 uses interrupts on the rising and falling edges of only channel A where as 
+//                    X4 uses them on both channels.
+//  )
+//  void     reset (void)
+//     Reset the encoder. 
+//  int      getCurrentState (void)
+//     Read the state of the encoder. 
+//  int      getPulses (void)
+//     Read the number of pulses recorded by the encoder. 
+//  int      getRevolutions (void)
+//     Read the number of revolutions recorded by the encoder on the index channel. 
+/*********** User setting for control parameters (end) ***************/
+
+AnalogOut   analog_out(DA_PORT);
+AnalogIn VshuntR_Uplus(p19);    // *3.3 [V], Volt of shunt R_SHUNT[Ohm]. The motor current i = v_shunt_r/R_SHUNT [A]
+AnalogIn VshuntR_Uminus(p20);    // *3.3 [V], Volt of shunt R_SHUNT[Ohm]. The motor current i = v_shunt_r/R_SHUNT [A]
+AnalogIn VshuntR_Vplus(p16);    // *3.3 [V], Volt of shunt R_SHUNT[Ohm]. The motor current i = v_shunt_r/R_SHUNT [A]
+AnalogIn VshuntR_Vminus(p17);    // *3.3 [V], Volt of shunt R_SHUNT[Ohm]. The motor current i = v_shunt_r/R_SHUNT [A]
+
+unsigned long _count;   // sampling number
+float   _time;          // time[s]
+float   debug[20];      // for debug
+float   disp[10];       // for printf to avoid interrupted by quicker process
+DigitalOut  led1(LED1);
+DigitalOut  led2(LED2);
+DigitalOut  led3(LED3);
+DigitalOut  led4(LED4);
+
+#ifdef  GOOD_DATA
+float data[1000][5];    // memory to save data offline instead of "online fprintf".
+unsigned int    count3; // 
+unsigned int    count2=(int)(TS2/TS0); // 
+unsigned short _count_data=0;
+#endif
+DigitalOut  debug_p26(p26); // p17 for debug
+DigitalOut  debug_p25(p25); // p17 for debug
+//DigitalOut  debug_p24(p24); // p17 for debug
+//AnalogIn VCC(p19);          // *3.3 [V], Volt of VCC for motor
+//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.
+
+unsigned short  f_find_origin;  // flag to find the origin of the rotor angle theta
+
+#if 1   //BUG!! if move sqrt2 to fast_math.h, sim starts and completed without working!?
+float  sqrt2(float x){    // √xのx=1まわりのテイラー展開 √x = 1 + 1/2*(x-1) -1/4*(x-1)^2 + ...
+//  return((1+x)*0.5);      // 一次近似
+    return(x+(1-x*x)*0.25); // 二次近似
+}
+#endif
+
+void init_parameters(){    // IPMSMの機器定数等の設定, 制御器の初期化
+#ifdef SIMULATION
+    p.L = 0.0063;   // H
+    p.R = 0.143;    // Ω
+    p.phi = 0.176;  // V s
+    p.Jm = 0.00018; // Nms^2
+    p.p = 2;        // 極対数
+#endif
+    p.th[0] = p.th[1] = 0;
+    p.w = 0;
+    p.i = 0;
+    p.v =0;
+
+    p.w = 0;
+
+    // 制御器の初期化
+    vl.i_ref=0;         // 電流指令[A]
+    vl.w_lpf = 0;       // 検出した速度[rad/s]
+    vl.eI_w = 0;        // 速度制御用偏差の積分値（積分項）
+    il.eI_i = 0;        // 電流制御用偏差の積分値（積分項）
+#ifndef SIMULATION
+    encoder.reset();    // set encoder counter zero
+    p.th[0] = p.th[1] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI;   // get angle [rad] from encoder
+#endif
+}
+
+void idq_control(){
+//  ｄｑ座標電流PID制御器（電流マイナーループのフィードバック制御）
+//      入力：指令電流 i_ref [A], 実電流 p.i [A], PI制御積分項 eI, サンプル時間 TS0 [s]
+//      出力：電圧指令 v_ref [A]
+    float  e;
+
+//debug[0]=il.i_ref;
+    // ｄｑ電流偏差の計算
+    e = il.i_ref - p.i;
+
+    // ｄｑ電流偏差の積分項の計算
+    il.eI_i = il.eI_i + TS0*e;
+
+    // ＰＩ制御
+    il.v_ref = iKP*e + iKI*il.eI_i;
+
+}
+
+void current_loop(){    // 電流制御マイナーループ
+    // 電流センサによってiu, iv を検出
+#ifndef SIMULATION
+    p.i = (VshuntR_Uplus - VshuntR_Uminus) /R_SHUNT;      // get i [A] from shunt resistance;
+#endif
+//debug[0]=p.i;
+    // dq電流制御（電流フィードバック制御）
+debug[0]=il.i_ref;
+#ifdef USE_CURRENT_CONTROL
+    idq_control();
+#else
+    il.v_ref = il.i_ref/iMAX*vMAX;
+#endif
+    // 電圧指令の大きさをMAX制限
+    // anti-windup: if u=v_ref is saturated, then reduce eI. 
+    if( il.v_ref > vMAX ){
+        il.eI_i -= (il.v_ref - vMAX)/iKI;    if( il.eI_i<0 ){   il.eI_i=0;}
+        il.v_ref = vMAX;
+    }else if( il.v_ref < -vMAX ){
+        il.eI_i -= (il.v_ref + vMAX)/iKI;    if( il.eI_i>0 ){   il.eI_i=0;}
+        il.v_ref = -vMAX;
+    }
+    p.v = il.v_ref;
+
+    p.th[1] = p.th[0];  // thを更新
+}
+
+
+void vel_control(){
+//  速度制御器：速度偏差が入力され、q軸電流指令を出力。
+//      入力：指令速度 w_ref [rad/s], 実速度 w_lpf [rad/s], PI制御積分項 eI, サンプル時間 TS1 [s]
+//      出力：電流指令 i_ref [A]
+    float  e;
+
+    // 速度偏差の計算
+    e = vl.w_ref - vl.w_lpf;
+
+    // 速度偏差の積分値の計算
+    vl.eI_w = vl.eI_w + TS1*e;
+
+    // ＰＩ制御
+    vl.i_ref = wKp*e + wKi*vl.eI_w;
+}
+
+void velocity_loop(){   // 速度制御メインループ(w_ref&beta_ref to idq_ref)
+    float  tmp;
+
+    // 速度ωを求めるために、位置θをオイラー微分して、一次ローパスフィルタに通す。
+#if 1
+    tmp = p.th[0]-p.th[1];
+    while( tmp> PI ){ tmp -= 2*PI;}
+    while( tmp<-PI ){ tmp += 2*PI;}
+    vl.w_lpf = iLPF*vl.w_lpf + tmp/TS0 *(1-iLPF);
+#else
+    vl.w_lpf = p.th[0];
+#endif
+    // 速度制御：速度偏差が入力され、電流指令を出力。
+    vel_control();
+
+    // 電流指令のMAX制限（異常に大きい指令値を制限する）
+    // anti-windup: if u=i_ref is saturated, then reduce eI. 
+    if( vl.i_ref > iMAX ){
+        vl.eI_w -= (vl.i_ref - iMAX)/wKi;    if( vl.eI_w<0 ){   vl.eI_w=0;}
+        vl.i_ref = iMAX;
+    }else if( vl.i_ref < -iMAX ){
+        vl.eI_w -= (vl.i_ref + iMAX)/wKi;    if( vl.eI_w>0 ){   vl.eI_w=0;}
+        vl.i_ref = -iMAX;
+    }
+//debug[0]=vl.eI_w;
+
+    // 電流の目標値を速度制御メインループから電流制御マイナーループへ渡す。
+    il.i_ref = vl.i_ref;
+//debug[0]=il.i_ref;
+}
+
+void    v2Hbridge(){ // vより、PWMを発生
+    float   duty;
+
+//    duty = (p.v/vMAX+1)*0.5;
+//    IN.duty = duty;
+    duty = p.v/vMAX;
+    if( duty>=0 ){
+        IN.duty = duty;
+        if( IN.fReverse[0]==1 ){
+            IN.fDeadtime = 1;
+        }
+        IN.fReverse[0] = 0;
+    }else{
+        IN.duty = -duty;
+        if( IN.fReverse[0]==0 ){
+            IN.fDeadtime = 1;
+        }
+        IN.fReverse[0] = 1;
+    }
+}
+
+#ifdef SIMULATION
+void    sim_motor(){
+//  モータシミュレータ
+//      入力信号：電圧p.v [V]、負荷トルクp.TL [Nm]
+//      出力信号：モータ角度p.th[0] [rad], モータ速度p.w [rad/s], モータ電流p.i [A]
+    float i_dot, Tall, TL;
+analog_out=p.v/100.+0.5;//debug
+//debug[0]=p.v;
+    // get i
+    i_dot = (1.0/p.L) * ( p.v - (p.R*p.i + p.w*p.phi) );
+    p.i = p.i + TS0*i_dot;
+
+    // モータトルクの計算
+    p.Tm = p.p * p.phi * p.i;
+
+    // モータ速度ωの計算
+    if( abs(p.w) > 5*2*PI )
+        if( p.w>=0 )    TL= p.TL;
+        else            TL=-p.TL;
+    else
+        TL = p.w/(5*2*PI)*p.TL;
+
+    Tall = p.Tm - TL;
+    p.w = p.w + TS0 * (1.0 / p.Jm) * Tall;
+
+    // モータ角度θの計算
+    p.th[0] = p.th[0] + TS0 * p.w;
+    if( p.th[0]>2*PI)
+        p.th[0] = p.th[0] - 2*PI;
+
+    if( p.th[0]<0 )
+        p.th[0] = p.th[0] + 2*PI;
+//debug[0]=p.v;
+}
+#endif
+
+void data2mbedUSB(){    // save data to mbed USB drive 
+    if( _count_data<1000 ){
+        data[_count_data][0]=p.th[0]/*vl.w_ref*/; data[_count_data][1]=debug[0];
+        data[_count_data][2]=vl.w_lpf; data[_count_data][3]=_count*TS0; data[_count_data][4]=il.v_ref;
+        _count_data++;
+    }
+#if 0
+  if( _count_data<500 ){
+    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];
+    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];
+    debug[10]=p.iuvw[1]; debug[11]=p.iuvw[2]; debug[12]=p.idq[0]; debug[13]=p.idq[1]; debug[14]=p.TL;
+    debug[15]=p.Tm; debug[16]=p.w; debug[17]=vl.w_lpf; debug[18]=p.th[0]; debug[19]=_count*TS0;//_time;
+//BUG    for(j=0;j<19;j++){  fprintf( fp, "%f, ",debug[j]);} fprintf( fp, "%f\n",debug[19]);
+    for(j=0;j<19;j++){  printf("%f, ",debug[j]);} printf("%f\n",debug[19]);
+//    for(j=0;j<19;j++){  pc.printf("%f, ",debug[j]);} pc.printf("%f\n",debug[19]);
+  }
+#endif
+}
+void timerTS0(){    // timer called every TS0[s].
+    debug_p26 = 1;
+    _count++;
+    _time += TS0;
+    
+    current_loop(); // 電流制御マイナーループ(i_ref to volt)
+    v2Hbridge();     // volt. to Hbridge
+ #ifdef SIMULATION
+//debug[0]=p.v;
+    // モータシミュレータ
+    sim_motor();    // IPM, ｄｑ座標
+ #else
+    p.th[1] = p.th[0];
+    p.th[0] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI;   // get angle [rad] from encoder
+ #endif
+    debug_p26 = 0;
+}
+void timerTS1(void const *argument){
+    debug_p25 = 1;
+    velocity_loop();    // 速度制御メインループ(w_ref&beta_ref to idq_ref)
+    debug_p25 = 0;
+}
+
+void display2PC(){  // 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, il.v_ref, vl.w_lpf/(2*PI), vl.w_ref/(2*PI), debug[0]);  // print to tera term
+//    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(){
+}
+void timerTS3(){
+    data2mbedUSB();  // data2mbedUSB() is called every TS3[s].
+}
+void timerTS4(){
+    display2PC();  // display to tera term on PC. display2PC() is called every TS4[s].
+}