simulator.cpp math.h regulace.h simulator.h regulace.h simulator.h math.h work/git/mixpp/pmsm/simulator_zdenek/demo_example/simulator.cpp 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 KalmanObs[4] [4] KalmanObs {0.,0.,0.,0.} double double t t 0. void void set_parameters (double Rs0, double Ls0, double Fmag0, double Bf0, double p0, double kp0, double J0, double Uc0, double DT0, double dt0) set_parameters double Rs0 double Ls0 double Fmag0 double Bf0 double p0 double kp0 double J0 double Uc0 double DT0 double dt0 void void eval (double Ww) eval 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 */ #include<math.h> #include"regulace.h" #include"simulator.h" #defineREZIM_REGULACE1//0...reg.momentu,1...reg.rychlosti,2...Isqw=sqrt(Imax^2-Id^2)-max.moment voidset_parameters(doubleRs0,doubleLs0,doubleFmag0,doubleBf0,doublep0,doublekp0,doubleJ0,doubleUc0,doubleDT0,doubledt0); voideval(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} //outputforEKF(voltagesandmeasuredcurrents,whicharefedtoKalmanObs) staticdoubleKalmanObs[4]={0.,0.,0.,0.};//usx,usy,Isx,Isy //real-time staticdoublet=0.; voidset_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_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-(DT/h)); citac_PR=h_reg_counter_mez; 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])); 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; if((citac2==citac_PR)||(citac2==0))smer2*=-1; citac+=smer; citac2+=smer2; } 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])); } voideval(doubleWw)//youmustlinkarrayKalmanObs[]toEKFmodul { doubleUmk,ua,ub; //while(t<=t_end) { pwm(modulace); pmsm_model(5); if(h_reg_counter>=h_reg_counter_mez)//pocatekISR { //voltagesandmeasuredcurrentsforEKF Umk=*u*Uc/Ucn; ua=Umk*cos(*(u+1)); ub=Umk*cos(*(u+1)-2./3.*M_PI); KalmanObs[0]=ua;//usx KalmanObs[1]=(ua+2.*ub)/sqrt(3.);//usy KalmanObs[2]=Isx; KalmanObs[3]=Isy; 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]; h_reg_counter=0; } t+=h; h_reg_counter++; } }