simulator.cpp math.h stdlib.h regulace.h simulator.h regulace.h stdlib.h simulator.h math.h work/git/mixpp/pmsm/simulator_zdenek/simulator.cpp _USE_MATH_DEFINES REZIM_REGULACE 1 double double Rs Rs double double Ls Ls double double Fmag Fmag double double Bf Bf double double p p double double kp kp double double J J double double Ucn Ucn double double Uc Uc double double DT DT double double U_modulace U_modulace double double Urm_max Urm_max double double h h double double h_reg h_reg double double h_reg_real h_reg_real unsigned int unsigned int h_reg_counter h_reg_counter unsigned int unsigned int h_reg_counter_mez h_reg_counter_mez double double va_char[16] [16] va_char {0,10,50,100,200,300,500,1000, 0,1,1.8,2.4,3.2,3.8,4.8,6.8} unsigned int unsigned int pocet pocet 8 double double x[9] [9] x int int smer smer int int smer2 smer2 int int citac citac int int citac2 citac2 int int citac_PR citac_PR int int modulace modulace double double dIsx dIsx double double dIsx1 dIsx1 double double dIsx2 dIsx2 double double dIsx3 dIsx3 double double dIsy dIsy double double dIsy1 dIsy1 double double dIsy2 dIsy2 double double dIsy3 dIsy3 double double dTheta dTheta double double dTheta1 dTheta1 double double dTheta2 dTheta2 double double dTheta3 dTheta3 double double dw dw double double dw1 dw1 double double dw2 dw2 double double dw3 dw3 double double Isx Isx double double Isy Isy double double theta theta double double speed speed double double u[2] [2] u {0.,0.} double double us[2] [2] us {0.,0.} double double usx_av usx_av 0. double double usy_av usy_av 0. double double sum_usx_av sum_usx_av 0. double double sum_usy_av sum_usy_av 0. double double isx_av isx_av 0. double double isy_av isy_av 0. double double sum_isx_av sum_isx_av 0. double double sum_isy_av sum_isy_av 0. double double usxf usxf 0. double double usyf usyf 0. double double Tf Tf 0.01 unsigned int unsigned int start_filter start_filter 1 double double KalmanObs[10] [10] KalmanObs {0.,0.,0.,0.,0.,0.,0.,0.,0.,0.} double double t t 0. double double ualfa ualfa 0. double double ubeta ubeta 0. void void pmsmsim_set_parameters (double Rs0, double Ls0, double Fmag0, double Bf0, double p0, double kp0, double J0, double Uc0, double DT0, double dt0) pmsmsim_set_parameters double Rs0 double Ls0 double Fmag0 double Bf0 double p0 double kp0 double J0 double Uc0 double DT0 double dt0 void void pmsmsim_step (double Ww) pmsmsim_step double Ww void static void pwm (unsigned int mod) pwm unsigned int mod double static double ubytek (double I) ubytek double I void static void pmsm_model (unsigned int mod) pmsm_model unsigned int mod /* SimulatorofVectorControlledPMSMDrive ThismoduleisbackgroundforPMSMdriveobjectdesignand introducesbasicfunctions...set_parameters()andeval(). Z.Peroutka Rev.16.3.2008 */ #define_USE_MATH_DEFINES #include<math.h> #include<stdlib.h>//nalinuxujeabsvstdlib #include"regulace.h" #include"simulator.h" #defineREZIM_REGULACE1//0...reg.momentu,1...reg.rychlosti,2...Isqw=sqrt(Imax^2-Id^2)-max.moment voidpmsmsim_set_parameters(doubleRs0,doubleLs0,doubleFmag0,doubleBf0,doublep0,doublekp0,doubleJ0,doubleUc0,doubleDT0,doubledt0); voidpmsmsim_step(doubleWw); //localfunctions staticvoidpwm(unsignedintmod); staticdoubleubytek(doubleI); staticvoidpmsm_model(unsignedintmod); //simulatorproperties/////////////////////// staticdoubleRs,Ls,Fmag,Bf,p,kp,J;//parametersofPMSMmodel staticdoubleUcn,Uc,DT,U_modulace;//dc-linkvoltageanddead-time staticdoubleUrm_max;//fieldweakening staticdoubleh,h_reg,h_reg_real;//simulationstepandsamplingofemployedloops unsignedinth_reg_counter,h_reg_counter_mez;//emulationofDSPoperation staticdoubleva_char[16]={0,10,50,100,200,300,500,1000,0,1,1.8,2.4,3.2,3.8,4.8,6.8};//ubytky staticunsignedintpocet=8;//velikostVA-charky //systemstate doublex[9];//(isx,isy,wme,theta_e,M,Fsd,Isd,Isq,Mz) //internalvariablesofPWMmodule staticintsmer,smer2,citac,citac2,citac_PR,modulace; //internalvariablesofPMSMmodel staticdoubledIsx,dIsx1,dIsx2,dIsx3,dIsy,dIsy1,dIsy2,dIsy3; staticdoubledTheta,dTheta1,dTheta2,dTheta3; staticdoubledw,dw1,dw2,dw3; //systemmeasures staticdoubleIsx,Isy,theta,speed; //control staticdoubleu[2]={0.,0.};//formatu={Um,beta} staticdoubleus[2]={0.,0.};//formatus={us_alfa,us_beta} //variablesforcalculationofmeanvaluesofstatorvoltagecomponents staticdoubleusx_av=0.,usy_av=0.,sum_usx_av=0.,sum_usy_av=0.; //variablesforcalculationofmeanvaluesofstatorcurrentcomponents-(alfa,beta) staticdoubleisx_av=0.,isy_av=0.,sum_isx_av=0.,sum_isy_av=0.; //statorvoltagecomponentsfiltering staticdoubleusxf=0.,usyf=0.,Tf=0.01; staticunsignedintstart_filter=1; //outputforEKF(voltagesandmeasuredcurrents,whicharefedtoKalmanObs) doubleKalmanObs[10]={0.,0.,0.,0.,0.,0.,0.,0.,0.,0.};//usx,usy,Isx,Isy,usx_av,usy_av //real-time doublet=0.;//VSremovedstaticduetoclashwithexportin.h //statorvoltagecomponentsinalfabeta(inludingimpactoftherealdc-linkvoltage) staticdoubleualfa=0.,ubeta=0.; voidpmsmsim_set_parameters(doubleRs0,doubleLs0,doubleFmag0,doubleBf0,doublep0,doublekp0,doubleJ0,doubleUc0,doubleDT0,doubledt0) { inttmp_i; //simulatorparameterssetup Rs=Rs0; Ls=Ls0; Fmag=Fmag0; Bf=Bf0; p=p0; kp=kp0; J=J0; Ucn=600.; Uc=Uc0; DT=DT0; //controlsetup Urm_max=0.95; //simulatorsampling-fixedsetup h=dt0; h_reg=125e-6;//fpwm=4kHz h_reg_counter_mez=(int)(h_reg/h);//emulationofoperationofDSPtimer //h_reg_counter=h_reg_counter_mez; h_reg_counter=1; h_reg_real=h_reg_counter_mez*h;//realsamplingperiod //resetofthesystemstatevariables for(tmp_i=0;tmp_i<9;tmp_i++) x[tmp_i]=0.; //emulationofthefirstmeasure Isx=0.;Isy=0.;theta=x[3];speed=x[2]; //===initofparticularmodulesofsimulator=== //PWMinit smer=-1;smer2=-1; citac=0; citac2=abs(0-(int)(DT/h));//VS:oprava,jetospravne? citac_PR=h_reg_counter_mez; //firstinterruptoccurafterfirstperiodmatch=>add1tobothcounterregisters citac++;smer=1; citac2--; modulace=1;//THIPWM if(modulace==1) U_modulace=Ucn/sqrt(3.); else U_modulace=Ucn/2.; //PMSMmodelinit dIsx=0;dIsx1=0;dIsx2=0;dIsx3=0;dIsy=0;dIsy1=0;dIsy2=0;dIsy3=0; dTheta=0;dTheta1=0;dTheta2=0;dTheta3=0; dw=0;dw1=0;dw2=0;dw3=0; init_regulace(Ls,Fmag,kp,p,h_reg_real); } staticvoidpwm(unsignedintmod) //mod...mod=0-sinusoidalPWM;mod=1-PWMwithinjected3rdharmonic { unsignedinti; doubleiabc[3],ur[3],ustr[3],ua,ub,uc; doubledtr[3],dd[3]; doubleUm,beta; doubleU3; doubleup,up2; Um=*u; beta=*(u+1); //emulationofcarrier-timer up=((double)citac/citac_PR-0.5)*Ucn; up2=((double)citac2/citac_PR-0.5)*Ucn; iabc[0]=*x; iabc[1]=(-*x+sqrt(3.)**(x+1))/2.; iabc[2]=(-*x-sqrt(3.)**(x+1))/2.; if(mod==0)//sin.PWM { ur[0]=Um*cos(beta); ur[1]=Um*cos(beta-2./3.*M_PI); ur[2]=Um*cos(beta+2./3.*M_PI); } else//PWMwithinjected3rdharmonic { U3=0.17*cos(3.*beta); ur[0]=Um*(cos(beta)-U3); ur[1]=Um*(cos(beta-2./3.*M_PI)-U3); ur[2]=Um*(cos(beta+2./3.*M_PI)-U3); } for(i=0;i<3;i++) {dtr[i]=ubytek(fabs(iabc[i]))*0.; dd[i]=dtr[i]*.73; } //implementationofvoltagedropsanddead-times for(i=0;i<3;i++) if(iabc[i]>=0) if((ur[i]>up)&&(ur[i]>up2)) ustr[i]=Uc/2-dtr[i]; else ustr[i]=-(Uc/2+dd[i]); else if((ur[i]<up)&&(ur[i]<up2)) ustr[i]=-(Uc/2-dtr[i]); else ustr[i]=Uc/2+dd[i]; //phasevoltages ua=(2.*ustr[0]-ustr[1]-ustr[2])/3.; ub=(2.*ustr[1]-ustr[0]-ustr[2])/3.; uc=(2.*ustr[2]-ustr[0]-ustr[1])/3.; //voltagevectorinstationaryreferenceframe(x,y) *us=(2.*ua-ub-uc)/3.; *(us+1)=(ub-uc)/sqrt(3.); //emulationofDSPtimers if((citac==citac_PR)||(citac==0)) { smer*=-1; //calculationofstatorvoltagecomponentsmeanvalues usx_av=h/h_reg*sum_usx_av; usy_av=h/h_reg*sum_usy_av; //resetofsumaccumulators sum_usx_av=0.; sum_usy_av=0.; //statorcurrentcomponentsmeanvalues-referenceframe(alfa,beta) isx_av=h/h_reg*sum_isx_av; isy_av=h/h_reg*sum_isy_av; //resetofsumaccumulators sum_isx_av=0.; sum_isy_av=0.; } if((citac2==citac_PR)||(citac2==0))smer2*=-1; citac+=smer; citac2+=smer2; //calculationofstatorvoltagecomponentsmeanvalues-sum sum_usx_av+=*us; sum_usy_av+=*(us+1); //statorvoltagecomponentsfiltering //if(start_filter==1) usxf+=(*us-usxf)*h/h_reg; usyf+=(*(us+1)-usyf)*h/h_reg; //statorcurrentcomponentsmeanvalues-referenceframe(alfa,beta) sum_isx_av+=*x; sum_isy_av+=*(x+1); } staticdoubleubytek(doubleI) { unsignedintii; doubledelta_u; ii=0; while((*(va_char+ii)<I)&&(ii<(pocet-1))) ii++; if(ii==(pocet-1)) delta_u=*(va_char+ii+pocet); else if(ii==0) delta_u=0; else delta_u=*(va_char+ii-1+pocet)+(I-*(va_char+ii-1))/(*(va_char+ii)-*(va_char+ii-1))*(*(va_char+ii+pocet)-*(va_char+ii-1+pocet)); returndelta_u; } staticvoidpmsm_model(unsignedintmod) //mod<5...Euler,mod>4...Adamsof4thorder { doubleusx,usy; usx=*us; usy=*(us+1); dIsx=-Rs/Ls*x[0]+Fmag/Ls*x[2]*sin(x[3])+usx/Ls; dIsy=-Rs/Ls*x[1]-Fmag/Ls*x[2]*cos(x[3])+usy/Ls; dTheta=x[2]; if(J>0) dw=kp*p*p*Fmag/J*(x[1]*cos(x[3])-x[0]*sin(x[3]))-Bf/J*x[2]-p/J*x[8]; else dw=0; //integration if(mod<5)//Euler {x[0]+=dIsx*h; x[1]+=dIsy*h; x[2]+=dw*h; x[3]+=dTheta*h; } else//Adams(4thorder) {x[0]+=h/24.*(55.*dIsx-59.*dIsx1+37.*dIsx2-9.*dIsx3); x[1]+=h/24.*(55.*dIsy-59.*dIsy1+37.*dIsy2-9.*dIsy3); x[2]+=h/24.*(55.*dw-59.*dw1+37.*dw2-9.*dw3); x[3]+=h/24.*(55.*dTheta-59.*dTheta1+37.*dTheta2-9.*dTheta3); } //saturationofthetato(-pi,pi) if(x[3]>M_PI)x[3]-=(2*M_PI); if(x[3]<-M_PI)x[3]+=(2*M_PI); //diff.shift-Adams dIsx3=dIsx2;dIsx2=dIsx1;dIsx1=dIsx; dIsy3=dIsy2;dIsy2=dIsy1;dIsy1=dIsy; dTheta3=dTheta2;dTheta2=dTheta1;dTheta1=dTheta; dw3=dw2;dw2=dw1;dw1=dw; //calculationofIsd,Isq x[6]=x[0]*cos(x[3])+x[1]*sin(x[3]);//Isd x[7]=x[1]*cos(x[3])-x[0]*sin(x[3]);//Isq //Fsd...d-componentofstatorflux x[5]=Ls*x[6]+Fmag; //Torque x[4]=kp*p*Fmag*(x[1]*cos(x[3])-x[0]*sin(x[3])); } voidpmsmsim_step(doubleWw)//youmustlinkarrayKalmanObs[]toEKFmodul { doubleUmk,ub,uc; //while(t<=t_end) { pwm(modulace); //*us=KalmanObs[0];*(us+1)=KalmanObs[1]; //*us=ualfa;*(us+1)=ubeta; pmsm_model(5); if(h_reg_counter>=h_reg_counter_mez)//pocatekISR { //voltagesandmeasuredcurrentsforEKF //Umk=*u*Uc/Ucn; //ualfa=Umk*cos(*(u+1)); //ub=Umk*cos(*(u+1)-2./3.*M_PI); KalmanObs[0]=ualfa;//usx //KalmanObs[1]=(ualfa+2.*ub)/sqrt(3.);//usy KalmanObs[1]=ubeta;//usy //realsampling-consideredtransportdelayequaltothesamplingperiod /*KalmanObs[2]=Isx; KalmanObs[3]=Isy;*/ //idealsampling KalmanObs[2]=x[0]; KalmanObs[3]=x[1]; //diagnostic-meanvaluesofstatorvoltagecomponents-pwm() KalmanObs[4]=usx_av; KalmanObs[5]=usy_av; KalmanObs[6]=usxf; KalmanObs[7]=usyf; KalmanObs[8]=isx_av; KalmanObs[9]=isy_av; vektor_regulace(0,0,Urm_max,Ww,u,Isx,Isy,theta,speed,U_modulace,Uc,Ucn,REZIM_REGULACE);//rezim=1...reg.rychlosti,rezim=0...reg.momentu //rezim=2...Iqw=sqrt(Imax^2-Idw^2) //emulationoftherealsamplingofA/Dconverter Isx=x[0];Isy=x[1];speed=x[2];theta=x[3]; //includeidealcommandedstatorvoltage Umk=*u*Uc/Ucn; ualfa=Umk*cos(*(u+1));//usx=usa ub=Umk*cos(*(u+1)-2./3.*M_PI); ubeta=(ualfa+2.*ub)/sqrt(3.);//usy //uc=-ualfa-ub; //ubeta=(ub-uc)/sqrt(3.); h_reg_counter=0; } t+=h; h_reg_counter++; } }