Kosaka Lab / Mbed 2 deprecated BLDCmotor

UVW 3 phases Brushless DC motor control

Dependencies:   QEI mbed-rtos mbed

Fork of BLDCmotor by manabu kosaka

controller.cpp@12:a4b17bb682eb, 2012-12-21 (annotated)

Committer:
kosaka
Date:
Fri Dec 21 22:06:56 2012 +0000
Revision:
12:a4b17bb682eb
Child:
13:791e20f1af43
121222a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kosaka 12:a4b17bb682eb 1 // BLDCmotor.cpp: 各種３相同期モータに対するセンサあり運転のシミュレーション
kosaka 12:a4b17bb682eb 2 // Kosaka Lab. 121215
kosaka 12:a4b17bb682eb 3 #include "mbed.h"
kosaka 12:a4b17bb682eb 4 #include "QEI.h"
kosaka 12:a4b17bb682eb 5
kosaka 12:a4b17bb682eb 6 #include "controller.h"
kosaka 12:a4b17bb682eb 7 #include "UVWpwm.h"
kosaka 12:a4b17bb682eb 8 #include "fast_math.h"
kosaka 12:a4b17bb682eb 9 Serial pc(USBTX, USBRX); // Display on tera term in PC
kosaka 12:a4b17bb682eb 10
kosaka 12:a4b17bb682eb 11 motor_parameters p; // モータの定数、信号など
kosaka 12:a4b17bb682eb 12 current_loop_parameters il; // 電流制御マイナーループの定数、変数
kosaka 12:a4b17bb682eb 13 velocity_loop_parameters vl; // 速度制御メインループの定数、変数
kosaka 12:a4b17bb682eb 14
kosaka 12:a4b17bb682eb 15 QEI encoder (CH_A, CH_B, NC, N_ENC, QEI::X4_ENCODING);
kosaka 12:a4b17bb682eb 16 // QEI(PinName channelA, mbed pin for channel A input.
kosaka 12:a4b17bb682eb 17 // PinName channelB, mbed pin for channel B input.
kosaka 12:a4b17bb682eb 18 // PinName index, mbed pin for channel Z input. (index channel input Z phase th=0), (pass NC if not needed).
kosaka 12:a4b17bb682eb 19 // int pulsesPerRev, Number of pulses in one revolution(=360 deg).
kosaka 12:a4b17bb682eb 20 // Encoding encoding = X2_ENCODING, X2 is default. X2 uses interrupts on the rising and falling edges of only channel A where as
kosaka 12:a4b17bb682eb 21 // X4 uses them on both channels.
kosaka 12:a4b17bb682eb 22 // )
kosaka 12:a4b17bb682eb 23 // void reset (void)
kosaka 12:a4b17bb682eb 24 // Reset the encoder.
kosaka 12:a4b17bb682eb 25 // int getCurrentState (void)
kosaka 12:a4b17bb682eb 26 // Read the state of the encoder.
kosaka 12:a4b17bb682eb 27 // int getPulses (void)
kosaka 12:a4b17bb682eb 28 // Read the number of pulses recorded by the encoder.
kosaka 12:a4b17bb682eb 29 // int getRevolutions (void)
kosaka 12:a4b17bb682eb 30 // Read the number of revolutions recorded by the encoder on the index channel.
kosaka 12:a4b17bb682eb 31 /*********** User setting for control parameters (end) ***************/
kosaka 12:a4b17bb682eb 32
kosaka 12:a4b17bb682eb 33 AnalogOut analog_out(DA_PORT);
kosaka 12:a4b17bb682eb 34 AnalogIn VshuntR_Uplus(p19); // *3.3 [V], Volt of shunt R_SHUNT[Ohm]. The motor current i = v_shunt_r/R_SHUNT [A]
kosaka 12:a4b17bb682eb 35 AnalogIn VshuntR_Uminus(p20); // *3.3 [V], Volt of shunt R_SHUNT[Ohm]. The motor current i = v_shunt_r/R_SHUNT [A]
kosaka 12:a4b17bb682eb 36 AnalogIn VshuntR_Vplus(p16); // *3.3 [V], Volt of shunt R_SHUNT[Ohm]. The motor current i = v_shunt_r/R_SHUNT [A]
kosaka 12:a4b17bb682eb 37 AnalogIn VshuntR_Vminus(p17); // *3.3 [V], Volt of shunt R_SHUNT[Ohm]. The motor current i = v_shunt_r/R_SHUNT [A]
kosaka 12:a4b17bb682eb 38
kosaka 12:a4b17bb682eb 39 unsigned long _count; // sampling number
kosaka 12:a4b17bb682eb 40 float _time; // time[s]
kosaka 12:a4b17bb682eb 41 float _r; // reference signal
kosaka 12:a4b17bb682eb 42 float _th=0; // [rad], motor angle, control output of angle controller
kosaka 12:a4b17bb682eb 43 float _i=0; // [A], motor current, control output of current controller
kosaka 12:a4b17bb682eb 44 float _e=0; // e=r-y for PID controller
kosaka 12:a4b17bb682eb 45 float _eI=0; // integral of e for PID controller
kosaka 12:a4b17bb682eb 46 float _iref; // reference current iref [A], output of angle th_contorller
kosaka 12:a4b17bb682eb 47 float _u; // control input[V], motor input volt.
kosaka 12:a4b17bb682eb 48 float _ei=0; // e=r-y for current PID controller
kosaka 12:a4b17bb682eb 49 float _eiI=0; // integral of e for current PID controller
kosaka 12:a4b17bb682eb 50 unsigned char _f_u_plus=1;// sign(u)
kosaka 12:a4b17bb682eb 51 unsigned char _f_umax=0;// flag showing u is max or not
kosaka 12:a4b17bb682eb 52 unsigned char _f_imax=0;// flag showing i is max or not
kosaka 12:a4b17bb682eb 53 float debug[20]; // for debug
kosaka 12:a4b17bb682eb 54 float disp[10]; // for printf to avoid interrupted by quicker process
kosaka 12:a4b17bb682eb 55 DigitalOut led1(LED1);
kosaka 12:a4b17bb682eb 56 DigitalOut led2(LED2);
kosaka 12:a4b17bb682eb 57 DigitalOut led3(LED3);
kosaka 12:a4b17bb682eb 58 DigitalOut led4(LED4);
kosaka 12:a4b17bb682eb 59
kosaka 12:a4b17bb682eb 60 #ifdef GOOD_DATA
kosaka 12:a4b17bb682eb 61 float data[1000][5]; // memory to save data offline instead of "online fprintf".
kosaka 12:a4b17bb682eb 62 unsigned int count3; //
kosaka 12:a4b17bb682eb 63 unsigned int count2=(int)(TS2/TS0); //
kosaka 12:a4b17bb682eb 64 unsigned short _count_data=0;
kosaka 12:a4b17bb682eb 65 #endif
kosaka 12:a4b17bb682eb 66 DigitalOut debug_p26(p26); // p17 for debug
kosaka 12:a4b17bb682eb 67 DigitalOut debug_p25(p25); // p17 for debug
kosaka 12:a4b17bb682eb 68 DigitalOut debug_p24(p24); // p17 for debug
kosaka 12:a4b17bb682eb 69 //AnalogIn VCC(p19); // *3.3 [V], Volt of VCC for motor
kosaka 12:a4b17bb682eb 70 //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.
kosaka 12:a4b17bb682eb 71
kosaka 12:a4b17bb682eb 72 unsigned short f_find_origin; // flag to find the origin of the rotor angle theta
kosaka 12:a4b17bb682eb 73
kosaka 12:a4b17bb682eb 74 #if 1 //BUG!! if move sqrt2 to fast_math.h, sim starts and completed without working!?
kosaka 12:a4b17bb682eb 75 float sqrt2(float x){ // √xのx=1まわりのテイラー展開 √x = 1 + 1/2*(x-1) -1/4*(x-1)^2 + ...
kosaka 12:a4b17bb682eb 76 // return((1+x)*0.5); // 一次近似
kosaka 12:a4b17bb682eb 77 return(x+(1-x*x)*0.25); // 二次近似
kosaka 12:a4b17bb682eb 78 }
kosaka 12:a4b17bb682eb 79 #endif
kosaka 12:a4b17bb682eb 80
kosaka 12:a4b17bb682eb 81 void init_parameters(){ // IPMSMの機器定数等の設定, 制御器の初期化
kosaka 12:a4b17bb682eb 82 float r2, r3;
kosaka 12:a4b17bb682eb 83
kosaka 12:a4b17bb682eb 84
kosaka 12:a4b17bb682eb 85 // 対象の機器定数 PA 5HP scroll from IPEC2000 "High Efficiency Control for Interior Permanent Magnet Synchronous Motor"
kosaka 12:a4b17bb682eb 86 // outside diameter of stator 150 mm
kosaka 12:a4b17bb682eb 87 // outside diameter of rotor 84.0 mm
kosaka 12:a4b17bb682eb 88 // width of rotor 70.0 mm
kosaka 12:a4b17bb682eb 89 // maximum speed 7500 r/min (min=900rpm)
kosaka 12:a4b17bb682eb 90 // maximum torque 15.0 Nm
kosaka 12:a4b17bb682eb 91 // Ψa 0.176 Wb
kosaka 12:a4b17bb682eb 92 // Ld 3.50 mH
kosaka 12:a4b17bb682eb 93 // Lq 6.30 mH
kosaka 12:a4b17bb682eb 94 // Ra 0.143Ω
kosaka 12:a4b17bb682eb 95 // Rc 200Ω
kosaka 12:a4b17bb682eb 96 #ifdef SIMULATION
kosaka 12:a4b17bb682eb 97 p.Ld = 0.0035; // H
kosaka 12:a4b17bb682eb 98 p.Lq = 0.0063; // H
kosaka 12:a4b17bb682eb 99 p.Lq0 = p.Lq;
kosaka 12:a4b17bb682eb 100 p.Lq1 = 0;
kosaka 12:a4b17bb682eb 101 p.R = 0.143; // Ω
kosaka 12:a4b17bb682eb 102 p.phi = 0.176; // V s
kosaka 12:a4b17bb682eb 103 p.Jm = 0.00018; // Nms^2
kosaka 12:a4b17bb682eb 104 #endif
kosaka 12:a4b17bb682eb 105 p.th[0] = 0;
kosaka 12:a4b17bb682eb 106 p.th[1] = 0;
kosaka 12:a4b17bb682eb 107 p.w = 0;
kosaka 12:a4b17bb682eb 108 p.iab[0] =0; p.iab[1] = 0; // iab = [iα;iβ];
kosaka 12:a4b17bb682eb 109 p.vab[0] =0; p.vab[1] = 0; // vab = [vα;vβ];
kosaka 12:a4b17bb682eb 110 p.p = 2; // 極対数
kosaka 12:a4b17bb682eb 111 // UVW座標からαβ座標への変換行列Cuvwの設定
kosaka 12:a4b17bb682eb 112 r2 = sqrt(2.);//1.414213562373095;//2^(1/2);
kosaka 12:a4b17bb682eb 113 r3 = sqrt(3.);//1.732050807568877;//3^(1/2);
kosaka 12:a4b17bb682eb 114 // p.Cuvw =[ r2/r3 -1/r2/r3 -1/r2/r3; ...
kosaka 12:a4b17bb682eb 115 // 0 1/r2 -1/r2 ];
kosaka 12:a4b17bb682eb 116 p.Cuvw[0][0] = r2/r3; p.Cuvw[0][1] = -1./r2/r3; p.Cuvw[0][2] = -1./r2/r3;
kosaka 12:a4b17bb682eb 117 p.Cuvw[1][0] = 0; p.Cuvw[1][1] = 1/r2 ; p.Cuvw[1][2] = -1./r2;
kosaka 12:a4b17bb682eb 118
kosaka 12:a4b17bb682eb 119 p.w = 0;
kosaka 12:a4b17bb682eb 120
kosaka 12:a4b17bb682eb 121 // 制御器の初期化
kosaka 12:a4b17bb682eb 122 vl.iq_ref=0; // ｑ軸電流指令[A]
kosaka 12:a4b17bb682eb 123 vl.w_lpf = 0; // 検出した速度[rad/s]
kosaka 12:a4b17bb682eb 124 vl.eI_w = 0; // 速度制御用偏差の積分値（積分項）
kosaka 12:a4b17bb682eb 125 il.eI_idq[0] = 0; // 電流制御用偏差の積分値（積分項）
kosaka 12:a4b17bb682eb 126 il.eI_idq[1] = 0; // 電流制御用偏差の積分値（積分項）
kosaka 12:a4b17bb682eb 127 }
kosaka 12:a4b17bb682eb 128
kosaka 12:a4b17bb682eb 129 void idq_control(float idq_act[2]){
kosaka 12:a4b17bb682eb 130 // ｄｑ座標電流PID制御器（電流マイナーループのフィードバック制御）
kosaka 12:a4b17bb682eb 131 // 入力：指令dq座標電流 idq_ref [A], 実dq座標電流 idq_act [A], PI制御積分項 eI, サンプル時間 ts [s]
kosaka 12:a4b17bb682eb 132 // 出力：αβ軸電圧指令 vdq_ref [A]
kosaka 12:a4b17bb682eb 133 // [vdq_ref,eI_idq] = idq_control(idq_ref,idq_act,eI_idq,ts);
kosaka 12:a4b17bb682eb 134 float Kp_d, Kp_q, Ki_d, Ki_q, e[2];
kosaka 12:a4b17bb682eb 135 // 電流制御ゲイン
kosaka 12:a4b17bb682eb 136 Kp_d = iKPd; // P gain (d-axis)
kosaka 12:a4b17bb682eb 137 Ki_d = iKId; // I gain (d-axis)
kosaka 12:a4b17bb682eb 138 Kp_q = iKPq; // P gain (q-axis)
kosaka 12:a4b17bb682eb 139 Ki_q = iKIq; // I gain (q-axis)
kosaka 12:a4b17bb682eb 140
kosaka 12:a4b17bb682eb 141 // ｄｑ電流偏差の計算
kosaka 12:a4b17bb682eb 142 e[0] = il.idq_ref[0] - idq_act[0];
kosaka 12:a4b17bb682eb 143 e[1] = il.idq_ref[1] - idq_act[1];
kosaka 12:a4b17bb682eb 144
kosaka 12:a4b17bb682eb 145 // ｄｑ電流偏差の積分項の計算
kosaka 12:a4b17bb682eb 146 il.eI_idq[0] = il.eI_idq[0] + TS0*e[0];
kosaka 12:a4b17bb682eb 147 il.eI_idq[1] = il.eI_idq[1] + TS0*e[1];
kosaka 12:a4b17bb682eb 148
kosaka 12:a4b17bb682eb 149 // ＰＩ制御
kosaka 12:a4b17bb682eb 150 // vdq_ref = [Kp_d 0;0 Kp_q]*e + [Ki_d 0;0 Ki_q]*eI;
kosaka 12:a4b17bb682eb 151 il.vdq_ref[0] = Kp_d*e[0] + Ki_d*il.eI_idq[0];
kosaka 12:a4b17bb682eb 152 il.vdq_ref[1] = Kp_q*e[1] + Ki_q*il.eI_idq[1];
kosaka 12:a4b17bb682eb 153
kosaka 12:a4b17bb682eb 154 // koko anti-windup
kosaka 12:a4b17bb682eb 155 }
kosaka 12:a4b17bb682eb 156
kosaka 12:a4b17bb682eb 157 void current_loop(){ // 電流制御マイナーループ
kosaka 12:a4b17bb682eb 158 float th, c, s, Cdq[2][2], iu, iv, iab[2], idq_act[2], vab_ref[2],tmp;
kosaka 12:a4b17bb682eb 159 if( f_find_origin==1 ){
kosaka 12:a4b17bb682eb 160 th = 0;
kosaka 12:a4b17bb682eb 161 }else{
kosaka 12:a4b17bb682eb 162 // 位置θをセンサで検出
kosaka 12:a4b17bb682eb 163 #ifndef SIMULATION
kosaka 12:a4b17bb682eb 164 p.th[0] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI; // get angle [rad] from encoder
kosaka 12:a4b17bb682eb 165 #endif
kosaka 12:a4b17bb682eb 166 th = p.th[0];
kosaka 12:a4b17bb682eb 167 }
kosaka 12:a4b17bb682eb 168
kosaka 12:a4b17bb682eb 169 // αβ座標からｄｑ座標への変換行列Cdqの設定
kosaka 12:a4b17bb682eb 170 #if 1 //BUG!! if move sqrt2 to fast_math.h, sim starts and completed without working!?
kosaka 12:a4b17bb682eb 171 c = cos(th);
kosaka 12:a4b17bb682eb 172 s = sin(th);
kosaka 12:a4b17bb682eb 173 #else
kosaka 12:a4b17bb682eb 174 c = (float)(_cos(th/(PI/3.)*(float)DEG60+0.5))/65535.;
kosaka 12:a4b17bb682eb 175 s = (float)(_sin(th/(PI/3.)*(float)DEG60+0.5))/65535.;
kosaka 12:a4b17bb682eb 176 #endif
kosaka 12:a4b17bb682eb 177 Cdq[0][0]= c; Cdq[0][1]=s; //Cdq ={{ c, s}
kosaka 12:a4b17bb682eb 178 Cdq[1][0]=-s; Cdq[1][1]=c; // {-s, c]};
kosaka 12:a4b17bb682eb 179
kosaka 12:a4b17bb682eb 180 // 電流センサによってiu, iv を検出
kosaka 12:a4b17bb682eb 181 #ifndef SIMULATION
kosaka 12:a4b17bb682eb 182 p.iuvw[0] = (VshuntR_Uplus - VshuntR_Uminus) /R_SHUNT; // get iu [A] from shunt resistance;
kosaka 12:a4b17bb682eb 183 p.iuvw[1] = (VshuntR_Vplus - VshuntR_Vminus) /R_SHUNT; // get iv [A] from shunt resistance;
kosaka 12:a4b17bb682eb 184 #endif
kosaka 12:a4b17bb682eb 185 iu = p.iuvw[0];
kosaka 12:a4b17bb682eb 186 iv = p.iuvw[1];
kosaka 12:a4b17bb682eb 187 // iw = -(iu + iv); // iu+iv+iw=0であることを利用してiw を計算
kosaka 12:a4b17bb682eb 188
kosaka 12:a4b17bb682eb 189 // iab = p.Cuvw*[iu;iv;iw];
kosaka 12:a4b17bb682eb 190 // iab[0] = p.Cuvw[0][0]*iu + p.Cuvw[0][1]*iv + p.Cuvw[0][2]*iw;
kosaka 12:a4b17bb682eb 191 // iab[1] = p.Cuvw[1][0]*iu + p.Cuvw[1][1]*iv + p.Cuvw[1][2]*iw;
kosaka 12:a4b17bb682eb 192 // iab[0] = p.Cuvw[0][0]*iu + p.Cuvw[0][1]*(iv+iw);
kosaka 12:a4b17bb682eb 193 // iab[1] = p.Cuvw[1][1]*(iv-iw);
kosaka 12:a4b17bb682eb 194 iab[0] = (p.Cuvw[0][0]-p.Cuvw[0][1])*iu;
kosaka 12:a4b17bb682eb 195 iab[1] = p.Cuvw[1][1]*(iu+2*iv);
kosaka 12:a4b17bb682eb 196
kosaka 12:a4b17bb682eb 197 // αβ座標電流をｄｑ座標電流に変換
kosaka 12:a4b17bb682eb 198 //idq_act = Cdq * iab;
kosaka 12:a4b17bb682eb 199 idq_act[0] = Cdq[0][0]*iab[0] + Cdq[0][1]*iab[1];
kosaka 12:a4b17bb682eb 200 idq_act[1] = Cdq[1][0]*iab[0] + Cdq[1][1]*iab[1];
kosaka 12:a4b17bb682eb 201
kosaka 12:a4b17bb682eb 202 // dq電流制御（電流フィードバック制御）
kosaka 12:a4b17bb682eb 203 // [vdq_ref,eI_idq] = idq_control(idq_ref,idq_act,eI_idq,ts);
kosaka 12:a4b17bb682eb 204 #ifdef USE_CURRENT_CONTROL
kosaka 12:a4b17bb682eb 205 idq_control(idq_act);
kosaka 12:a4b17bb682eb 206 #else
kosaka 12:a4b17bb682eb 207 il.vdq_ref[0] = il.idq_ref[0];
kosaka 12:a4b17bb682eb 208 il.vdq_ref[1] = il.idq_ref[1];
kosaka 12:a4b17bb682eb 209 #endif
kosaka 12:a4b17bb682eb 210 // ｄｑ軸電圧指令ベクトルの大きさをMAX制限（コンバータ出力電圧値に設定）
kosaka 12:a4b17bb682eb 211 // if( norm(vdq_ref) > vdqmax ){ vdq_ref= vdqmax/norm(vdq_ref)*vdq_ref}
kosaka 12:a4b17bb682eb 212 if( (tmp=il.vdq_ref[0]*il.vdq_ref[0]+il.vdq_ref[1]*il.vdq_ref[1])>SQRvdqMAX ){
kosaka 12:a4b17bb682eb 213 tmp = sqrt2(SQRvdqMAX/tmp);
kosaka 12:a4b17bb682eb 214 il.vdq_ref[0] = tmp*il.vdq_ref[0]; //= vdqmax/norm(vdq_ref)*vdq_ref
kosaka 12:a4b17bb682eb 215 il.vdq_ref[1] = tmp*il.vdq_ref[1];
kosaka 12:a4b17bb682eb 216 // koko anti-windup
kosaka 12:a4b17bb682eb 217 }
kosaka 12:a4b17bb682eb 218
kosaka 12:a4b17bb682eb 219 // ｄｑ座標指令電圧 vd_ref, vq_refからiα, iβを計算
kosaka 12:a4b17bb682eb 220 // vab_ref = Cdq'*vdq_ref;
kosaka 12:a4b17bb682eb 221 vab_ref[0] = Cdq[0][0]*il.vdq_ref[0] + Cdq[1][0]*il.vdq_ref[1];
kosaka 12:a4b17bb682eb 222 vab_ref[1] = Cdq[0][1]*il.vdq_ref[0] + Cdq[1][1]*il.vdq_ref[1];
kosaka 12:a4b17bb682eb 223
kosaka 12:a4b17bb682eb 224 // モータに印加するUVW相電圧を計算 （vα, vβからvu, vv, vwを計算）
kosaka 12:a4b17bb682eb 225 // vu = √(2/3)*va;
kosaka 12:a4b17bb682eb 226 // vv = -1/√6*va + 1/√2*vb;
kosaka 12:a4b17bb682eb 227 // vw = -1/√6*va - 1/√2*vb;
kosaka 12:a4b17bb682eb 228 // p.Cuvw =[ r2/r3 -1/r2/r3 -1/r2/r3; ...
kosaka 12:a4b17bb682eb 229 // 0 1/r2 -1/r2 ];
kosaka 12:a4b17bb682eb 230 // p.vuvw = p.Cuvw'*vab_ref;
kosaka 12:a4b17bb682eb 231 p.vuvw[0] = p.Cuvw[0][0]*vab_ref[0];
kosaka 12:a4b17bb682eb 232 p.vuvw[1] = p.Cuvw[0][1]*vab_ref[0] + p.Cuvw[1][1]*vab_ref[1];
kosaka 12:a4b17bb682eb 233 p.vuvw[2] = -p.vuvw[0] - p.vuvw[1];
kosaka 12:a4b17bb682eb 234 // p.vuvw[0] = p.Cuvw[0][0]*vab_ref[0] + p.Cuvw[1][0]*vab_ref[1];
kosaka 12:a4b17bb682eb 235 // p.vuvw[2] = p.Cuvw[0][2]*vab_ref[0] + p.Cuvw[1][2]*vab_ref[1];
kosaka 12:a4b17bb682eb 236
kosaka 12:a4b17bb682eb 237 p.th[1] = p.th[0]; // thを更新
kosaka 12:a4b17bb682eb 238 }
kosaka 12:a4b17bb682eb 239
kosaka 12:a4b17bb682eb 240
kosaka 12:a4b17bb682eb 241 void vel_control(){
kosaka 12:a4b17bb682eb 242 // 速度制御器：速度偏差が入力され、q軸電流指令を出力。
kosaka 12:a4b17bb682eb 243 // 入力：指令速度 w_ref [rad/s], 実速度 w_lpf [rad/s], PI制御積分項 eI, サンプル時間 TS1 [s]
kosaka 12:a4b17bb682eb 244 // 出力：q軸電流指令 iq_ref [A]
kosaka 12:a4b17bb682eb 245 // [iq_ref,eI_w] = vel_control(w_ref,w_lpf,eI_w,ts);
kosaka 12:a4b17bb682eb 246 float Kp, Ki, e;
kosaka 12:a4b17bb682eb 247 // 速度制御PIDゲイン
kosaka 12:a4b17bb682eb 248 Kp = wKp; // 速度制御PIDのPゲイン
kosaka 12:a4b17bb682eb 249 Ki = wKi; // 速度制御PIDのIゲイン
kosaka 12:a4b17bb682eb 250
kosaka 12:a4b17bb682eb 251 // 速度偏差の計算
kosaka 12:a4b17bb682eb 252 e = vl.w_ref - vl.w_lpf;
kosaka 12:a4b17bb682eb 253
kosaka 12:a4b17bb682eb 254 // 速度偏差の積分値の計算
kosaka 12:a4b17bb682eb 255 vl.eI_w = vl.eI_w + TS1*e;
kosaka 12:a4b17bb682eb 256
kosaka 12:a4b17bb682eb 257 // ＰＩ制御
kosaka 12:a4b17bb682eb 258 vl.iq_ref = Kp*e + Ki*vl.eI_w;
kosaka 12:a4b17bb682eb 259 // koko anti-windup
kosaka 12:a4b17bb682eb 260 }
kosaka 12:a4b17bb682eb 261
kosaka 12:a4b17bb682eb 262 void velocity_loop(){ // 速度制御メインループ(w_ref&beta_ref to idq_ref)
kosaka 12:a4b17bb682eb 263 float tmp, idq_ref[2];
kosaka 12:a4b17bb682eb 264
kosaka 12:a4b17bb682eb 265 // 速度ωを求めるために、位置θをオイラー微分して、一次ローパスフィルタに通す。
kosaka 12:a4b17bb682eb 266 tmp = p.th[0]-p.th[1];
kosaka 12:a4b17bb682eb 267 while( tmp> PI ){ tmp -= 2*PI;}
kosaka 12:a4b17bb682eb 268 while( tmp<-PI ){ tmp += 2*PI;}
kosaka 12:a4b17bb682eb 269 vl.w_lpf = (1-iLPF)*vl.w_lpf + tmp/TS0 *iLPF;
kosaka 12:a4b17bb682eb 270
kosaka 12:a4b17bb682eb 271 // 速度制御：速度偏差が入力され、q軸電流指令を出力。
kosaka 12:a4b17bb682eb 272 // [iq_ref,eI_w] = vel_control(w_ref,w_act,eI_w,ts);
kosaka 12:a4b17bb682eb 273 vel_control();
kosaka 12:a4b17bb682eb 274
kosaka 12:a4b17bb682eb 275 // ｑ軸電流指令のMAX制限（異常に大きい指令値を制限する）
kosaka 12:a4b17bb682eb 276 if( vl.iq_ref > iqMAX ){
kosaka 12:a4b17bb682eb 277 vl.iq_ref = iqMAX;
kosaka 12:a4b17bb682eb 278 }else if( vl.iq_ref < -iqMAX ){
kosaka 12:a4b17bb682eb 279 vl.iq_ref = -iqMAX;
kosaka 12:a4b17bb682eb 280 }
kosaka 12:a4b17bb682eb 281
kosaka 12:a4b17bb682eb 282 // 電流ベクトル制御
kosaka 12:a4b17bb682eb 283 if( vl.iq_ref>=0 ){ tmp = vl.tan_beta_ref;} // 負のトルクを発生させるときはidは負のままでiqを正から負にする
kosaka 12:a4b17bb682eb 284 else{ tmp = -vl.tan_beta_ref;}// Tm = p((phi+(Ld-Lq)id) iqより
kosaka 12:a4b17bb682eb 285 //idq_ref = {{-tmp, 1}}*iq_ref;
kosaka 12:a4b17bb682eb 286 idq_ref[0] = -tmp*vl.iq_ref; idq_ref[1] = vl.iq_ref;
kosaka 12:a4b17bb682eb 287
kosaka 12:a4b17bb682eb 288 // ｄｑ軸電流の目標値を速度制御メインループから電流制御マイナーループへ渡す。
kosaka 12:a4b17bb682eb 289 il.idq_ref[0] = idq_ref[0];
kosaka 12:a4b17bb682eb 290 il.idq_ref[1] = idq_ref[1];
kosaka 12:a4b17bb682eb 291 }
kosaka 12:a4b17bb682eb 292
kosaka 12:a4b17bb682eb 293 void vuvw2pwm(){ // vu, vv, vwより、UVW相の上アームPWMを発生
kosaka 12:a4b17bb682eb 294 float duty_u, duty_v, duty_w;
kosaka 12:a4b17bb682eb 295
kosaka 12:a4b17bb682eb 296 duty_u = (p.vuvw[0]/vdqMAX+1)*0.5;
kosaka 12:a4b17bb682eb 297 duty_v = (p.vuvw[1]/vdqMAX+1)*0.5;
kosaka 12:a4b17bb682eb 298 duty_w = (p.vuvw[2]/vdqMAX+1)*0.5;
kosaka 12:a4b17bb682eb 299 uvw[0].duty = duty_u;
kosaka 12:a4b17bb682eb 300 uvw[1].duty = duty_v;
kosaka 12:a4b17bb682eb 301 uvw[2].duty = duty_w;
kosaka 12:a4b17bb682eb 302 }
kosaka 12:a4b17bb682eb 303
kosaka 12:a4b17bb682eb 304 #ifdef SIMULATION
kosaka 12:a4b17bb682eb 305 void sim_motor(){
kosaka 12:a4b17bb682eb 306 // モータシミュレータ
kosaka 12:a4b17bb682eb 307 // 入力信号：UVW相電圧p.vuvw [V]、負荷トルクp.TL [Nm]
kosaka 12:a4b17bb682eb 308 // 出力信号：モータ角度p.th[0] [rad], モータ速度p.w [rad/s], モータUVW相電流p.iuvw [A]
kosaka 12:a4b17bb682eb 309 // p = motor(p, ts); // IPM, ｄｑ座標
kosaka 12:a4b17bb682eb 310 float c, s, Cdq[2][2], idq_dot[2], id,iq, vdq[2], idq[2], Tall,TL, Cdq_inv[2][2];
kosaka 12:a4b17bb682eb 311 analog_out=p.vuvw[0]/100.+0.5;//debug
kosaka 12:a4b17bb682eb 312 // vu, vv, vwからvα, vβを計算
kosaka 12:a4b17bb682eb 313 p.vab[0] = p.Cuvw[0][0]*p.vuvw[0] + p.Cuvw[0][1]*p.vuvw[1] + p.Cuvw[0][2]*p.vuvw[2];
kosaka 12:a4b17bb682eb 314 p.vab[1] = p.Cuvw[1][0]*p.vuvw[0] + p.Cuvw[1][1]*p.vuvw[1] + p.Cuvw[1][2]*p.vuvw[2];
kosaka 12:a4b17bb682eb 315 //printf("vab=%f, %f ",p.vab[0],p.vab[1]);scanf("%f",&c);
kosaka 12:a4b17bb682eb 316
kosaka 12:a4b17bb682eb 317 // αβ座標からｄｑ座標への変換行列Cdqの設定
kosaka 12:a4b17bb682eb 318 c = cos(p.th[0]);
kosaka 12:a4b17bb682eb 319 s = sin(p.th[0]);
kosaka 12:a4b17bb682eb 320 // Cdq =[ c s; ...
kosaka 12:a4b17bb682eb 321 // -s c];
kosaka 12:a4b17bb682eb 322 Cdq[0][0] = c; Cdq[0][1] = s;
kosaka 12:a4b17bb682eb 323 Cdq[1][0] =-s; Cdq[1][1] = c;
kosaka 12:a4b17bb682eb 324
kosaka 12:a4b17bb682eb 325 // ｖα, ｖβからｖｄ, ｖｑを計算
kosaka 12:a4b17bb682eb 326 // vd = c*p.va + s*p.vb;
kosaka 12:a4b17bb682eb 327 // vq =-s*p.va + c*p.vb;
kosaka 12:a4b17bb682eb 328 // vdq = Cdq * p.vab;
kosaka 12:a4b17bb682eb 329 vdq[0] = Cdq[0][0]*p.vab[0] + Cdq[0][1]*p.vab[1];
kosaka 12:a4b17bb682eb 330 vdq[1] = Cdq[1][0]*p.vab[0] + Cdq[1][1]*p.vab[1];
kosaka 12:a4b17bb682eb 331
kosaka 12:a4b17bb682eb 332 // iα, iβからid, iqを計算
kosaka 12:a4b17bb682eb 333 // id = c*p.ia + s*p.ib;
kosaka 12:a4b17bb682eb 334 // iq =-s*p.ia + c*p.ib;
kosaka 12:a4b17bb682eb 335 // idq = Cdq * p.iab;
kosaka 12:a4b17bb682eb 336 idq[0] = Cdq[0][0]*p.iab[0] + Cdq[0][1]*p.iab[1];
kosaka 12:a4b17bb682eb 337 idq[1] = Cdq[1][0]*p.iab[0] + Cdq[1][1]*p.iab[1];
kosaka 12:a4b17bb682eb 338
kosaka 12:a4b17bb682eb 339 // get id,iq
kosaka 12:a4b17bb682eb 340 // id_dot = (1.0/p.Ld) * ( vd - (p.R*id - p.w*p.Lq*iq) );
kosaka 12:a4b17bb682eb 341 // iq_dot = (1.0/p.Lq) * ( vq - (p.R*iq + p.w*p.Ld*id + p.w*p.phi) );
kosaka 12:a4b17bb682eb 342 // idq_dot = [p.Ld 0;0 p.Lq]\( vdq - p.R*idq - p.w*[0 -p.Lq;p.Ld 0]*idq - p.w*[0;p.phi]);
kosaka 12:a4b17bb682eb 343 idq_dot[0] = (1.0/p.Ld) * ( vdq[0] - (p.R*idq[0] - p.w*p.Lq*idq[1]) );
kosaka 12:a4b17bb682eb 344 idq_dot[1] = (1.0/p.Lq) * ( vdq[1] - (p.R*idq[1] + p.w*p.Ld*idq[0] + p.w*p.phi) );
kosaka 12:a4b17bb682eb 345 // id = id + ts * id_dot;
kosaka 12:a4b17bb682eb 346 // iq = iq + ts * iq_dot;
kosaka 12:a4b17bb682eb 347 p.idq[0] = idq[0] + TS0*idq_dot[0];
kosaka 12:a4b17bb682eb 348 p.idq[1] = idq[1] + TS0*idq_dot[1];
kosaka 12:a4b17bb682eb 349 id = p.idq[0];
kosaka 12:a4b17bb682eb 350 iq = p.idq[1];
kosaka 12:a4b17bb682eb 351
kosaka 12:a4b17bb682eb 352 // 磁気飽和を考慮したLqの計算
kosaka 12:a4b17bb682eb 353 p.Lq = p.Lq0 + p.Lq1*abs(iq);
kosaka 12:a4b17bb682eb 354 if( p.Lq < p.Ld )
kosaka 12:a4b17bb682eb 355 p.Lq = p.Ld;
kosaka 12:a4b17bb682eb 356
kosaka 12:a4b17bb682eb 357 // モータトルクの計算
kosaka 12:a4b17bb682eb 358 p.Tm = p.p * (p.phi + (p.Ld-p.Lq)*id) * iq;
kosaka 12:a4b17bb682eb 359
kosaka 12:a4b17bb682eb 360 // モータ速度ωの計算
kosaka 12:a4b17bb682eb 361 if( abs(p.w) > 5*2*PI )
kosaka 12:a4b17bb682eb 362 if( p.w>=0 ) TL= p.TL;
kosaka 12:a4b17bb682eb 363 else TL=-p.TL;
kosaka 12:a4b17bb682eb 364 else
kosaka 12:a4b17bb682eb 365 TL = p.w/(5*2*PI)*p.TL;
kosaka 12:a4b17bb682eb 366
kosaka 12:a4b17bb682eb 367 Tall = p.Tm - TL;
kosaka 12:a4b17bb682eb 368 p.w = p.w + TS0 * (1.0 / p.Jm) * Tall;
kosaka 12:a4b17bb682eb 369
kosaka 12:a4b17bb682eb 370 // モータ角度θの計算
kosaka 12:a4b17bb682eb 371 p.th[0] = p.th[0] + TS0 * p.w;
kosaka 12:a4b17bb682eb 372 if( p.th[0]>4*PI)
kosaka 12:a4b17bb682eb 373 p.th[0] = p.th[0] - 4*PI;
kosaka 12:a4b17bb682eb 374
kosaka 12:a4b17bb682eb 375 if( p.th[0]<0 )
kosaka 12:a4b17bb682eb 376 p.th[0] = p.th[0] + 4*PI;
kosaka 12:a4b17bb682eb 377
kosaka 12:a4b17bb682eb 378 // ｄｑ座標からαβ座標への変換行列Cdq_invの設定
kosaka 12:a4b17bb682eb 379 c = cos(p.th[0]);
kosaka 12:a4b17bb682eb 380 s = sin(p.th[0]);
kosaka 12:a4b17bb682eb 381 // Cdq_inv =[ c -s; ...
kosaka 12:a4b17bb682eb 382 // s c];
kosaka 12:a4b17bb682eb 383 Cdq_inv[0][0] = c; Cdq_inv[0][1] =-s;
kosaka 12:a4b17bb682eb 384 Cdq_inv[1][0] = s; Cdq_inv[1][1] = c;
kosaka 12:a4b17bb682eb 385
kosaka 12:a4b17bb682eb 386 // id, iqからiα, iβを計算
kosaka 12:a4b17bb682eb 387 //p.iab = Cdq_inv * p.idq;
kosaka 12:a4b17bb682eb 388 p.iab[0] = Cdq_inv[0][0]*p.idq[0] + Cdq_inv[0][1]*p.idq[1];
kosaka 12:a4b17bb682eb 389 p.iab[1] = Cdq_inv[1][0]*p.idq[0] + Cdq_inv[1][1]*p.idq[1];
kosaka 12:a4b17bb682eb 390
kosaka 12:a4b17bb682eb 391 // αβ座標からUVW座標への変換行列Cuvw_inv=Cuvw'
kosaka 12:a4b17bb682eb 392 // iα, iβからiu, iv, iwを計算
kosaka 12:a4b17bb682eb 393 // iu = r2/r3*ia;
kosaka 12:a4b17bb682eb 394 // iv = -1/r2/r3*ia + 1/r2*ib;
kosaka 12:a4b17bb682eb 395 // iw = -1/r2/r3*ia - 1/r2*ib;
kosaka 12:a4b17bb682eb 396 //p.iuvw = p.Cuvw' * p.iab;
kosaka 12:a4b17bb682eb 397 p.iuvw[0] = p.Cuvw[0][0]*p.iab[0] + p.Cuvw[1][0]*p.iab[1];
kosaka 12:a4b17bb682eb 398 p.iuvw[1] = p.Cuvw[0][1]*p.iab[0] + p.Cuvw[1][1]*p.iab[1];
kosaka 12:a4b17bb682eb 399 p.iuvw[2] = p.Cuvw[0][2]*p.iab[0] + p.Cuvw[1][2]*p.iab[1];
kosaka 12:a4b17bb682eb 400 }
kosaka 12:a4b17bb682eb 401 #endif
kosaka 12:a4b17bb682eb 402
kosaka 12:a4b17bb682eb 403 void data2mbedUSB(){ // save data to mbed USB drive
kosaka 12:a4b17bb682eb 404 if( _count_data<1000 ){
kosaka 12:a4b17bb682eb 405 data[_count_data][0]=p.th[0]/*vl.w_ref*/; data[_count_data][1]=p.vuvw[0];
kosaka 12:a4b17bb682eb 406 data[_count_data][2]=vl.w_lpf; data[_count_data][3]=_count*TS0; data[_count_data][4]=il.vdq_ref[1];
kosaka 12:a4b17bb682eb 407 _count_data++;
kosaka 12:a4b17bb682eb 408 }
kosaka 12:a4b17bb682eb 409 #if 0
kosaka 12:a4b17bb682eb 410 if( _count_data<500 ){
kosaka 12:a4b17bb682eb 411 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];
kosaka 12:a4b17bb682eb 412 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];
kosaka 12:a4b17bb682eb 413 debug[10]=p.iuvw[1]; debug[11]=p.iuvw[2]; debug[12]=p.idq[0]; debug[13]=p.idq[1]; debug[14]=p.TL;
kosaka 12:a4b17bb682eb 414 debug[15]=p.Tm; debug[16]=p.w; debug[17]=vl.w_lpf; debug[18]=p.th[0]; debug[19]=_count*TS0;//_time;
kosaka 12:a4b17bb682eb 415 //BUG for(j=0;j<19;j++){ fprintf( fp, "%f, ",debug[j]);} fprintf( fp, "%f\n",debug[19]);
kosaka 12:a4b17bb682eb 416 for(j=0;j<19;j++){ printf("%f, ",debug[j]);} printf("%f\n",debug[19]);
kosaka 12:a4b17bb682eb 417 // for(j=0;j<19;j++){ pc.printf("%f, ",debug[j]);} pc.printf("%f\n",debug[19]);
kosaka 12:a4b17bb682eb 418 }
kosaka 12:a4b17bb682eb 419 #endif
kosaka 12:a4b17bb682eb 420 }
kosaka 12:a4b17bb682eb 421 void timerTS0(){ // timer called every TS0[s].
kosaka 12:a4b17bb682eb 422 debug_p26 = 1;
kosaka 12:a4b17bb682eb 423 _count++;
kosaka 12:a4b17bb682eb 424 _time += TS0;
kosaka 12:a4b17bb682eb 425
kosaka 12:a4b17bb682eb 426 current_loop(); // 電流制御マイナーループ(idq_ref to vuvw)
kosaka 12:a4b17bb682eb 427 vuvw2pwm(); // vuvw to pwm
kosaka 12:a4b17bb682eb 428 #ifdef SIMULATION
kosaka 12:a4b17bb682eb 429 // モータシミュレータ
kosaka 12:a4b17bb682eb 430 sim_motor(); // IPM, ｄｑ座標
kosaka 12:a4b17bb682eb 431 #endif
kosaka 12:a4b17bb682eb 432 debug_p26 = 0;
kosaka 12:a4b17bb682eb 433 }
kosaka 12:a4b17bb682eb 434 void timerTS1(void const *argument){
kosaka 12:a4b17bb682eb 435 debug_p25 = 1;
kosaka 12:a4b17bb682eb 436 velocity_loop(); // 速度制御メインループ(w_ref&beta_ref to idq_ref)
kosaka 12:a4b17bb682eb 437 debug_p25 = 0;
kosaka 12:a4b17bb682eb 438 }
kosaka 12:a4b17bb682eb 439
kosaka 12:a4b17bb682eb 440 void display2PC(){ // display to tera term on PC
kosaka 12:a4b17bb682eb 441 pc.printf("%8.1f[s]\t%8.5f[V]\t%4d [Hz]\t%d\r\n", _time, il.vdq_ref[0], (int)(vl.w_lpf/(2*PI)+0.5), (int)(vl.w_ref/(2*PI)+0.5)); // print to tera term
kosaka 12:a4b17bb682eb 442 // 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
kosaka 12:a4b17bb682eb 443 }
kosaka 12:a4b17bb682eb 444 void timerTS2(){
kosaka 12:a4b17bb682eb 445 }
kosaka 12:a4b17bb682eb 446 void timerTS3(){
kosaka 12:a4b17bb682eb 447 data2mbedUSB(); // data2mbedUSB() is called every TS3[s].
kosaka 12:a4b17bb682eb 448 }
kosaka 12:a4b17bb682eb 449 void timerTS4(){
kosaka 12:a4b17bb682eb 450 display2PC(); // display to tera term on PC. display2PC() is called every TS4[s].
kosaka 12:a4b17bb682eb 451 }