manabu kosaka / Mbed 2 deprecated BLDCmotor

UVW 3 phases Brushless DC motor control

Dependencies:   QEI mbed-rtos mbed

Fork of DCmotor by manabu kosaka

controller.cpp@14:7f83c4b96d34, 2013-06-07 (annotated)

Committer:
kosaka
Date:
Fri Jun 07 08:49:44 2013 +0000
Revision:
14:7f83c4b96d34
Parent:
13:791e20f1af43 
mbed-rtos is updated

Who changed what in which revision?

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