UVW 3 phases Brushless DC motor control
Dependencies: QEI mbed-rtos mbed
Fork of BLDCmotor by
controller.cpp@17:1ac855d69c78, 2013-09-07 (annotated)
- Committer:
- kosakaLab
- Date:
- Sat Sep 07 02:47:09 2013 +0000
- Revision:
- 17:1ac855d69c78
- Parent:
- 16:d05404eef8ee
for TBLM-01 18T and OME-360-2MCA
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kosaka | 13:791e20f1af43 | 1 | // controller.cpp: 各種3相同期モータに対するセンサあり運転のシミュレーション |
kosakaLab | 16:d05404eef8ee | 2 | // Kosaka Lab. 130905 |
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; // q軸電流指令[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 | // dq座標電流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 | // dq電流偏差の計算 |
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 | // dq電流偏差の積分項の計算 |
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 | // dq電流偏差の微分値の計算 |
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 ){ |
kosakaLab | 15:427f5ae8e957 | 148 | th = p.th_const; |
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 | // αβ座標からdq座標への変換行列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 | // αβ座標電流をdq座標電流に変換 |
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 | // dq軸電圧指令ベクトルの大きさを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 |
kosakaLab | 15:427f5ae8e957 | 210 | //analog_out=il.vdq_ref[1]/3.3+0.4;//koko |
kosaka | 12:a4b17bb682eb | 211 | // dq座標指令電圧 vd_ref, vq_refからiα, iβを計算 |
kosaka | 12:a4b17bb682eb | 212 | // vab_ref = Cdq'*vdq_ref; |
kosaka | 12:a4b17bb682eb | 213 | vab_ref[0] = Cdq[0][0]*il.vdq_ref[0] + Cdq[1][0]*il.vdq_ref[1]; |
kosaka | 12:a4b17bb682eb | 214 | vab_ref[1] = Cdq[0][1]*il.vdq_ref[0] + Cdq[1][1]*il.vdq_ref[1]; |
kosakaLab | 15:427f5ae8e957 | 215 | //analog_out=vab_ref[1]/3.3+0.4; |
kosaka | 12:a4b17bb682eb | 216 | |
kosaka | 12:a4b17bb682eb | 217 | // モータに印加するUVW相電圧を計算 (vα, vβからvu, vv, vwを計算) |
kosaka | 12:a4b17bb682eb | 218 | // vu = √(2/3)*va; |
kosaka | 12:a4b17bb682eb | 219 | // vv = -1/√6*va + 1/√2*vb; |
kosaka | 12:a4b17bb682eb | 220 | // vw = -1/√6*va - 1/√2*vb; |
kosaka | 12:a4b17bb682eb | 221 | // p.Cuvw =[ r2/r3 -1/r2/r3 -1/r2/r3; ... |
kosaka | 12:a4b17bb682eb | 222 | // 0 1/r2 -1/r2 ]; |
kosaka | 12:a4b17bb682eb | 223 | // p.vuvw = p.Cuvw'*vab_ref; |
kosaka | 12:a4b17bb682eb | 224 | p.vuvw[0] = p.Cuvw[0][0]*vab_ref[0]; |
kosaka | 12:a4b17bb682eb | 225 | p.vuvw[1] = p.Cuvw[0][1]*vab_ref[0] + p.Cuvw[1][1]*vab_ref[1]; |
kosaka | 12:a4b17bb682eb | 226 | p.vuvw[2] = -p.vuvw[0] - p.vuvw[1]; |
kosaka | 12:a4b17bb682eb | 227 | // p.vuvw[0] = p.Cuvw[0][0]*vab_ref[0] + p.Cuvw[1][0]*vab_ref[1]; |
kosaka | 12:a4b17bb682eb | 228 | // p.vuvw[2] = p.Cuvw[0][2]*vab_ref[0] + p.Cuvw[1][2]*vab_ref[1]; |
kosaka | 12:a4b17bb682eb | 229 | } |
kosaka | 12:a4b17bb682eb | 230 | |
kosaka | 12:a4b17bb682eb | 231 | |
kosaka | 12:a4b17bb682eb | 232 | void vel_control(){ |
kosaka | 12:a4b17bb682eb | 233 | // 速度制御器:速度偏差が入力され、q軸電流指令を出力。 |
kosaka | 12:a4b17bb682eb | 234 | // 入力:指令速度 w_ref [rad/s], 実速度 w_lpf [rad/s], PI制御積分項 eI, サンプル時間 TS1 [s] |
kosaka | 12:a4b17bb682eb | 235 | // 出力:q軸電流指令 iq_ref [A] |
kosaka | 13:791e20f1af43 | 236 | // [iq_ref,eI] = vel_control(w_ref,w_lpf,eI,ts); |
kosaka | 13:791e20f1af43 | 237 | float e, ed; |
kosaka | 12:a4b17bb682eb | 238 | |
kosaka | 12:a4b17bb682eb | 239 | // 速度偏差の計算 |
kosaka | 12:a4b17bb682eb | 240 | e = vl.w_ref - vl.w_lpf; |
kosaka | 12:a4b17bb682eb | 241 | |
kosaka | 12:a4b17bb682eb | 242 | // 速度偏差の積分値の計算 |
kosaka | 13:791e20f1af43 | 243 | vl.eI = vl.eI + TS1*e; |
kosaka | 13:791e20f1af43 | 244 | |
kosaka | 13:791e20f1af43 | 245 | ed = (e-vl.e_old)/TS1; // 速度偏差の微分値の計算 |
kosaka | 13:791e20f1af43 | 246 | vl.e_old = e; // 速度偏差の1サンプル過去の値を更新 |
kosaka | 12:a4b17bb682eb | 247 | |
kosaka | 12:a4b17bb682eb | 248 | // PI制御 |
kosaka | 13:791e20f1af43 | 249 | vl.iq_ref = wKp*e + wKi*vl.eI + wKd*ed; // PID制御器の出力を計算 |
kosaka | 12:a4b17bb682eb | 250 | } |
kosaka | 12:a4b17bb682eb | 251 | |
kosaka | 12:a4b17bb682eb | 252 | void velocity_loop(){ // 速度制御メインループ(w_ref&beta_ref to idq_ref) |
kosaka | 12:a4b17bb682eb | 253 | float tmp, idq_ref[2]; |
kosaka | 12:a4b17bb682eb | 254 | |
kosaka | 12:a4b17bb682eb | 255 | // 速度ωを求めるために、位置θをオイラー微分して、一次ローパスフィルタに通す。 |
kosaka | 12:a4b17bb682eb | 256 | tmp = p.th[0]-p.th[1]; |
kosaka | 12:a4b17bb682eb | 257 | while( tmp> PI ){ tmp -= 2*PI;} |
kosaka | 12:a4b17bb682eb | 258 | while( tmp<-PI ){ tmp += 2*PI;} |
kosaka | 13:791e20f1af43 | 259 | vl.w_lpf = iLPF*vl.w_lpf + tmp/TS0 *(1-iLPF); |
kosakaLab | 15:427f5ae8e957 | 260 | tmp=vl.w_lpf/(2*PI) /20; if(tmp>1) tmp=1;else if(tmp<0) tmp=0; |
kosakaLab | 15:427f5ae8e957 | 261 | analog_out=tmp;//tmp;//koko |
kosaka | 12:a4b17bb682eb | 262 | |
kosaka | 12:a4b17bb682eb | 263 | // 速度制御:速度偏差が入力され、q軸電流指令を出力。 |
kosaka | 13:791e20f1af43 | 264 | // [iq_ref,eI] = vel_control(w_ref,w_act,eI,ts); |
kosaka | 12:a4b17bb682eb | 265 | vel_control(); |
kosaka | 12:a4b17bb682eb | 266 | |
kosaka | 12:a4b17bb682eb | 267 | // q軸電流指令のMAX制限(異常に大きい指令値を制限する) |
kosaka | 13:791e20f1af43 | 268 | // anti-windup: if u=i_ref is saturated, then reduce eI. |
kosaka | 12:a4b17bb682eb | 269 | if( vl.iq_ref > iqMAX ){ |
kosaka | 13:791e20f1af43 | 270 | vl.eI -= (vl.iq_ref - iqMAX)/wKi; if( vl.eI<0 ){ vl.eI=0;} |
kosaka | 12:a4b17bb682eb | 271 | vl.iq_ref = iqMAX; |
kosaka | 12:a4b17bb682eb | 272 | }else if( vl.iq_ref < -iqMAX ){ |
kosaka | 13:791e20f1af43 | 273 | vl.eI -= (vl.iq_ref + iqMAX)/wKi; if( vl.eI>0 ){ vl.eI=0;} |
kosaka | 12:a4b17bb682eb | 274 | vl.iq_ref = -iqMAX; |
kosaka | 12:a4b17bb682eb | 275 | } |
kosaka | 12:a4b17bb682eb | 276 | |
kosaka | 12:a4b17bb682eb | 277 | // 電流ベクトル制御 |
kosaka | 12:a4b17bb682eb | 278 | if( vl.iq_ref>=0 ){ tmp = vl.tan_beta_ref;} // 負のトルクを発生させるときはidは負のままでiqを正から負にする |
kosaka | 12:a4b17bb682eb | 279 | else{ tmp = -vl.tan_beta_ref;}// Tm = p((phi+(Ld-Lq)id) iqより |
kosaka | 12:a4b17bb682eb | 280 | //idq_ref = {{-tmp, 1}}*iq_ref; |
kosaka | 12:a4b17bb682eb | 281 | idq_ref[0] = -tmp*vl.iq_ref; idq_ref[1] = vl.iq_ref; |
kosaka | 12:a4b17bb682eb | 282 | |
kosaka | 12:a4b17bb682eb | 283 | // dq軸電流の目標値を速度制御メインループから電流制御マイナーループへ渡す。 |
kosaka | 12:a4b17bb682eb | 284 | il.idq_ref[0] = idq_ref[0]; |
kosaka | 12:a4b17bb682eb | 285 | il.idq_ref[1] = idq_ref[1]; |
kosakaLab | 15:427f5ae8e957 | 286 | if( f_find_origin==1 ){ |
kosakaLab | 15:427f5ae8e957 | 287 | il.idq_ref[0] = iqMAX/1.0; // idをプラス、iqをゼロにして、 |
kosakaLab | 15:427f5ae8e957 | 288 | il.idq_ref[1] = 0; // 無負荷のときにθ=0とさせる。 |
kosakaLab | 15:427f5ae8e957 | 289 | } |
kosaka | 12:a4b17bb682eb | 290 | } |
kosaka | 12:a4b17bb682eb | 291 | |
kosaka | 12:a4b17bb682eb | 292 | void vuvw2pwm(){ // vu, vv, vwより、UVW相の上アームPWMを発生 |
kosaka | 12:a4b17bb682eb | 293 | float duty_u, duty_v, duty_w; |
kosaka | 12:a4b17bb682eb | 294 | |
kosaka | 13:791e20f1af43 | 295 | duty_u = p.vuvw[0]/vdqMAX+0.5; // dutyを計算 |
kosaka | 13:791e20f1af43 | 296 | duty_v = p.vuvw[1]/vdqMAX+0.5; // dutyを計算 |
kosaka | 13:791e20f1af43 | 297 | duty_w = p.vuvw[2]/vdqMAX+0.5; // dutyを計算 |
kosaka | 13:791e20f1af43 | 298 | uvw[0].duty = duty_u; // dutyをPWM発生関数に渡す |
kosaka | 13:791e20f1af43 | 299 | uvw[1].duty = duty_v; // dutyをPWM発生関数に渡す |
kosaka | 13:791e20f1af43 | 300 | uvw[2].duty = duty_w; // dutyをPWM発生関数に渡す |
kosaka | 12:a4b17bb682eb | 301 | } |
kosaka | 12:a4b17bb682eb | 302 | |
kosaka | 12:a4b17bb682eb | 303 | #ifdef SIMULATION |
kosaka | 12:a4b17bb682eb | 304 | void sim_motor(){ |
kosaka | 12:a4b17bb682eb | 305 | // モータシミュレータ |
kosaka | 12:a4b17bb682eb | 306 | // 入力信号:UVW相電圧p.vuvw [V]、負荷トルクp.TL [Nm] |
kosaka | 12:a4b17bb682eb | 307 | // 出力信号:モータ角度p.th[0] [rad], モータ速度p.w [rad/s], モータUVW相電流p.iuvw [A] |
kosaka | 12:a4b17bb682eb | 308 | // p = motor(p, ts); // IPM, dq座標 |
kosaka | 12:a4b17bb682eb | 309 | float c, s, Cdq[2][2], idq_dot[2], id,iq, vdq[2], idq[2], Tall,TL, Cdq_inv[2][2]; |
kosaka | 12:a4b17bb682eb | 310 | analog_out=p.vuvw[0]/100.+0.5;//debug |
kosaka | 12:a4b17bb682eb | 311 | // vu, vv, vwからvα, vβを計算 |
kosaka | 12:a4b17bb682eb | 312 | 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 | 313 | 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 | 314 | //printf("vab=%f, %f ",p.vab[0],p.vab[1]);scanf("%f",&c); |
kosaka | 12:a4b17bb682eb | 315 | |
kosaka | 12:a4b17bb682eb | 316 | // αβ座標からdq座標への変換行列Cdqの設定 |
kosaka | 12:a4b17bb682eb | 317 | c = cos(p.th[0]); |
kosaka | 12:a4b17bb682eb | 318 | s = sin(p.th[0]); |
kosaka | 12:a4b17bb682eb | 319 | // Cdq =[ c s; ... |
kosaka | 12:a4b17bb682eb | 320 | // -s c]; |
kosaka | 12:a4b17bb682eb | 321 | Cdq[0][0] = c; Cdq[0][1] = s; |
kosaka | 12:a4b17bb682eb | 322 | Cdq[1][0] =-s; Cdq[1][1] = c; |
kosaka | 12:a4b17bb682eb | 323 | |
kosaka | 12:a4b17bb682eb | 324 | // vα, vβからvd, vqを計算 |
kosaka | 12:a4b17bb682eb | 325 | // vd = c*p.va + s*p.vb; |
kosaka | 12:a4b17bb682eb | 326 | // vq =-s*p.va + c*p.vb; |
kosaka | 12:a4b17bb682eb | 327 | // vdq = Cdq * p.vab; |
kosaka | 12:a4b17bb682eb | 328 | vdq[0] = Cdq[0][0]*p.vab[0] + Cdq[0][1]*p.vab[1]; |
kosaka | 12:a4b17bb682eb | 329 | vdq[1] = Cdq[1][0]*p.vab[0] + Cdq[1][1]*p.vab[1]; |
kosaka | 12:a4b17bb682eb | 330 | |
kosaka | 12:a4b17bb682eb | 331 | // iα, iβからid, iqを計算 |
kosaka | 12:a4b17bb682eb | 332 | // id = c*p.ia + s*p.ib; |
kosaka | 12:a4b17bb682eb | 333 | // iq =-s*p.ia + c*p.ib; |
kosaka | 12:a4b17bb682eb | 334 | // idq = Cdq * p.iab; |
kosaka | 12:a4b17bb682eb | 335 | idq[0] = Cdq[0][0]*p.iab[0] + Cdq[0][1]*p.iab[1]; |
kosaka | 12:a4b17bb682eb | 336 | idq[1] = Cdq[1][0]*p.iab[0] + Cdq[1][1]*p.iab[1]; |
kosaka | 12:a4b17bb682eb | 337 | |
kosaka | 12:a4b17bb682eb | 338 | // get id,iq |
kosaka | 12:a4b17bb682eb | 339 | // id_dot = (1.0/p.Ld) * ( vd - (p.R*id - p.w*p.Lq*iq) ); |
kosaka | 12:a4b17bb682eb | 340 | // iq_dot = (1.0/p.Lq) * ( vq - (p.R*iq + p.w*p.Ld*id + p.w*p.phi) ); |
kosaka | 12:a4b17bb682eb | 341 | // 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 | 342 | idq_dot[0] = (1.0/p.Ld) * ( vdq[0] - (p.R*idq[0] - p.w*p.Lq*idq[1]) ); |
kosaka | 12:a4b17bb682eb | 343 | 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 | 344 | // id = id + ts * id_dot; |
kosaka | 12:a4b17bb682eb | 345 | // iq = iq + ts * iq_dot; |
kosaka | 12:a4b17bb682eb | 346 | p.idq[0] = idq[0] + TS0*idq_dot[0]; |
kosaka | 12:a4b17bb682eb | 347 | p.idq[1] = idq[1] + TS0*idq_dot[1]; |
kosaka | 12:a4b17bb682eb | 348 | id = p.idq[0]; |
kosaka | 12:a4b17bb682eb | 349 | iq = p.idq[1]; |
kosaka | 12:a4b17bb682eb | 350 | |
kosaka | 12:a4b17bb682eb | 351 | // 磁気飽和を考慮したLqの計算 |
kosaka | 12:a4b17bb682eb | 352 | p.Lq = p.Lq0 + p.Lq1*abs(iq); |
kosaka | 12:a4b17bb682eb | 353 | if( p.Lq < p.Ld ) |
kosaka | 12:a4b17bb682eb | 354 | p.Lq = p.Ld; |
kosaka | 12:a4b17bb682eb | 355 | |
kosaka | 12:a4b17bb682eb | 356 | // モータトルクの計算 |
kosaka | 12:a4b17bb682eb | 357 | p.Tm = p.p * (p.phi + (p.Ld-p.Lq)*id) * iq; |
kosaka | 12:a4b17bb682eb | 358 | |
kosaka | 12:a4b17bb682eb | 359 | // モータ速度ωの計算 |
kosaka | 12:a4b17bb682eb | 360 | if( abs(p.w) > 5*2*PI ) |
kosaka | 12:a4b17bb682eb | 361 | if( p.w>=0 ) TL= p.TL; |
kosaka | 12:a4b17bb682eb | 362 | else TL=-p.TL; |
kosaka | 12:a4b17bb682eb | 363 | else |
kosaka | 12:a4b17bb682eb | 364 | TL = p.w/(5*2*PI)*p.TL; |
kosaka | 12:a4b17bb682eb | 365 | |
kosaka | 12:a4b17bb682eb | 366 | Tall = p.Tm - TL; |
kosaka | 12:a4b17bb682eb | 367 | p.w = p.w + TS0 * (1.0 / p.Jm) * Tall; |
kosaka | 12:a4b17bb682eb | 368 | |
kosaka | 12:a4b17bb682eb | 369 | // モータ角度θの計算 |
kosaka | 12:a4b17bb682eb | 370 | p.th[0] = p.th[0] + TS0 * p.w; |
kosaka | 12:a4b17bb682eb | 371 | if( p.th[0]>4*PI) |
kosaka | 12:a4b17bb682eb | 372 | p.th[0] = p.th[0] - 4*PI; |
kosaka | 12:a4b17bb682eb | 373 | |
kosaka | 12:a4b17bb682eb | 374 | if( p.th[0]<0 ) |
kosaka | 12:a4b17bb682eb | 375 | p.th[0] = p.th[0] + 4*PI; |
kosaka | 12:a4b17bb682eb | 376 | |
kosaka | 12:a4b17bb682eb | 377 | // dq座標からαβ座標への変換行列Cdq_invの設定 |
kosaka | 12:a4b17bb682eb | 378 | c = cos(p.th[0]); |
kosaka | 12:a4b17bb682eb | 379 | s = sin(p.th[0]); |
kosaka | 12:a4b17bb682eb | 380 | // Cdq_inv =[ c -s; ... |
kosaka | 12:a4b17bb682eb | 381 | // s c]; |
kosaka | 12:a4b17bb682eb | 382 | Cdq_inv[0][0] = c; Cdq_inv[0][1] =-s; |
kosaka | 12:a4b17bb682eb | 383 | Cdq_inv[1][0] = s; Cdq_inv[1][1] = c; |
kosaka | 12:a4b17bb682eb | 384 | |
kosaka | 12:a4b17bb682eb | 385 | // id, iqからiα, iβを計算 |
kosaka | 12:a4b17bb682eb | 386 | //p.iab = Cdq_inv * p.idq; |
kosaka | 12:a4b17bb682eb | 387 | p.iab[0] = Cdq_inv[0][0]*p.idq[0] + Cdq_inv[0][1]*p.idq[1]; |
kosaka | 12:a4b17bb682eb | 388 | p.iab[1] = Cdq_inv[1][0]*p.idq[0] + Cdq_inv[1][1]*p.idq[1]; |
kosaka | 12:a4b17bb682eb | 389 | |
kosaka | 12:a4b17bb682eb | 390 | // αβ座標からUVW座標への変換行列Cuvw_inv=Cuvw' |
kosaka | 12:a4b17bb682eb | 391 | // iα, iβからiu, iv, iwを計算 |
kosaka | 12:a4b17bb682eb | 392 | // iu = r2/r3*ia; |
kosaka | 12:a4b17bb682eb | 393 | // iv = -1/r2/r3*ia + 1/r2*ib; |
kosaka | 12:a4b17bb682eb | 394 | // iw = -1/r2/r3*ia - 1/r2*ib; |
kosaka | 12:a4b17bb682eb | 395 | //p.iuvw = p.Cuvw' * p.iab; |
kosaka | 12:a4b17bb682eb | 396 | p.iuvw[0] = p.Cuvw[0][0]*p.iab[0] + p.Cuvw[1][0]*p.iab[1]; |
kosaka | 12:a4b17bb682eb | 397 | p.iuvw[1] = p.Cuvw[0][1]*p.iab[0] + p.Cuvw[1][1]*p.iab[1]; |
kosaka | 12:a4b17bb682eb | 398 | p.iuvw[2] = p.Cuvw[0][2]*p.iab[0] + p.Cuvw[1][2]*p.iab[1]; |
kosaka | 12:a4b17bb682eb | 399 | } |
kosaka | 12:a4b17bb682eb | 400 | #endif |
kosaka | 12:a4b17bb682eb | 401 | |
kosaka | 12:a4b17bb682eb | 402 | void data2mbedUSB(){ // save data to mbed USB drive |
kosaka | 12:a4b17bb682eb | 403 | if( _count_data<1000 ){ |
kosaka | 12:a4b17bb682eb | 404 | data[_count_data][0]=p.th[0]/*vl.w_ref*/; data[_count_data][1]=p.vuvw[0]; |
kosaka | 12:a4b17bb682eb | 405 | data[_count_data][2]=vl.w_lpf; data[_count_data][3]=_count*TS0; data[_count_data][4]=il.vdq_ref[1]; |
kosaka | 12:a4b17bb682eb | 406 | _count_data++; |
kosaka | 12:a4b17bb682eb | 407 | } |
kosaka | 12:a4b17bb682eb | 408 | #if 0 |
kosaka | 12:a4b17bb682eb | 409 | if( _count_data<500 ){ |
kosaka | 12:a4b17bb682eb | 410 | 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 | 411 | 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 | 412 | 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 | 413 | 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 | 414 | //BUG for(j=0;j<19;j++){ fprintf( fp, "%f, ",debug[j]);} fprintf( fp, "%f\n",debug[19]); |
kosaka | 12:a4b17bb682eb | 415 | for(j=0;j<19;j++){ printf("%f, ",debug[j]);} printf("%f\n",debug[19]); |
kosaka | 12:a4b17bb682eb | 416 | // for(j=0;j<19;j++){ pc.printf("%f, ",debug[j]);} pc.printf("%f\n",debug[19]); |
kosaka | 12:a4b17bb682eb | 417 | } |
kosaka | 12:a4b17bb682eb | 418 | #endif |
kosaka | 12:a4b17bb682eb | 419 | } |
kosaka | 12:a4b17bb682eb | 420 | void timerTS0(){ // timer called every TS0[s]. |
kosaka | 13:791e20f1af43 | 421 | // debug_p26 = 1; |
kosaka | 12:a4b17bb682eb | 422 | _count++; |
kosaka | 12:a4b17bb682eb | 423 | _time += TS0; |
kosaka | 12:a4b17bb682eb | 424 | |
kosaka | 13:791e20f1af43 | 425 | p.th[1] = p.th[0]; // thを更新 |
kosaka | 12:a4b17bb682eb | 426 | #ifdef SIMULATION |
kosaka | 12:a4b17bb682eb | 427 | // モータシミュレータ |
kosaka | 12:a4b17bb682eb | 428 | sim_motor(); // IPM, dq座標 |
kosaka | 13:791e20f1af43 | 429 | #else |
kosaka | 13:791e20f1af43 | 430 | #ifdef DOUKI |
kosaka | 13:791e20f1af43 | 431 | led1=1; |
kosaka | 13:791e20f1af43 | 432 | p.th[0] += 2*PI*TS0 * 1; if(p.th[0]>4*PI){ p.th[0]-=4*PI;} |
kosakaLab | 15:427f5ae8e957 | 433 | //debug[0]=p.th[0]/PI*180; |
kosaka | 13:791e20f1af43 | 434 | analog_out=debug[0]/180*PI/4/PI; |
kosaka | 13:791e20f1af43 | 435 | led1=0; |
kosakaLab | 14:8e205264baa8 | 436 | #else |
kosakaLab | 14:8e205264baa8 | 437 | // 位置θをセンサで検出 |
kosakaLab | 14:8e205264baa8 | 438 | p.th[0] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI; // get angle [rad] from encoder |
kosakaLab | 15:427f5ae8e957 | 439 | debug[0]=p.th[0]/PI*180; |
kosakaLab | 15:427f5ae8e957 | 440 | debug[1]=p.th[0]/(2*PI); debug[1]=debug[1]-(int)debug[1]; if(debug[1]<0) debug[1]+=1; |
kosakaLab | 15:427f5ae8e957 | 441 | debug[0]=debug[1]*360; |
kosakaLab | 15:427f5ae8e957 | 442 | //analog_out=debug[1]; |
kosaka | 13:791e20f1af43 | 443 | #endif |
kosaka | 12:a4b17bb682eb | 444 | #endif |
kosaka | 13:791e20f1af43 | 445 | current_loop(); // 電流制御マイナーループ(idq_ref to vuvw) |
kosaka | 13:791e20f1af43 | 446 | vuvw2pwm(); // vuvw to pwm |
kosaka | 13:791e20f1af43 | 447 | // debug_p26 = 0; |
kosaka | 12:a4b17bb682eb | 448 | } |
kosaka | 12:a4b17bb682eb | 449 | void timerTS1(void const *argument){ |
kosaka | 13:791e20f1af43 | 450 | // debug_p25 = 1; |
kosaka | 12:a4b17bb682eb | 451 | velocity_loop(); // 速度制御メインループ(w_ref&beta_ref to idq_ref) |
kosaka | 13:791e20f1af43 | 452 | // debug_p25 = 0; |
kosaka | 12:a4b17bb682eb | 453 | } |
kosaka | 12:a4b17bb682eb | 454 | |
kosaka | 12:a4b17bb682eb | 455 | void display2PC(){ // display to tera term on PC |
kosaka | 13:791e20f1af43 | 456 | pc.printf("%8.1f[s]\t%8.5f[V]\t%8.2f [Hz]\t%8.2f\t%8.2f\r\n", |
kosaka | 13:791e20f1af43 | 457 | _time, il.vdq_ref[1], vl.w_lpf/(2*PI), vl.w_ref/(2*PI), debug[0]); // print to tera term |
kosaka | 12:a4b17bb682eb | 458 | // 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 | 459 | } |
kosaka | 12:a4b17bb682eb | 460 | void timerTS2(){ |
kosaka | 12:a4b17bb682eb | 461 | } |
kosaka | 12:a4b17bb682eb | 462 | void timerTS3(){ |
kosaka | 12:a4b17bb682eb | 463 | data2mbedUSB(); // data2mbedUSB() is called every TS3[s]. |
kosaka | 12:a4b17bb682eb | 464 | } |
kosaka | 12:a4b17bb682eb | 465 | void timerTS4(){ |
kosaka | 12:a4b17bb682eb | 466 | display2PC(); // display to tera term on PC. display2PC() is called every TS4[s]. |
kosaka | 12:a4b17bb682eb | 467 | } |