pmsm_mod.cpp math.h pmsm_mod.h work/git/mixpp/pmsm/simulator_zdenek/ekf_example/pmsm_mod.cpp math.h pmsm_mod.h _USE_MATH_DEFINES double double x[10] [10] x double double Isx Isx double double Isy Isy double double wme wme double double Mz Mz double double M M double double Fs Fs double double Isd Isd double double Isq Isq double double Rs Rs double double Ls Ls double double Fmag Fmag double double Bfric Bfric double double p p double double kp kp double double J J double double Uref Uref double double Iref Iref double double Wref Wref double double Mref Mref double double Fref Fref double double Thetaref Thetaref 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 A A double double B B double double C C double double D D double double E E double double F F double double G G double double Fsq Fsq void void init_pmsm (double *param, double *REL) init_pmsm double * param double * REL void void pmsm_double (double usx, double usy, double h, unsigned int vypocet) pmsm_double double usx double usy double h unsigned int vypocet double double uhel (double x, double y) uhel double x double y /*modelsynchronnihomotoruspermanentnimimagnety PMSMvplovouciradovecarce-fyzikalnimodel! 31.5.2007 UpravenvypocetmomentuM=Fmag*Isq!!(nikolivM=Fs*Isq!!) 31.5.2007doplnenvypocetzateznehouhlu(beta) */ #define_USE_MATH_DEFINES #include<math.h> #include"pmsm_mod.h" doublex[10];//(isx,isy,wme,theta_e,M,Fsd,Isd,Isq,Mz,beta)...beta=zateznyuhel voidinit_pmsm(double*param,double*REL); voidpmsm_double(doubleusx,doubleusy,doubleh,unsignedintvypocet); //pomocnefunkce staticdoubleuhel(doublex,doubley); staticdoubleIsx,Isy,wme,Mz,M,Fs,Isd,Isq; staticdoubleRs,Ls,Fmag,Bfric,p,kp,J; //Rs=odporstator;Ls=statorovaindukcnost(synchronnireaktance) //p=pocetpolparu,kp=konst.Parkovytransf., //J=momentsetrvacnosti(J<0...nekonecny) staticdoubleUref,Iref,Wref,Mref,Fref,Thetaref;//referencnihodnoty staticdoubledIsx,dIsx1,dIsx2,dIsx3,dIsy,dIsy1,dIsy2,dIsy3; staticdoubledTheta,dTheta1,dTheta2,dTheta3; staticdoubledw,dw1,dw2,dw3; staticdoubleA,B,C,D,E,F,G; staticdoubleFsq; doubleuhel(doublex,doubley) { doubleth; if(x==0) if(y==0)th=0.; elseif(y>0)th=M_PI/2.; elseth=-M_PI/2.; else th=atan(y/x); if(x<0)th+=M_PI; returnth; } voidinit_pmsm(double*param,double*REL) { unsignedinti; Rs=*(param); Ls=*(param+1); Fmag=*(param+2); Bfric=*(param+3); p=*(param+4); kp=*(param+5); J=*(param+6); Uref=*REL; Iref=*(REL+1); Wref=*(REL+2); Thetaref=*(REL+3); Mref=*(REL+4); Fref=*(REL+5); //definicekonstantmodelu A=Rs/Ls; B=Fmag/Ls*Wref/Iref; C=Uref/Iref/Ls; D=Bfric/J; E=kp*p*p*Fmag/J*Iref/Wref; F=p/J*Mref/Wref; G=Wref/Thetaref; //definicepocatecnichhodnot 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; } voidpmsm_double(doubleusx,doubleusy,doubleh,unsignedintvypocet) //vypocet<5...Euler,vypocet>4...Adams4.radu { //(isx,isy,wme,theta_e,M,Fs,Mz) //vypoctydiferenci dIsx=-A*x[0]+B*x[2]*sin(x[3]*Thetaref)+C*usx; dIsy=-A*x[1]-B*x[2]*cos(x[3]*Thetaref)+C*usy; dTheta=G*x[2]; if(J>0) dw=E*(x[1]*cos(x[3]*Thetaref)-x[0]*sin(x[3]*Thetaref))-D*x[2]-F*x[8]; else dw=0; //integrace if(vypocet<5)//Euler {x[0]+=dIsx*h; x[1]+=dIsy*h; x[2]+=dw*h; x[3]+=dTheta*h; } else//Adams4.radu {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); } //upravathetatak,abybylostalevrozmezi(-pi,pi) if(x[3]>M_PI/Thetaref)x[3]-=(2*M_PI/Thetaref); if(x[3]<-M_PI/Thetaref)x[3]+=(2*M_PI/Thetaref); //zamenadiferenci dIsx3=dIsx2;dIsx2=dIsx1;dIsx1=dIsx; dIsy3=dIsy2;dIsy2=dIsy1;dIsy1=dIsy; dTheta3=dTheta2;dTheta2=dTheta1;dTheta1=dTheta; dw3=dw2;dw2=dw1;dw1=dw; //vypocetIsd,Isq x[6]=x[0]*cos(x[3]*Thetaref)+x[1]*sin(x[3]*Thetaref);//Isd x[7]=x[1]*cos(x[3]*Thetaref)-x[0]*sin(x[3]*Thetaref);//Isq //vypocettokustatoruFsd-neninormovany!! x[5]=Ls*x[6]*Iref+Fmag; //Vypocetmomentu x[4]=kp*p*Fmag*Iref/Mref*(x[1]*cos(x[3]*Thetaref)-x[0]*sin(x[3]*Thetaref)); //vypocetzateznehouhlu Fsq=Ls*x[7]*Iref; x[10]=uhel(x[5],Fsq); }