akiyoshi oguro / Mbed 2 deprecated Vector_SVPWM_drive_F302R8

Vector SVPWM Drive SPMSM

Dependencies:   mbed mbed-rtos

main.cpp@0:866aafac0128, 2019-04-02 (annotated)

Committer:
oguro
Date:
Tue Apr 02 04:38:25 2019 +0000
Revision:
0:866aafac0128
Child:
1:74d7a989b741
Vector SVPWM SPMSM

Who changed what in which revision?

UserRevisionLine numberNew contents of line
oguro 0:866aafac0128 1 #include "mbed.h"
oguro 0:866aafac0128 2 #include "rtos.h"
oguro 0:866aafac0128 3 #define TS1 0.2
oguro 0:866aafac0128 4 #include <math.h>
oguro 0:866aafac0128 5 int q=0,START=10,i=0,r=0,s=0;
oguro 0:866aafac0128 6 float uii=0,ut=0,ut1=0,ut2=0,usi=0;
oguro 0:866aafac0128 7 float vii=0,vt=0,vt1=0,vt2=0,vsi=0;
oguro 0:866aafac0128 8 float wii=0,wt=0,wt1=0,wt2=0,wsi=0;
oguro 0:866aafac0128 9
oguro 0:866aafac0128 10 unsigned int ui=0,vi=0,wi=0,et=0;
oguro 0:866aafac0128 11 float su=0,sv=0,sw=0,suv=0,svv=0,swv=0;
oguro 0:866aafac0128 12 float uci=0,uc=0;
oguro 0:866aafac0128 13 float vst=0,vstt=0,vsti=0,vstf=0,Edw=0,PI=3.141592;
oguro 0:866aafac0128 14 float Speed;
oguro 0:866aafac0128 15 float Wnon,Ednon,W,PLL;
oguro 0:866aafac0128 16 float uti,vti,wti;
oguro 0:866aafac0128 17 PwmOut mypwmA(PA_8); //PWM_OUT 8
oguro 0:866aafac0128 18 PwmOut mypwmB(PA_9); //9
oguro 0:866aafac0128 19 PwmOut mypwmC(PA_10);//10
oguro 0:866aafac0128 20
oguro 0:866aafac0128 21 PwmOut Current_Ref(PB_4);
oguro 0:866aafac0128 22
oguro 0:866aafac0128 23 DigitalOut Vector(PC_4);
oguro 0:866aafac0128 24 DigitalOut EN1(PC_10);
oguro 0:866aafac0128 25 DigitalOut EN2(PC_11);
oguro 0:866aafac0128 26 DigitalOut EN3(PC_12);
oguro 0:866aafac0128 27
oguro 0:866aafac0128 28 DigitalOut vcl(PC_8);
oguro 0:866aafac0128 29
oguro 0:866aafac0128 30 InterruptIn HA(PA_15);
oguro 0:866aafac0128 31 InterruptIn HB(PB_3);
oguro 0:866aafac0128 32 InterruptIn HC(PB_10);
oguro 0:866aafac0128 33
oguro 0:866aafac0128 34 //AnalogIn V_adc(PB_1); //volume
oguro 0:866aafac0128 35 AnalogIn V_adc(PC_2); // Gaibu Potention
oguro 0:866aafac0128 36
oguro 0:866aafac0128 37
oguro 0:866aafac0128 38 /*AnalogIn Vshuntu(PA_0);
oguro 0:866aafac0128 39 AnalogIn Vshuntv(PC_1);
oguro 0:866aafac0128 40 AnalogIn Vshuntw(PC_0);*/
oguro 0:866aafac0128 41
oguro 0:866aafac0128 42 AnalogIn BEMF1(PC_3);//C7_37
oguro 0:866aafac0128 43 AnalogIn BEMF2(PB_0);//C7_34
oguro 0:866aafac0128 44 AnalogIn BEMF3(PA_7);//C10_26
oguro 0:866aafac0128 45
oguro 0:866aafac0128 46
oguro 0:866aafac0128 47 DigitalIn GPIO_BEMF(PC_9);
oguro 0:866aafac0128 48 DigitalIn CPOUT(PA_12);
oguro 0:866aafac0128 49
oguro 0:866aafac0128 50 AnalogIn Curr_ui(PA_0);
oguro 0:866aafac0128 51 AnalogIn Curr_vi(PC_1);
oguro 0:866aafac0128 52 AnalogIn Curr_wi(PC_0); //PA_1
oguro 0:866aafac0128 53 Timer uT;
oguro 0:866aafac0128 54 Timer vT;
oguro 0:866aafac0128 55 Timer wT;
oguro 0:866aafac0128 56 AnalogOut SWAVE(PA_4);
oguro 0:866aafac0128 57
oguro 0:866aafac0128 58 Serial pc(USBTX,USBRX);
oguro 0:866aafac0128 59
oguro 0:866aafac0128 60 DigitalOut myled(LED1);
oguro 0:866aafac0128 61
oguro 0:866aafac0128 62 float r2=sqrt(2.0f),r3=sqrt(3.0f);
oguro 0:866aafac0128 63 float Cuvw[2][3]={{r2/r3,-1.0f/r2/r3,-1.0f/r2/r3},
oguro 0:866aafac0128 64 {0,1.0f/r2,-1.0f/r2}};
oguro 0:866aafac0128 65 float iuvw[3];
oguro 0:866aafac0128 66 float iab[2],Vab[2];
oguro 0:866aafac0128 67 float idq[2],idqi[2],idqo[2];
oguro 0:866aafac0128 68 float thave,th,thu,thv,thw,Ed,Vd,Vq,Vqi,West,Wz,Wo,Icom,Wcom;
oguro 0:866aafac0128 69 float zet=sqrt(2.0f/3.0f),cos23=cos((2.0f/3.0f)*PI);
oguro 0:866aafac0128 70 float cos43=cos((4.0f/3.0f)*PI),sin23=sin((2.0f/3.0f)*PI),sin43=sin((4.0f/3.0f)*PI);
oguro 0:866aafac0128 71 float Curr_u,Curr_v,Curr_w;
oguro 0:866aafac0128 72 float Xin,Xout;
oguro 0:866aafac0128 73 float Idin,Vdout,Iqin,Vqout,Xsi;
oguro 0:866aafac0128 74 int vstctle=0,vstctlz=0;
oguro 0:866aafac0128 75 float Vr_adc=0.0f,iso=0,Vqp;;
oguro 0:866aafac0128 76 int adc,vtv=1;
oguro 0:866aafac0128 77
oguro 0:866aafac0128 78 float therr,dth,eth,phm;
oguro 0:866aafac0128 79 float sq32=sqrt(3.0f/2.0f);
oguro 0:866aafac0128 80 float sq23=2.0f/sqrt(3.0f);
oguro 0:866aafac0128 81 float sq3=1.0f/sqrt(3.0f);
oguro 0:866aafac0128 82 float Vdlink=0.6; //3.0f/5.0f; 0.45
oguro 0:866aafac0128 83 float aVa;
oguro 0:866aafac0128 84 float a3Vb;
oguro 0:866aafac0128 85
oguro 0:866aafac0128 86 float z=0.5;//0.7
oguro 0:866aafac0128 87 float Va,Vb;
oguro 0:866aafac0128 88 float d1,d2,d3,d4,d5,d6,d07;
oguro 0:866aafac0128 89 float du,dv,dw;
oguro 0:866aafac0128 90 bool cu,cv,cw;
oguro 0:866aafac0128 91
oguro 0:866aafac0128 92 Ticker sinTime;
oguro 0:866aafac0128 93
oguro 0:866aafac0128 94 void HAH(){
oguro 0:866aafac0128 95 ut1=uT.read_us();
oguro 0:866aafac0128 96 ut=0;
oguro 0:866aafac0128 97 ui=0;
oguro 0:866aafac0128 98
oguro 0:866aafac0128 99 }
oguro 0:866aafac0128 100 void HAL(){
oguro 0:866aafac0128 101 ut2=uT.read_us();
oguro 0:866aafac0128 102 uT.reset();
oguro 0:866aafac0128 103 ui=0;
oguro 0:866aafac0128 104
oguro 0:866aafac0128 105 }
oguro 0:866aafac0128 106 void HBH(){
oguro 0:866aafac0128 107
oguro 0:866aafac0128 108 vt1=vT.read_us();
oguro 0:866aafac0128 109 vi=0;
oguro 0:866aafac0128 110
oguro 0:866aafac0128 111 }
oguro 0:866aafac0128 112 void HBL(){
oguro 0:866aafac0128 113 vt2=vT.read_us();
oguro 0:866aafac0128 114 vT.reset();
oguro 0:866aafac0128 115
oguro 0:866aafac0128 116 }
oguro 0:866aafac0128 117 void HCH(){
oguro 0:866aafac0128 118 wt1=wT.read_us();
oguro 0:866aafac0128 119 wi=0;
oguro 0:866aafac0128 120 et=0;
oguro 0:866aafac0128 121
oguro 0:866aafac0128 122 }
oguro 0:866aafac0128 123
oguro 0:866aafac0128 124 void HCL(){
oguro 0:866aafac0128 125 wt2=wT.read_us();
oguro 0:866aafac0128 126 wT.reset();
oguro 0:866aafac0128 127
oguro 0:866aafac0128 128 }
oguro 0:866aafac0128 129
oguro 0:866aafac0128 130 void CPLT(){
oguro 0:866aafac0128 131 pc.printf("%.3f , %.3f \r" ,Speed ,Vr_adc);
oguro 0:866aafac0128 132 }
oguro 0:866aafac0128 133
oguro 0:866aafac0128 134 void timerTS1(void const*argument){
oguro 0:866aafac0128 135 CPLT();
oguro 0:866aafac0128 136 }
oguro 0:866aafac0128 137
oguro 0:866aafac0128 138 Timer Timer1;
oguro 0:866aafac0128 139 int main() {
oguro 0:866aafac0128 140 pc.baud(128000);
oguro 0:866aafac0128 141
oguro 0:866aafac0128 142 Timer1.start();
oguro 0:866aafac0128 143 EN1=1;
oguro 0:866aafac0128 144 EN2=1;
oguro 0:866aafac0128 145 EN3=1;
oguro 0:866aafac0128 146
oguro 0:866aafac0128 147 mypwmA.period_us(20); //20
oguro 0:866aafac0128 148
oguro 0:866aafac0128 149 mypwmB.period_us(20);
oguro 0:866aafac0128 150
oguro 0:866aafac0128 151 mypwmC.period_us(20);
oguro 0:866aafac0128 152
oguro 0:866aafac0128 153 Current_Ref.period_us(50);
oguro 0:866aafac0128 154 Current_Ref.write(0.55);
oguro 0:866aafac0128 155 uT.start();
oguro 0:866aafac0128 156 vT.start();
oguro 0:866aafac0128 157 wT.start();
oguro 0:866aafac0128 158
oguro 0:866aafac0128 159 RtosTimer RtosTimerTS1(timerTS1);
oguro 0:866aafac0128 160 RtosTimerTS1.start((unsigned int)(TS1*5000)); //3000
oguro 0:866aafac0128 161 Thread::wait(100);
oguro 0:866aafac0128 162
oguro 0:866aafac0128 163 while(1) {
oguro 0:866aafac0128 164
oguro 0:866aafac0128 165 Vr_adc=V_adc.read();
oguro 0:866aafac0128 166
oguro 0:866aafac0128 167 if((Vr_adc>0.15f)&&(q==0)){
oguro 0:866aafac0128 168
oguro 0:866aafac0128 169 while(q<50){
oguro 0:866aafac0128 170
oguro 0:866aafac0128 171 mypwmA.write(0.5f);
oguro 0:866aafac0128 172 mypwmB.write(0);
oguro 0:866aafac0128 173 mypwmC.write(0);
oguro 0:866aafac0128 174 wait_ms(START);
oguro 0:866aafac0128 175
oguro 0:866aafac0128 176 mypwmA.write(0);
oguro 0:866aafac0128 177 mypwmB.write(0.5f);
oguro 0:866aafac0128 178 mypwmC.write(0.0);
oguro 0:866aafac0128 179 wait_ms(START);
oguro 0:866aafac0128 180
oguro 0:866aafac0128 181 mypwmA.write(0);
oguro 0:866aafac0128 182 mypwmB.write(0.0);
oguro 0:866aafac0128 183 mypwmC.write(0.5f);
oguro 0:866aafac0128 184 wait_ms(START);
oguro 0:866aafac0128 185
oguro 0:866aafac0128 186
oguro 0:866aafac0128 187 q++;
oguro 0:866aafac0128 188
oguro 0:866aafac0128 189 }
oguro 0:866aafac0128 190 }
oguro 0:866aafac0128 191 //i++;
oguro 0:866aafac0128 192
oguro 0:866aafac0128 193 HA.rise(&HAH);
oguro 0:866aafac0128 194 HC.fall(&HCL);
oguro 0:866aafac0128 195 HB.rise(&HBH);
oguro 0:866aafac0128 196 HA.fall(&HAL);
oguro 0:866aafac0128 197 HC.rise(&HCH);
oguro 0:866aafac0128 198 HB.fall(&HBL);
oguro 0:866aafac0128 199
oguro 0:866aafac0128 200
oguro 0:866aafac0128 201 Speed=60*(1/(7.0*2.0*usi*1E-6));
oguro 0:866aafac0128 202
oguro 0:866aafac0128 203 if(Vr_adc < 0.08f){
oguro 0:866aafac0128 204 q=0;
oguro 0:866aafac0128 205 Vr_adc=0;
oguro 0:866aafac0128 206 r=0;
oguro 0:866aafac0128 207 vst=0;
oguro 0:866aafac0128 208 i=0;
oguro 0:866aafac0128 209 }
oguro 0:866aafac0128 210
oguro 0:866aafac0128 211
oguro 0:866aafac0128 212 ui=ui+1;
oguro 0:866aafac0128 213 vi=vi+1;
oguro 0:866aafac0128 214 wi=wi+1;
oguro 0:866aafac0128 215 et=et+1;
oguro 0:866aafac0128 216
oguro 0:866aafac0128 217 uti=fabs(ut1-ut2)/float(ui)*1E-6;
oguro 0:866aafac0128 218 vti=fabs(vt1-vt2)/float(vi)*1E-6;
oguro 0:866aafac0128 219 wti=fabs(wt1-wt2)/float(wi)*1E-6;
oguro 0:866aafac0128 220 //pc.printf("%.3f %.3f %.3f \r\n", uti, vti, wti);
oguro 0:866aafac0128 221 //pc.printf(" %d \r\n",ui);
oguro 0:866aafac0128 222 thu=2*PI*(1/(2*usi*1E-6))*ui*1.3E-4; //usi 1.3E-4 vst
oguro 0:866aafac0128 223 thv=2*PI*(1/(2*vsi*1E-6))*vi*1.3E-4;
oguro 0:866aafac0128 224 thw=2*PI*(1/(2*wsi*1E-6))*wi*1.3E-4;
oguro 0:866aafac0128 225
oguro 0:866aafac0128 226 if(r==0){
oguro 0:866aafac0128 227 thave=(thu+thv+thw)/3;
oguro 0:866aafac0128 228 th=thu;
oguro 0:866aafac0128 229
oguro 0:866aafac0128 230 s=0;
oguro 0:866aafac0128 231 }
oguro 0:866aafac0128 232 if(r==1){
oguro 0:866aafac0128 233
oguro 0:866aafac0128 234 if((Speed > 1000)&&(Speed <=1500)){
oguro 0:866aafac0128 235 iso=-0.3;
oguro 0:866aafac0128 236 Vqp=0.3*Vq;
oguro 0:866aafac0128 237 Vdlink=0.3;
oguro 0:866aafac0128 238
oguro 0:866aafac0128 239 z=0.7;
oguro 0:866aafac0128 240 }
oguro 0:866aafac0128 241 if((Speed > 1500)&&(Speed <=2000)){
oguro 0:866aafac0128 242 iso=0.7;
oguro 0:866aafac0128 243 Vqp=0.3*Vq;
oguro 0:866aafac0128 244 Vdlink=0.3;
oguro 0:866aafac0128 245
oguro 0:866aafac0128 246 z=0.7;
oguro 0:866aafac0128 247 }
oguro 0:866aafac0128 248 if((Speed > 2000)&&(Speed <=3000)){
oguro 0:866aafac0128 249 iso=0.8;
oguro 0:866aafac0128 250 Vqp=0.3*Vq;
oguro 0:866aafac0128 251 Vdlink=0.35;
oguro 0:866aafac0128 252
oguro 0:866aafac0128 253 z=0.7;
oguro 0:866aafac0128 254 }
oguro 0:866aafac0128 255 if((Speed > 3000)&&(Speed <=4000)){
oguro 0:866aafac0128 256 iso=1.2;
oguro 0:866aafac0128 257 Vqp=0.4*Vq;
oguro 0:866aafac0128 258 Vdlink=0.5;
oguro 0:866aafac0128 259
oguro 0:866aafac0128 260 z=0.7;
oguro 0:866aafac0128 261 }
oguro 0:866aafac0128 262 if((Speed > 4000)&&(Speed <=5000)){
oguro 0:866aafac0128 263 iso=1.8;
oguro 0:866aafac0128 264 Vqp=0.5*Vq;
oguro 0:866aafac0128 265 Vdlink=0.5;
oguro 0:866aafac0128 266
oguro 0:866aafac0128 267 z=0.7;
oguro 0:866aafac0128 268 }
oguro 0:866aafac0128 269 if((Speed > 5000)&&(Speed <=5500)){
oguro 0:866aafac0128 270 iso=2.2;
oguro 0:866aafac0128 271 Vqp=0.5*Vq;
oguro 0:866aafac0128 272 Vdlink=0.5;
oguro 0:866aafac0128 273
oguro 0:866aafac0128 274 z=0.8;
oguro 0:866aafac0128 275 }
oguro 0:866aafac0128 276 if((Speed > 5500)&&(Speed <=6000)){
oguro 0:866aafac0128 277 iso=2.8;
oguro 0:866aafac0128 278 Vqp=0.7*Vq;
oguro 0:866aafac0128 279 Vdlink=0.6;
oguro 0:866aafac0128 280
oguro 0:866aafac0128 281 z=0.9;
oguro 0:866aafac0128 282 }
oguro 0:866aafac0128 283 if((Speed > 6000)){
oguro 0:866aafac0128 284 iso=3.2;
oguro 0:866aafac0128 285 Vqp=0.7*Vq;
oguro 0:866aafac0128 286 Vdlink=0.9;
oguro 0:866aafac0128 287
oguro 0:866aafac0128 288 z=0.95;
oguro 0:866aafac0128 289 }
oguro 0:866aafac0128 290
oguro 0:866aafac0128 291
oguro 0:866aafac0128 292 // th=(W*et*1.9E-4)+iso; //1.9E-4
oguro 0:866aafac0128 293 // th=(W*et*7.0E-4)+iso; //5.0E-4 +iso 1.9
oguro 0:866aafac0128 294 // th=Wo*wi*1.85E-4;
oguro 0:866aafac0128 295 }
oguro 0:866aafac0128 296
oguro 0:866aafac0128 297 if(r==0){
oguro 0:866aafac0128 298 su=(sin(thu)+0.2);//-0.2f; // p51 0.5
oguro 0:866aafac0128 299 sv=(sin(thv)+0.2);//-0.05f;
oguro 0:866aafac0128 300 sw=(sin(thw)+0.2);//-0.05f;//-0.7
oguro 0:866aafac0128 301
oguro 0:866aafac0128 302 mypwmA.write(su*Vr_adc);
oguro 0:866aafac0128 303 mypwmB.write(sv*Vr_adc);
oguro 0:866aafac0128 304 mypwmC.write(sw*Vr_adc);
oguro 0:866aafac0128 305 }
oguro 0:866aafac0128 306 if(r==1){
oguro 0:866aafac0128 307 mypwmA.write(du);
oguro 0:866aafac0128 308 mypwmB.write(dv);
oguro 0:866aafac0128 309 mypwmC.write(dw);
oguro 0:866aafac0128 310 }
oguro 0:866aafac0128 311
oguro 0:866aafac0128 312
oguro 0:866aafac0128 313 myled = !myled;
oguro 0:866aafac0128 314 // }
oguro 0:866aafac0128 315 // }
oguro 0:866aafac0128 316
oguro 0:866aafac0128 317 Curr_u=(Curr_ui);
oguro 0:866aafac0128 318 Curr_v=(Curr_vi);
oguro 0:866aafac0128 319 Curr_w=(Curr_wi);
oguro 0:866aafac0128 320
oguro 0:866aafac0128 321 float Itau=1.0E-6,Idt=1.0E-6; // Itau=1.0E-6,Idt=1.0E-6;
oguro 0:866aafac0128 322 /****Filter Iu********/
oguro 0:866aafac0128 323 float Iu1,Iu2,Iu3,Iu4;//0.01
oguro 0:866aafac0128 324 Iu1=Idt*(Curr_u-iuvw[0])/Itau;
oguro 0:866aafac0128 325 Iu2=Idt*(Curr_u-(iuvw[0]+Iu1/2.0))/Itau;
oguro 0:866aafac0128 326 Iu3=Idt*(Curr_u-(iuvw[0]+Iu2/2.0))/Itau;
oguro 0:866aafac0128 327 Iu4=Idt*(Curr_u-(iuvw[0]+Iu3/2.0))/Itau;
oguro 0:866aafac0128 328 iuvw[0]=iuvw[0]+(Iu1+2.0*Iu2+2.0*Iu3+Iu4)/6.0;
oguro 0:866aafac0128 329 /*************************************/
oguro 0:866aafac0128 330 /****Filter Iv********/
oguro 0:866aafac0128 331 float Iv1,Iv2,Iv3,Iv4;//0.01
oguro 0:866aafac0128 332 Iv1=Idt*(Curr_v-iuvw[1])/Itau;
oguro 0:866aafac0128 333 Iv2=Idt*(Curr_v-(iuvw[1]+Iv1/2.0))/Itau;
oguro 0:866aafac0128 334 Iv3=Idt*(Curr_v-(iuvw[1]+Iv2/2.0))/Itau;
oguro 0:866aafac0128 335 Iv4=Idt*(Curr_v-(iuvw[1]+Iv3/2.0))/Itau;
oguro 0:866aafac0128 336 iuvw[1]=iuvw[1]+(Iv1+2.0*Iv2+2.0*Iv3+Iv4)/6.0;
oguro 0:866aafac0128 337 /*************************************/
oguro 0:866aafac0128 338 /****Filter Iw********/
oguro 0:866aafac0128 339 float Iw1,Iw2,Iw3,Iw4;//0.01
oguro 0:866aafac0128 340 Iw1=Idt*(Curr_w-iuvw[2])/Itau;
oguro 0:866aafac0128 341 Iw2=Idt*(Curr_w-(iuvw[2]+Iw1/2.0))/Itau;
oguro 0:866aafac0128 342 Iw3=Idt*(Curr_w-(iuvw[2]+Iw2/2.0))/Itau;
oguro 0:866aafac0128 343 Iw4=Idt*(Curr_w-(iuvw[2]+Iw3/2.0))/Itau;
oguro 0:866aafac0128 344 iuvw[2]=iuvw[2]+(Iw1+2.0*Iw2+2.0*Iw3+Iw4)/6.0;
oguro 0:866aafac0128 345 /*************************************/
oguro 0:866aafac0128 346
oguro 0:866aafac0128 347
oguro 0:866aafac0128 348 iab[0]=(iuvw[0]+iuvw[1]*cos23+iuvw[2]*cos43)*zet;
oguro 0:866aafac0128 349 iab[1]=(iuvw[1]*sin23+iuvw[2]*sin43)*zet;
oguro 0:866aafac0128 350
oguro 0:866aafac0128 351
oguro 0:866aafac0128 352 idq[0]=cos(th)*iab[0]+sin(th)*iab[1]+0.5; //thave
oguro 0:866aafac0128 353 idq[1]=-sin(th)*iab[0]+cos(th)*iab[1]+0.5; //th
oguro 0:866aafac0128 354
oguro 0:866aafac0128 355
oguro 0:866aafac0128 356 /****Filter Id********/
oguro 0:866aafac0128 357 idqo[0]=idq[0];
oguro 0:866aafac0128 358 float Id1,Id2,Id3,Id4;//0.01
oguro 0:866aafac0128 359 float Idtau= 1.0E-6,Iddt=1.0E-6; //1E-1
oguro 0:866aafac0128 360 Id1=Iddt*(idqi[0]-idqo[0])/Idtau;
oguro 0:866aafac0128 361 Id2=Iddt*(idqi[0]-(idqo[0]+Id1/2.0))/Idtau;
oguro 0:866aafac0128 362 Id3=Iddt*(idqi[0]-(idqo[0]+Id2/2.0))/Idtau;
oguro 0:866aafac0128 363 Id4=Iddt*(idqi[0]-(idqo[0]+Id3))/Idtau;
oguro 0:866aafac0128 364 idq[0]=idqo[0]+(Id1+2.0*Id2+2.0*Id3+Id4)/6.0;
oguro 0:866aafac0128 365 /*************************************/
oguro 0:866aafac0128 366
oguro 0:866aafac0128 367 /****Filter Iq********/
oguro 0:866aafac0128 368 idqo[1]=idq[1];
oguro 0:866aafac0128 369 float Iq1,Iq2,Iq3,Iq4;//0.01
oguro 0:866aafac0128 370 float Iqtau= 1.0E-6,Iqdt=1.0E-6;
oguro 0:866aafac0128 371 Iq1=Iqdt*(idqi[1]-idqo[1])/Iqtau;
oguro 0:866aafac0128 372 Iq2=Iqdt*(idqi[1]-(idqo[1]+Iq1/2.0))/Iqtau;
oguro 0:866aafac0128 373 Iq3=Iqdt*(idqi[1]-(idqo[1]+Iq2/2.0))/Iqtau;
oguro 0:866aafac0128 374 Iq4=Iqdt*(idqi[1]-(idqo[1]+Iq3/2.0))/Iqtau;
oguro 0:866aafac0128 375 idq[1]=idqo[1]+(Iq1+2.0*Iq2+2.0*Iq3+Iq4)/6.0;
oguro 0:866aafac0128 376 /*************************************/
oguro 0:866aafac0128 377
oguro 0:866aafac0128 378 /*****PID Id　*****/
oguro 0:866aafac0128 379 Idin=(-0.25)-idq[0];
oguro 0:866aafac0128 380 float adi,bdi,cdi,workdi[2];
oguro 0:866aafac0128 381 float kpdi=1.0,kidi=0.5,kddi=0.0;
oguro 0:866aafac0128 382 float dtdi=1.0E-6;//1E-6
oguro 0:866aafac0128 383 adi=Idin;
oguro 0:866aafac0128 384 bdi=workdi[1]+(Idin+workdi[0])/2.0*dtdi;
oguro 0:866aafac0128 385 cdi=(Idin-workdi[0])/dtdi;
oguro 0:866aafac0128 386 workdi[0]=Idin;
oguro 0:866aafac0128 387 workdi[1]=bdi;
oguro 0:866aafac0128 388 Vd=adi*kpdi+bdi*kidi+cdi*kddi;
oguro 0:866aafac0128 389 /**********************************/
oguro 0:866aafac0128 390
oguro 0:866aafac0128 391 /*****PID Iq　*****/
oguro 0:866aafac0128 392 Iqin=Vr_adc-idq[1];
oguro 0:866aafac0128 393 float aqi,bqi,cqi,workqi[2];
oguro 0:866aafac0128 394 float kpqi=2.0,kiqi=1.2,kdqi=0.0; // 1.5 0.8
oguro 0:866aafac0128 395 float dtqi=1.0E-6;//1E-2
oguro 0:866aafac0128 396 aqi=Iqin;
oguro 0:866aafac0128 397 bqi=workqi[1]+(Iqin+workqi[0])/2.0*dtqi;
oguro 0:866aafac0128 398 cqi=(Iqin-workqi[0])/dtqi;
oguro 0:866aafac0128 399 workqi[0]=Iqin;
oguro 0:866aafac0128 400 workdi[1]=bqi;
oguro 0:866aafac0128 401 Vq=aqi*kpqi+bqi*kiqi+cqi*kdqi;
oguro 0:866aafac0128 402 /**********************************/
oguro 0:866aafac0128 403
oguro 0:866aafac0128 404 //if(i<10000){
oguro 0:866aafac0128 405 //if(r==0){
oguro 0:866aafac0128 406 usi=ut2-ut1;
oguro 0:866aafac0128 407 vsi=vt2-vt1;
oguro 0:866aafac0128 408 wsi=wt2-wt1;
oguro 0:866aafac0128 409 if(i<10000){
oguro 0:866aafac0128 410 vst=vsi;
oguro 0:866aafac0128 411 vstf=vst;
oguro 0:866aafac0128 412 }
oguro 0:866aafac0128 413 //}
oguro 0:866aafac0128 414 else { //kokokara else
oguro 0:866aafac0128 415 //if(r==1){
oguro 0:866aafac0128 416 // usi=vstf;vsi=vstf;wsi=vstf;
oguro 0:866aafac0128 417 vst=vstf;
oguro 0:866aafac0128 418
oguro 0:866aafac0128 419 i=10000;
oguro 0:866aafac0128 420
oguro 0:866aafac0128 421 // }
oguro 0:866aafac0128 422 // r=i%100;
oguro 0:866aafac0128 423 // i++;
oguro 0:866aafac0128 424
oguro 0:866aafac0128 425
oguro 0:866aafac0128 426 Wz=(2*PI)/(vst*1E-6); //vst
oguro 0:866aafac0128 427 Ed= (Vq)-0.11f*idq[1]-Wz*0.018E-3*idq[0];//0.018E-3
oguro 0:866aafac0128 428 phm=Ed/(Wz);
oguro 0:866aafac0128 429 dth=(Vd-0.11f*(idq[0])+Wz*0.018E-3*(idq[1]))/(Wz*phm); //0.018E-3
oguro 0:866aafac0128 430 eth=asin(dth);
oguro 0:866aafac0128 431
oguro 0:866aafac0128 432 /*****PID θ　*****/
oguro 0:866aafac0128 433
oguro 0:866aafac0128 434 PLL=4.0;
oguro 0:866aafac0128 435 //float WPLL=150.0;
oguro 0:866aafac0128 436 float as,bs,cs,works[2];
oguro 0:866aafac0128 437 float kps=PLL,kis=PLL*PLL/5.0,kds=0.0;
oguro 0:866aafac0128 438 float dts=1.0E-6;
oguro 0:866aafac0128 439 Xsi=5.0-eth;
oguro 0:866aafac0128 440 as=Xsi;
oguro 0:866aafac0128 441 bs=works[1]+(Xsi+works[0])/2.0*dts;
oguro 0:866aafac0128 442 cs=(Xsi-works[0])/dts;
oguro 0:866aafac0128 443 works[0]=Xsi;
oguro 0:866aafac0128 444 works[1]=bs;
oguro 0:866aafac0128 445 therr=as*kps+bs*kis+cs*kds;
oguro 0:866aafac0128 446 /*******PLL W ***********/
oguro 0:866aafac0128 447 if(therr>0.01){
oguro 0:866aafac0128 448 W=(2*PI)/((vst*1E-6)+(therr/Wz));
oguro 0:866aafac0128 449 }
oguro 0:866aafac0128 450 if(therr<-0.01){
oguro 0:866aafac0128 451 W=(2*PI)/((vst*1E-6)-(therr/Wz));
oguro 0:866aafac0128 452 }
oguro 0:866aafac0128 453
oguro 0:866aafac0128 454 /*****PID ω　*****/
oguro 0:866aafac0128 455 Xin=5500*(1.07-Vr_adc)-vstf; // 5500 1.05
oguro 0:866aafac0128 456 float a,b,c,work[2];
oguro 0:866aafac0128 457 float kp=2.0,ki=0.5,kd=0.0; // 1.2 0.7
oguro 0:866aafac0128 458 float dt=1.0E-6;//10.0E-6
oguro 0:866aafac0128 459 a=Xin;
oguro 0:866aafac0128 460 b=work[1]+(Xin+work[0])/2.0*dt;
oguro 0:866aafac0128 461 c=(Xin-work[0])/dt;
oguro 0:866aafac0128 462 work[0]=Xin;
oguro 0:866aafac0128 463 work[1]=b;
oguro 0:866aafac0128 464 Xout=a*kp+b*ki+c*kd;
oguro 0:866aafac0128 465
oguro 0:866aafac0128 466 vstt=Xout;
oguro 0:866aafac0128 467 /********************************/
oguro 0:866aafac0128 468 if(2000>abs(vst-vstt)){ // ie 1000
oguro 0:866aafac0128 469 vsti=vstf;
oguro 0:866aafac0128 470 // vsti=vst;
oguro 0:866aafac0128 471 r=1;
oguro 0:866aafac0128 472 }
oguro 0:866aafac0128 473 /*else{
oguro 0:866aafac0128 474 vsti=vst;
oguro 0:866aafac0128 475 r=0;
oguro 0:866aafac0128 476 }*/
oguro 0:866aafac0128 477 /****Filter********/
oguro 0:866aafac0128 478 float vstfo=vstf;
oguro 0:866aafac0128 479 float dttt=10.0E-6;//100E-6
oguro 0:866aafac0128 480 float k11,k22,k33,k44,tau1=0.01;//0.01
oguro 0:866aafac0128 481 k11=dttt*(vsti-vstfo)/tau1;
oguro 0:866aafac0128 482 k22=dttt*(vsti-(vstfo+k11/2.0))/tau1;
oguro 0:866aafac0128 483 k33=dttt*(vsti-(vstfo+k22/2.0))/tau1;
oguro 0:866aafac0128 484 k44=dttt*(vsti-(vstfo+k33))/tau1;
oguro 0:866aafac0128 485 vstf=vstfo+(k11+2.0*k22+2.0*k33+k44)/6.0;
oguro 0:866aafac0128 486 //vstf=vsti;
oguro 0:866aafac0128 487 /*************************************/
oguro 0:866aafac0128 488
oguro 0:866aafac0128 489
oguro 0:866aafac0128 490
oguro 0:866aafac0128 491 }//else kokomade
oguro 0:866aafac0128 492
oguro 0:866aafac0128 493
oguro 0:866aafac0128 494
oguro 0:866aafac0128 495 Va=cos(th)*Vd-sin(th)*Vq; //Vqp
oguro 0:866aafac0128 496 Vb=sin(th)*Vd+cos(th)*Vq; //Vqp
oguro 0:866aafac0128 497
oguro 0:866aafac0128 498 aVa=abs(Va);
oguro 0:866aafac0128 499 a3Vb=abs(sq3*Vb);
oguro 0:866aafac0128 500 if((Va>=0)&&(Vb>=0)&&(aVa>=a3Vb)){ //sect 0
oguro 0:866aafac0128 501
oguro 0:866aafac0128 502 d1=sq32*(Va-sq3*Vb)*Vdlink;
oguro 0:866aafac0128 503 d2=sq32*(sq23*Vb)*Vdlink;
oguro 0:866aafac0128 504 d07=(z-(d1+d2))*0.5;
oguro 0:866aafac0128 505 // d07=0;
oguro 0:866aafac0128 506 du=d1+d2+d07;
oguro 0:866aafac0128 507 dv=d2+d07;
oguro 0:866aafac0128 508 dw=d07;
oguro 0:866aafac0128 509
oguro 0:866aafac0128 510 }
oguro 0:866aafac0128 511 if((aVa<=sq3*Vb)){ //sect 1
oguro 0:866aafac0128 512
oguro 0:866aafac0128 513 d3=sq32*(-Va+sq3*Vb)*Vdlink;
oguro 0:866aafac0128 514 d2=sq32*(Va+sq3*Vb)*Vdlink;
oguro 0:866aafac0128 515 d07=(z-(d2+d3))*0.5;
oguro 0:866aafac0128 516 //d07=0;
oguro 0:866aafac0128 517 du=d2+d07;
oguro 0:866aafac0128 518 dv=d2+d3+d07;
oguro 0:866aafac0128 519 dw=d07;
oguro 0:866aafac0128 520 }
oguro 0:866aafac0128 521
oguro 0:866aafac0128 522 if((Va<=0)&&(Vb>=0)&&(aVa>=a3Vb)){ //sect 2
oguro 0:866aafac0128 523
oguro 0:866aafac0128 524 d3=sq32*sq23*Vb*Vdlink;
oguro 0:866aafac0128 525 d4=sq32*(-Va-sq3*Vb)*Vdlink;
oguro 0:866aafac0128 526 d07=(z-(d3+d4))*0.5;
oguro 0:866aafac0128 527 // d07=0;
oguro 0:866aafac0128 528 du=d07;
oguro 0:866aafac0128 529 dv=d3+d4+d07;
oguro 0:866aafac0128 530 dw=d4+d07;
oguro 0:866aafac0128 531 }
oguro 0:866aafac0128 532
oguro 0:866aafac0128 533 if((Va<=0)&&(Vb<=0)&&(aVa>=a3Vb)){ //sect 3
oguro 0:866aafac0128 534
oguro 0:866aafac0128 535 d5=-sq32*sq23*Vb*Vdlink;
oguro 0:866aafac0128 536 d4=sq32*(-Va+sq3*Vb)*Vdlink;
oguro 0:866aafac0128 537 d07=(z-(d4+d5))*0.5;
oguro 0:866aafac0128 538 //d07=0;
oguro 0:866aafac0128 539 du=d07;
oguro 0:866aafac0128 540 dv=d4+d07;
oguro 0:866aafac0128 541 dw=d4+d5+d07;
oguro 0:866aafac0128 542 }
oguro 0:866aafac0128 543
oguro 0:866aafac0128 544 if((aVa<=-sq3*Vb)){ //sect 4
oguro 0:866aafac0128 545
oguro 0:866aafac0128 546 d5=sq32*(-Va-sq3*Vb)*Vdlink;;
oguro 0:866aafac0128 547 d6=sq32*(Va-sq3*Vb)*Vdlink;
oguro 0:866aafac0128 548 d07=(z-(d5+d6))*0.5;
oguro 0:866aafac0128 549 //d07=0;
oguro 0:866aafac0128 550 du=d6+d07;
oguro 0:866aafac0128 551 dv=d07;
oguro 0:866aafac0128 552 dw=d5+d6+d07;
oguro 0:866aafac0128 553 }
oguro 0:866aafac0128 554
oguro 0:866aafac0128 555 if((Va>=0)&&(Vb<=0)&&(aVa>=a3Vb)){ //sect 5
oguro 0:866aafac0128 556
oguro 0:866aafac0128 557 d1=sq32*(Va+sq3*Vb)*Vdlink;;
oguro 0:866aafac0128 558 d6=-sq32*sq23*Vb*Vdlink;;
oguro 0:866aafac0128 559 d07=(z-(d1+d6))*0.5;
oguro 0:866aafac0128 560 //d07=0;
oguro 0:866aafac0128 561 du=d1+d6+d07;
oguro 0:866aafac0128 562 dv=d07;
oguro 0:866aafac0128 563 dw=d6+d07;
oguro 0:866aafac0128 564 }
oguro 0:866aafac0128 565
oguro 0:866aafac0128 566
oguro 0:866aafac0128 567
oguro 0:866aafac0128 568 // SWAVE=Vd;
oguro 0:866aafac0128 569 // SWAVE=th/4;
oguro 0:866aafac0128 570 // SWAVE=Vq;
oguro 0:866aafac0128 571 // SWAVE=West/5000;
oguro 0:866aafac0128 572 //SWAVE=Wo/5000;
oguro 0:866aafac0128 573 //SWAVE=Wz/5000;
oguro 0:866aafac0128 574 // SWAVE=sin(suv);
oguro 0:866aafac0128 575 // SWAVE=iab[0]+0.5;
oguro 0:866aafac0128 576 // SWAVE=Ed/10;
oguro 0:866aafac0128 577 // SWAVE=idq[1];
oguro 0:866aafac0128 578 // SWAVE=sin(th);
oguro 0:866aafac0128 579 // SWAVE=vst/5000;
oguro 0:866aafac0128 580 //SWAVE=Vb;
oguro 0:866aafac0128 581 //SWAVE=iuvw[0];
oguro 0:866aafac0128 582 SWAVE=su;
oguro 0:866aafac0128 583 Vector=r;
oguro 0:866aafac0128 584 i++;
oguro 0:866aafac0128 585 }
oguro 0:866aafac0128 586 }