Vector sin
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "rtos.h" 00003 #define TS1 0.2 00004 #include <math.h> 00005 int q=0,START=7,i=0,s=0,vector; 00006 float uii=0,ut=0,ut1=0,ut2=0,usi=0; 00007 float vii=0,vt=0,vt1=0,vt2=0,vsi=0; 00008 float wii=0,wt=0,wt1=0,wt2=0,wsi=0; 00009 unsigned int ui=0,vi=0,wi=0,vect=0; 00010 float su=0,sv=0,sw=0,suv=0,svv=0,swv=0; 00011 float uci=0,uc=0; 00012 float vst=0,vstt=0,vsti=0,vstf=0,Edw=0,PI=3.141592; 00013 float Speed; 00014 float w,Vqp; 00015 float dth,phm,W,eth,therr,Em,PLL; 00016 float uvect,vvect,wvect; 00017 float sig=0; 00018 PwmOut mypwmA(PA_8); //PWM_OUT 8 00019 PwmOut mypwmB(PA_9); //9 00020 PwmOut mypwmC(PA_10);//10 00021 00022 PwmOut Current_Ref(PB_4); 00023 00024 DigitalOut Vector(PC_4); 00025 DigitalOut EN1(PC_10); 00026 DigitalOut EN2(PC_11); 00027 DigitalOut EN3(PC_12); 00028 00029 DigitalOut vcl(PC_8); 00030 00031 InterruptIn HA(PA_15); 00032 InterruptIn HB(PB_3); 00033 InterruptIn HC(PB_10); 00034 00035 //AnalogIn V_adc(PB_1); //volume 00036 AnalogIn V_adc(PC_2); //Potention 00037 00038 00039 AnalogIn Vshuntu(PA_1); 00040 AnalogIn Vshuntv(PA_0); 00041 AnalogIn Vshuntw(PB_0); 00042 00043 AnalogIn BEMF1(PC_3);//C7_37 00044 AnalogIn BEMF2(PB_0);//C7_34 00045 AnalogIn BEMF3(PA_7);//C10_26 00046 00047 DigitalIn GPIO_BEMF(PC_9); 00048 DigitalIn CPOUT(PA_12); 00049 AnalogIn Curr_ui(PA_0); 00050 AnalogIn Curr_vi(PC_1); 00051 AnalogIn Curr_wi(PC_0); //PA_1 00052 Timer uT; 00053 Timer vT; 00054 Timer wT; 00055 AnalogOut SWAVE(PA_4); 00056 00057 Serial pc(USBTX,USBRX); 00058 DigitalOut myled(LED1); 00059 00060 float r2=sqrt(2.0f),r3=sqrt(3.0f); 00061 float Cuvw[2][3]={{r2/r3,-1.0f/r2/r3,-1.0f/r2/r3}, 00062 {0,1.0f/r2,-1.0f/r2}}; 00063 float iuvw[3]; 00064 float iab[2],iabi[2],Vab[2]; 00065 float idq[2],idqi[2]; 00066 float thave,th,thu,thv,thw,Ed,Vd,Vq,Vqi,West,Wz,Wo,Icom,Wcom; 00067 float zet=sqrt(2.0f/3.0f),cos23=cos((2.0f/3.0f)*PI); 00068 float cos43=cos((4.0f/3.0f)*PI),sin23=sin((2.0f/3.0f)*PI),sin43=sin((4.0f/3.0f)*PI); 00069 float Curr_u,Curr_v,Curr_w; 00070 float Xin,Xout; 00071 float Idin,Vdout,Iqin,Vqout,Xsi; 00072 int vstctle=0,vstctlz=0; 00073 float Vr_adc=0.0f; 00074 int adc,vtv=0; 00075 00076 Ticker sinTime; 00077 00078 void HAH(){ 00079 00080 ut1=uT.read_us(); 00081 ut=0; 00082 ui=0; 00083 // vect=0; 00084 if(vector==0){ 00085 EN1=1; 00086 EN2=1; 00087 EN3=0; 00088 } 00089 /*if(vector==0){ 00090 mypwmA.write(su*Vr_adc); //0.5 00091 mypwmB.write(0); 00092 mypwmC.write(0); 00093 }*/ 00094 } 00095 void HAL(){ 00096 00097 ut2=uT.read_us(); 00098 uT.reset(); 00099 if(Speed<=2500){ 00100 // wait_us(100); 00101 vect=0; //****** 00102 } 00103 00104 if(vector==0){ 00105 EN1=1; 00106 EN2=0; 00107 EN3=1; 00108 } 00109 } 00110 void HBH(){ 00111 00112 vt1=vT.read_us(); 00113 vi=0; 00114 if(Speed>2500){ 00115 vect=0; 00116 } 00117 if(vector==0){ 00118 EN1=0; 00119 EN2=1; 00120 EN3=1; 00121 } 00122 /* if(vector==0){ 00123 mypwmA.write(0); 00124 mypwmB.write(sv*Vr_adc); 00125 mypwmC.write(0); 00126 }*/ 00127 } 00128 void HBL(){ 00129 00130 vt2=vT.read_us(); 00131 // vect=0; 00132 vT.reset(); 00133 if(vector==0){ 00134 EN1=1; 00135 EN2=1; 00136 EN3=0; 00137 } 00138 00139 } 00140 void HCH(){ 00141 wt1=wT.read_us(); 00142 wi=0; 00143 //vect=0; 00144 if(vector==0){ 00145 EN1=1; 00146 EN2=0; 00147 EN3=1; 00148 } 00149 /* if(vector==0){ 00150 mypwmA.write(0); 00151 mypwmB.write(0); 00152 mypwmC.write(sw*Vr_adc); 00153 }*/ 00154 } 00155 00156 void HCL(){ 00157 wt2=wT.read_us(); 00158 // vect=0; 00159 wT.reset(); 00160 if(vector==0){ 00161 EN1=0; 00162 EN2=1; 00163 EN3=1; 00164 } 00165 } 00166 00167 void CPLT(){ 00168 pc.printf("%.3f , %.3f \r" ,Speed ,Vr_adc); 00169 } 00170 00171 void timerTS1(void const*argument){ 00172 CPLT(); 00173 } 00174 00175 Timer Timer1; 00176 int main() { 00177 00178 pc.baud(128000); 00179 Timer1.start(); 00180 EN1=1; 00181 EN2=1; 00182 EN3=1; 00183 00184 mypwmA.period_us(20); //20 00185 00186 mypwmB.period_us(20); 00187 00188 mypwmC.period_us(20); 00189 00190 Current_Ref.period_us(15); 00191 Current_Ref.write(0.6f); 00192 uT.start(); 00193 vT.start(); 00194 wT.start(); 00195 Timer1.start(); 00196 00197 RtosTimer RtosTimerTS1(timerTS1); 00198 RtosTimerTS1.start((unsigned int)(TS1*3000)); 00199 Thread::wait(100); 00200 00201 while(1) { 00202 00203 /* if(CPOUT==1){ 00204 vect=0; 00205 }*/ 00206 00207 Vr_adc=V_adc.read(); 00208 00209 if((Vr_adc>0.08f)&&(q==0)){ 00210 00211 while(q<20){ 00212 00213 /* mypwmA.write(0.5f); 00214 mypwmB.write(0); 00215 mypwmC.write(0); 00216 wait_ms(START); 00217 00218 mypwmA.write(0); 00219 mypwmB.write(0.5f); 00220 mypwmC.write(0.0); 00221 wait_ms(START); 00222 00223 mypwmA.write(0); 00224 mypwmB.write(0.0); 00225 mypwmC.write(0.5f); 00226 wait_ms(START);*/ 00227 00228 EN1=1; 00229 EN2=1; 00230 EN3=0; 00231 00232 mypwmA.write(0.5f); //0.5 00233 mypwmB.write(0); 00234 mypwmC.write(0); 00235 wait_ms(START); 00236 //wait_ms(START); 00237 00238 EN1=1; 00239 EN2=0; 00240 EN3=1; 00241 00242 wait_ms(START); 00243 00244 EN1=0; 00245 EN2=1; 00246 EN3=1; 00247 00248 mypwmA.write(0); 00249 mypwmB.write(0.5f); 00250 mypwmC.write(0); 00251 00252 wait_ms(START); 00253 00254 EN1=1; 00255 EN2=1; 00256 EN3=0; 00257 00258 wait_ms(START); 00259 00260 EN1=1; 00261 EN2=0; 00262 EN3=1; 00263 00264 mypwmA.write(0); 00265 mypwmB.write(0); 00266 mypwmC.write(0.5f); 00267 00268 wait_ms(START); 00269 EN1=0; 00270 EN2=1; 00271 EN3=1; 00272 q++; 00273 00274 } 00275 } 00276 00277 /* HA.rise(&HAH); 00278 HC.fall(&HCL); 00279 HB.rise(&HBH); 00280 HA.fall(&HAL); 00281 HC.rise(&HCH); 00282 HB.fall(&HBL);*/ 00283 00284 HA.rise(&HAH); 00285 HC.fall(&HCL); 00286 HB.rise(&HBH); 00287 HA.fall(&HAL); 00288 HC.rise(&HCH); 00289 HB.fall(&HBL); 00290 00291 Speed=60*(1/(7.0*2.0*usi*1E-6)); 00292 00293 if(Vr_adc < 0.05f){ 00294 q=0; 00295 Vr_adc=0; 00296 vector=0; 00297 vst=0; 00298 i=0; 00299 } 00300 00301 ui=ui+1; 00302 vi=vi+1; 00303 wi=wi+1; 00304 vect++; 00305 /* if(Curr_ui>0.48){ //&&(vtv==0)){ 00306 vect=0; 00307 00308 vtv=1; 00309 vcl=1; 00310 }*/ 00311 /* if((Curr_ui<=0.4)&&(vtv==0)){ 00312 // vect=0; 00313 vtv=1; 00314 vcl=0; 00315 }*/ 00316 /*if(vect==100000){ 00317 vtv=1; 00318 }*/ 00319 00320 if(vector==0){ 00321 thu=2*PI*(1/(usi*1E-6))*ui*1.54E-4; //1.24E-4 00322 thv=2*PI*(1/(vsi*1E-6))*vi*1.54E-4; 00323 thw=2*PI*(1/(wsi*1E-6))*wi*1.54E-4; 00324 00325 thave=(thu+thv+thw)/3; 00326 th=thu; 00327 s=0; 00328 } 00329 if((vector==1)){ 00330 if((Speed > 500)&&(Speed <=1000)){ 00331 00332 Vqp=0.3*Vq; 00333 th=W*vect*1.0E-4; 00334 00335 } 00336 if((Speed > 1000)&&(Speed <=1500)){//1000 00337 00338 Vqp=0.3*Vq; //0.5 00339 th=W*vect*1.05E-4; 00340 00341 } 00342 if((Speed > 1500)&&(Speed <=2000)){ 00343 00344 Vqp=0.4*Vq; //0.65 00345 th=W*vect*1.1E-4; 00346 00347 } 00348 if((Speed > 2000)&&(Speed <=2500)){ 00349 00350 Vqp=0.5*Vq; 00351 th=W*vect*1.15E-4; 00352 00353 } 00354 if((Speed > 2500)&&(Speed <=3000)){ 00355 00356 Vqp=0.6*Vq; 00357 th=W*vect*1.2E-4; 00358 00359 } 00360 if((Speed > 3000)&&(Speed <=3500)){ 00361 00362 Vqp=0.7*Vq; 00363 th=W*vect*1.25E-4; 00364 00365 } 00366 if((Speed > 3500)&&(Speed <=4000)){ 00367 00368 Vqp=0.8*Vq; 00369 th=W*vect*1.3E-4; 00370 00371 } 00372 if((Speed > 4000)&&(Speed <=4500)){ 00373 00374 Vqp=0.9*Vq; 00375 th=W*vect*1.35E-4; 00376 00377 } 00378 if((Speed > 4500)&&(Speed <=5000)){ 00379 00380 Vqp=1.0*Vq; 00381 th=W*vect*1.4E-4; 00382 00383 } 00384 if((Speed > 5000)&&(Speed <= 6000)){ 00385 00386 Vqp=1.2*Vq; 00387 th=W*vect*1.45E-4; 00388 00389 } 00390 if((Speed > 6000)){ 00391 00392 Vqp=1.3*Vq; 00393 th=W*vect*1.5E-4; 00394 00395 } 00396 00397 // th=(W*((vst*1.0E-6))); //1.0E-4 @HAL 00398 // th=W*vect*1.5E-4; 00399 } 00400 00401 if(vector==0){ 00402 su=sin(thu); 00403 sv=sin(thv); 00404 sw=sin(thw); 00405 00406 mypwmA.write(su*Vr_adc); 00407 mypwmB.write(sv*Vr_adc); 00408 mypwmC.write(sw*Vr_adc); 00409 } 00410 if((vector==1)){ 00411 EN1=1; 00412 EN2=1; 00413 EN3=1; 00414 mypwmA.write(uvect); 00415 mypwmB.write(vvect); 00416 mypwmC.write(wvect); 00417 } 00418 00419 myled = !myled; 00420 00421 Curr_u=(Curr_ui); 00422 Curr_v=(Curr_vi); 00423 Curr_w=(Curr_wi); 00424 00425 float Itau=10.0E-6,Idt=1.0E-6; // Itau=1.0E-6,Idt=1.0E-6 00426 /****Filter Iu********/ 00427 float Iu1,Iu2,Iu3,Iu4;//0.01 00428 Iu1=Idt*(Curr_u-iuvw[0])/Itau; 00429 Iu2=Idt*(Curr_u-(iuvw[0]+Iu1/2.0))/Itau; 00430 Iu3=Idt*(Curr_u-(iuvw[0]+Iu2/2.0))/Itau; 00431 Iu4=Idt*(Curr_u-(iuvw[0]+Iu3))/Itau; 00432 iuvw[0]=iuvw[0]+(Iu1+2.0*Iu2+2.0*Iu3+Iu4)/6.0; 00433 /*************************************/ 00434 /****Filter Iv********/ 00435 float Iv1,Iv2,Iv3,Iv4;//0.01 00436 Iv1=Idt*(Curr_v-iuvw[1])/Itau; 00437 Iv2=Idt*(Curr_v-(iuvw[1]+Iv1/2.0))/Itau; 00438 Iv3=Idt*(Curr_v-(iuvw[1]+Iv2/2.0))/Itau; 00439 Iv4=Idt*(Curr_v-(iuvw[1]+Iv3))/Itau; 00440 iuvw[1]=iuvw[1]+(Iv1+2.0*Iv2+2.0*Iv3+Iv4)/6.0; 00441 /*************************************/ 00442 /****Filter Iw********/ 00443 float Iw1,Iw2,Iw3,Iw4;//0.01 00444 Iw1=Idt*(Curr_w-iuvw[2])/Itau; 00445 Iw2=Idt*(Curr_w-(iuvw[2]+Iw1/2.0))/Itau; 00446 Iw3=Idt*(Curr_w-(iuvw[2]+Iw2/2.0))/Itau; 00447 Iw4=Idt*(Curr_w-(iuvw[2]+Iw3))/Itau; 00448 iuvw[2]=iuvw[2]+(Iw1+2.0*Iw2+2.0*Iw3+Iw4)/6.0; 00449 /*************************************/ 00450 00451 iabi[0]=(iuvw[0]+iuvw[1]*cos23+iuvw[2]*cos43)*zet; 00452 iabi[1]=(iuvw[1]*sin23+iuvw[2]*sin43)*zet; 00453 /****Filter Ia********/ 00454 float Ia1,Ia2,Ia3,Ia4; 00455 float Iatau= 3.0E-6,Iadt=1.0E-6; //2.0E-4 1.0E-6 00456 Ia1=Iadt*(iabi[0]-iab[0])/Iatau; 00457 Ia2=Iadt*(iabi[0]-(iab[0]+Ia1/2.0))/Iatau; 00458 Ia3=Iadt*(iabi[0]-(iab[0]+Ia2/2.0))/Iatau; 00459 Ia4=Iadt*(iabi[0]-(iab[0]+Ia3))/Iatau; 00460 iab[0]=iab[0]+(Ia1+2.0*Ia2+2.0*Ia3+Ia4)/6.0; 00461 /*************************************/ 00462 00463 /****Filter Ib********/ 00464 float Ib1,Ib2,Ib3,Ib4;//0.01 00465 float Ibtau= 3.0E-6,Ibdt=1.0E-6; //2.0E-2 1.0E-6 00466 Ib1=Ibdt*(iabi[1]-iab[1])/Ibtau; 00467 Ib2=Ibdt*(iabi[1]-(iab[1]+Ib1/2.0))/Ibtau; 00468 Ib3=Ibdt*(iabi[1]-(iab[1]+Ib2/2.0))/Ibtau; 00469 Ib4=Ibdt*(iabi[1]-(iab[1]+Ib3))/Ibtau; 00470 iab[1]=iab[1]+(Ib1+2.0*Ib2+2.0*Ib3+Ib4)/6.0; 00471 /*************************************/ 00472 00473 idq[0]=cos(th)*iab[0]+sin(th)*iab[1]; 00474 idq[1]=-sin(th)*iab[0]+cos(th)*iab[1]; 00475 00476 /****Filter Id********/ 00477 /* float Id1,Id2,Id3,Id4; 00478 float Idtau= 2.0E-4,Iddt=1.0E-6; 00479 Id1=Iddt*(idqi[0]-idq[0])/Idtau; 00480 Id2=Iddt*(idqi[0]-(idq[0]+Id1/2.0))/Idtau; 00481 Id3=Iddt*(idqi[0]-(idq[0]+Id2/2.0))/Idtau; 00482 Id4=Iddt*(idqi[0]-(idq[0]+Id3))/Idtau; 00483 idq[0]=idq[0]+(Id1+2.0*Id2+2.0*Id3+Id4)/6.0;*/ 00484 /*************************************/ 00485 00486 /****Filter Iq********/ 00487 /* float Iq1,Iq2,Iq3,Iq4;//0.01 00488 float Iqtau= 2.0E-4,Iqdt=1.0E-6; //2.0E-2 1.0E-6 00489 Iq1=Iqdt*(idqi[1]-idq[1])/Iqtau; 00490 Iq2=Iqdt*(idqi[1]-(idq[1]+Iq1/2.0))/Iqtau; 00491 Iq3=Iqdt*(idqi[1]-(idq[1]+Iq2/2.0))/Iqtau; 00492 Iq4=Iqdt*(idqi[1]-(idq[1]+Iq3))/Iqtau; 00493 idq[1]=idq[1]+(Iq1+2.0*Iq2+2.0*Iq3+Iq4)/6.0;*/ 00494 /*************************************/ 00495 00496 /*****PID Id *****/ 00497 Idin=0.0-idq[0]; // -0.2 00498 float adi,bdi,cdi,workdi[2]; 00499 float kpdi=3.0,kidi=1.5,kddi=0.0; //0.3 0.1 0.0 00500 float dtdi=10.0E-6;//1E-5 00501 adi=Idin; 00502 bdi=workdi[1]+(Idin+workdi[0])/2.0*dtdi; 00503 cdi=(Idin-workdi[0])/dtdi; 00504 workdi[0]=Idin; 00505 workdi[1]=bdi; 00506 Vd=adi*kpdi+bdi*kidi+cdi*kddi; 00507 /**********************************/ 00508 00509 /*****PID Iq *****/ 00510 Iqin=(Vr_adc)-idq[1]; //kaisha 600 ie 500 00511 float aqi,bqi,cqi,workqi[2]; 00512 float kpqi=2.0,kiqi=0.7,kdqi=0.0; // 1.0 0.7 00513 float dtqi=10.0E-6;//1E-2 00514 aqi=Iqin; 00515 bqi=workqi[1]+(Iqin+workqi[0])/2.0*dtqi; 00516 cqi=(Iqin-workqi[0])/dtqi; 00517 workqi[0]=Iqin; 00518 workdi[1]=bqi; 00519 Vq=(aqi*kpqi+bqi*kiqi+cqi*kdqi); 00520 /***************************************/ 00521 usi=ut2-ut1; 00522 vsi=vt2-vt1; 00523 wsi=wt2-wt1; 00524 00525 if(i<10000){ 00526 vst=vsi; 00527 vstf=vst; 00528 } 00529 else { 00530 00531 00532 00533 vst=vstf; 00534 i=10000; 00535 00536 Wz=(2*PI)/(vst*1E-6); //vst 00537 Ed= (Vq)-0.11f*idq[1]-Wz*0.018E-3*idq[0];//0.018E-3 0.11 00538 phm=Ed/(Wz); 00539 dth=(Vd-0.11f*(idq[0])+Wz*0.018E-3*(idq[1]))/(Wz*phm); // Vd 0.018E-3 00540 eth=asin(dth); 00541 00542 /*****PID θ *****/ 00543 //PLL 00544 float as,bs,cs,works[2]; 00545 float kps=5.0,kis=2.0,kds=0.0; 00546 float dts=1.0E-3;//1.0E-6 00547 Xsi=0.0-eth; 00548 as=Xsi; 00549 bs=works[1]+(Xsi+works[0])/2.0*dts; 00550 cs=(Xsi-works[0])/dts; 00551 works[0]=Xsi; 00552 works[1]=bs; 00553 therr=as*kps+bs*kis+cs*kds; 00554 /*******PLL W ***********/ 00555 if(therr>0.2){ //0.01 00556 W=(2*PI)/((vst*1E-6)+(therr/Wz)); //+ 00557 } 00558 if(therr<-0.01){ //0.01 00559 W=(2*PI)/((vst*1E-6)-(therr/Wz)); //- 00560 } 00561 /*****PID ω *****/ 00562 Xin=5000*(1.15-Vr_adc)-vst; // 6500 1.15 00563 float a,b,c,work[2]; 00564 float kp=1.1,ki=0.7,kd=0.0; // 1.2 0.7 0.0 00565 float dt=10.0E-6;//100E-6 00566 a=Xin; 00567 b=work[1]+(Xin+work[0])/2.0*dt; 00568 c=(Xin-work[0])/dt; 00569 work[0]=Xin; 00570 work[1]=b; 00571 Xout=a*kp+b*ki+c*kd; 00572 vstt=Xout; 00573 /********************************/ 00574 if(2000>abs(vst-vstt)){ 00575 vsti=vstt; //vstt 00576 vector=1; 00577 } 00578 else{ 00579 vsti=vst; 00580 vector=0; 00581 } 00582 /****Filter********/ 00583 float dttt=100.0E-6;//100E-6 00584 float k11,k22,k33,k44,tau1=0.01;//0.01 00585 k11=dttt*(vsti-vstf)/tau1; 00586 k22=dttt*(vsti-(vstf+k11/2.0))/tau1; 00587 k33=dttt*(vsti-(vstf+k22/2.0))/tau1; 00588 k44=dttt*(vsti-(vstf+k33/2.0))/tau1; 00589 vstf=vstf+(k11+2.0*k22+2.0*k33+k44)/6.0; 00590 /*************************************/ 00591 00592 }//else kokomade 00593 00594 00595 Vab[0]=cos(th)*Vd-sin(th)*Vqp;//Vqp 00596 Vab[1]=sin(th)*Vd+cos(th)*Vqp; 00597 00598 00599 uvect=(Vab[0]*zet); 00600 vvect=((Vab[0]*cos23+Vab[1]*sin23)*zet); 00601 wvect=((Vab[0]*cos43+Vab[1]*sin43)*zet); 00602 00603 00604 // SWAVE=Vd; 00605 // SWAVE=th/4; 00606 // SWAVE=Vq; 00607 // SWAVE=West/5000; 00608 //SWAVE=Wo/5000; 00609 //SWAVE=Wz/5000; 00610 // SWAVE=sin(svv); 00611 // SWAVE=iab[0]+0.5; 00612 //SWAVE=Ed; 00613 // SWAVE=therr+0.5; 00614 //SWAVE=idq[1]; 00615 // SWAVE=Curr_v; 00616 // SWAVE=iuvw[1]; 00617 // SWAVE=vst/5000; 00618 //SWAVE=Vab[1]; 00619 SWAVE=iuvw[0]; 00620 //SWAVE=vvect; 00621 //SWAVE=BEMF1; 00622 Vector=vector; 00623 i++; 00624 } 00625 }
Generated on Thu Jul 21 2022 05:23:26 by
1.7.2