simul.cpp stdio.h math.h pmsm_mod.h pwm.h regulace.h ekf.h pwm.h work/git/mixpp/pmsm/simulator_zdenek/ekf_example/simul.cpp ekf.h math.h regulace.h stdio.h pmsm_mod.h Qm 13 CHYBA_POLOHY (0./180.*M_PI) MODULACE 1 REZIM_REGULACE 1 double double param[7] [7] param {0.28,0.003465,0.1989,0.0,4,1.5,0.04} double double REL[6] [6] REL double double REL1[6] [6] REL1 {1.,1.,1.,1.,1.,1.} double double DT DT 3e-6 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 Rf Rf 5e-3 double double Lf Lf 1e-3 double double Cf Cf 1e-3 double double Ut Ut double double Uc Uc double double Ucn Ucn double double It It double double Iz Iz double double fc fc double double fr fr double double fs fs double double t t double double t_end t_end double double t_sense t_sense double double h h double double hx hx double double h_disp h_disp 0 double double h_reg h_reg double double h_model h_model double double hpom hpom double double hpom_model hpom_model unsigned int unsigned int h_model_counter h_model_counter unsigned int unsigned int h_model_counter_mez h_model_counter_mez double double u[2] [2] u {0.,0.} double double us[2] [2] us {0.,0.} double double Isd Isd double double Isq Isq double double Fr Fr double double Fs Fs int int reg_counter reg_counter 1 int int print_counter print_counter 0 double double Idwf Idwf double double Iqwf Iqwf double double Urm_max Urm_max double double Ww Ww double double rychlost rychlost double double Isx Isx double double Isy Isy double double theta theta double double Uc_mer Uc_mer int int k_rampa k_rampa 1 int int k_rampa_tmp k_rampa_tmp 0 double double tmp_uhel tmp_uhel 0 double double ekf_estim[2] [2] ekf_estim {0.,0.} FILE * FILE* fw fw void static void sim_init (void) sim_init void int int main (void) main void /*Hlavnisimulacniprogram SimulacetramvajesPMSMvplovouciradovecarce PresneparametrytramvajeSkoda15T 31.5.2007 REV. 31.5.2007DoplnenamoznostprovozuPWMse3.harmonickou-konstantaMODULACEnastavujetypmodulace 25.6.2007Vyrazenssobvodatrolej(uvazovanokonst.napetinatroleji) */ #include<stdio.h> #include<math.h> #include"pmsm_mod.h" #include"pwm.h" #include"regulace.h" #include"ekf.h" #defineQm13 #defineCHYBA_POLOHY(0./180.*M_PI)//chyba+/-8stupnuelektrickychprosenzorovanyloziskovystit #defineMODULACE1//0...sinusovaPWM,1...PWMse3.harmonickou #defineREZIM_REGULACE1//0...reg.momentu,1...reg.rychlosti,2...Isqw=sqrt(Imax^2-Id^2)-max.moment staticdoubleparam[7]={0.28,0.003465,0.1989,0.0,4,1.5,0.04}; //[Rs,Ls,Fmag,Bf,p,kp,J=0.04?]; staticdoubleREL[6],REL1[6]={1.,1.,1.,1.,1.,1.}; //REL=[Ur,Ir,wr,thetar,Mr,Fr]; staticdoubleDT=3e-6;//mrtvecasy 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 //trolejovyfiltr //staticdoubleRf=22e-3,Lf=1.8e-3,Cf=10e-3;//filtrVEKTRA staticdoubleRf=5e-3,Lf=1e-3,Cf=1e-3;//filtr15T //parametryssobvodu staticdoubleUt,Uc,Ucn,It,Iz; //modulaceaskalarnirizeni doublefc,fr,fs; doublet,t_end,t_sense; doubleh,hx,h_disp=0; doubleh_reg,h_model,hpom,hpom_model; unsignedinth_model_counter,h_model_counter_mez; staticdoubleu[2]={0.,0.};//formatu={Um,beta} staticdoubleus[2]={0.,0.};//formatus={us_alfa,us_beta} doubleIsd,Isq,Fr,Fs; staticintreg_counter=1,print_counter=0; staticdoubleIdwf,Iqwf,Urm_max,Ww; staticdoublerychlost; staticdoubleIsx,Isy; staticdoubletheta; staticdoubleUc_mer; staticintk_rampa=1,k_rampa_tmp=0; staticdoubletmp_uhel=0; //EKF-vysledekestimace staticdoubleekf_estim[2]={0.,0.};//w_est,theta_est FILE*fw; staticvoidsim_init(void); staticvoidsim_init(void) { //parametrytrolejeamodulator Ucn=600.;Ut=200.;Uc=Ut;It=0.;Iz=0.; fc=4000.; //vzorkovani h_reg=1./2./fc; h_model=h_reg; hpom=h;hpom_model=h; //vzorkovanipomocicitacu h_model_counter_mez=(int)(h_model/h); h_model_counter=h_model_counter_mez; fs=0.;//zadaniprimovHz //fs=700.1*(param[4]/60.);//zadanipresmechanickeotackyfs=n*pp/60 //pocatecnistavmotoru x[0]=0.;x[1]=0.;x[3]=0.;x[4]=0.;x[5]=0.;x[6]=0.;x[7]=0.;x[8]=0.; x[2]=2.*M_PI*fs;//konstantnirychlost x[3]=M_PI/10.; //REL[0]=600;REL[1]=12.0*sqrt(2);REL[2]=2*M_PI*200;REL[3]=3.14159; //REL[4]=17.8;REL[5]=1.0;//parametryprov�konPMSM6kW!!! Isx=0.;Isy=0.;theta=x[3];rychlost=x[2];Uc_mer=Uc; } intmain(void) { h=1e-6; t_end=9.;//profilIsq5s,rozbeh12s,rozbeh+/-20s,reverzace12s t_sense=0.0;//1. t=h; sim_init(); //pwm_full_new_init(h,fc,DT,0,1,1); pwm_full_new_init_3h(h,fc,DT,0,-1,-1); init_pmsm(param,REL1); init_regulace(param,h_reg); init_ekf(h_reg,param); fw=fopen("data/graf1.txt","w"); Idwf=0.; Ww=0.; Iqwf=0.; while(t<=t_end) { pwm_full_new_3h(us,u,&Iz,Ucn,Uc,x,va_char,pocet,MODULACE);//mod=0...sinusovaPWM,mod=1...3.harmonicka pmsm_double(*us,*(us+1),h,5); if(h_model_counter>=h_model_counter_mez)//pocatekISR { if(reg_counter>-1)//reg.smyckajedevkazdemISR=>reg_counter>-1,jinakjakouASMreg_counter>1 { if(t>.05)//0.05 { //skokovazmenaIsqw //Iqwf=30.; //zmenaIsqwporampa-profil /*Iqwf+=k_rampa*0.0125;//0.055 if(Iqwf>30.){Iqwf=30.;if(k_rampa_tmp<8000)k_rampa_tmp++;else{k_rampa=-1;k_rampa_tmp=0;}} if(Iqwf<-30.){Iqwf=-30.;if(k_rampa_tmp<8000)k_rampa_tmp++;else{k_rampa=1;k_rampa_tmp=0;}} if((t>3.)&&(Iqwf>0))Iqwf=0;/*konecprofiluIsqw*/ //if(Iqwf>0)Iqwf=0; //if(Ww<2.*M_PI*275.)Ww+=2.*M_PI*0.0125; //if(Ww>-2.*M_PI*275.)Ww-=2.*M_PI*0.0125; //Ww=2.*M_PI*180; //jednoduchareverzace-celydejcca11s /*Ww+=k_rampa*2.*M_PI*0.125/2.;//1000Hz/s if(Ww>2.*M_PI*150.){Ww=2.*M_PI*150.;if(k_rampa_tmp<8000)k_rampa_tmp++;else{k_rampa=-1;k_rampa_tmp=0;}} if(Ww<-2.*M_PI*150.)Ww=-2.*M_PI*150.;/**/ //skokovazmenanapetitroleje //Ut=500;//nutnozadatkonstantniIsdwpristartu } if(t>0.2)x[8]=1.2;//1A if(t>0.4)x[8]=10.8;//9A if(t>0.6)x[8]=25.2;//21A if(t>0.7)Ww=2.*M_PI*10.; if(t>1.0)x[8]=1.2;//1A if(t>1.2)x[8]=10.8;//9A if(t>1.4)x[8]=25.2;//21A if(t>1.6)Ww=2.*M_PI*50.; if(t>1.9)x[8]=1.2;//1A if(t>2.1)x[8]=10.8;//9A if(t>2.3)x[8]=25.2;//21A if(t>2.5)Ww=2.*M_PI*100; if(t>2.8)x[8]=1.2;//1A if(t>3.0)x[8]=10.8;//9A if(t>3.2)x[8]=25.2;//21A if(t>3.4)Ww=2.*M_PI*150; if(t>3.7)x[8]=1.2;//1A if(t>3.9)x[8]=10.8;//9A if(t>4.1)x[8]=25.2;//21A if(t>4.3)Ww=2.*M_PI*0; if(t>4.8)x[8]=-1.2;//1A if(t>5.0)x[8]=-10.8;//9A if(t>5.2)x[8]=-25.2;//21A if(t>5.4)Ww=2.*M_PI*(-10.); if(t>5.7)x[8]=-1.2;//1A if(t>5.9)x[8]=-10.8;//9A if(t>6.1)x[8]=-25.2;//21A if(t>6.3)Ww=2.*M_PI*(-50.); if(t>6.7)x[8]=-1.2;//1A if(t>6.9)x[8]=-10.8;//9A if(t>7.1)x[8]=-25.2;//21A if(t>7.3)Ww=2.*M_PI*(-100.); if(t>7.7)x[8]=-1.2;//1A if(t>7.9)x[8]=-10.8;//9A if(t>8.1)x[8]=-25.2;//21A if(t>8.3)x[8]=10.8;//9A if(t>8.5)x[8]=25.2;//21A //letmystartsnenulovymmomentem //Iqwf=149.*sqrt(2.); //pomal�rozjezd10sdomax.rychlosti700rpm /*Iqwf=149.*sqrt(2.);//konst.Isqw=Imax x[2]+=2.*M_PI*0.004; if(x[2]>1615)x[2]=1615; if(x[2]<-1615)x[2]=-1615;/*osetreniprotestrozjezdudozapornychotacek*/ //ramparozjezd5sdomax.rychlosti700rpma10sdo-700rpm /*Iqwf=149.*sqrt(2.);//konst.Isqw=Imax x[2]+=k_rampa*2.*M_PI*0.008; if(x[2]>1615){x[2]=1615;if(k_rampa_tmp<24000)k_rampa_tmp++;else{k_rampa=-1;k_rampa_tmp=0;}} if(x[2]<-1615)x[2]=-1615;/**/ //KALMAN ekf(ekf_estim,*u,*(u+1),Ucn,Uc_mer,Isx,Isy); //vystupzEKFzavedendoregulatoru //rychlost=ekf_estim[0];theta=ekf_estim[1];//VS Urm_max=1.0; //idealnivzorkovani-testvlivu //Isx=x[0];Isy=x[1];rychlost=x[2];theta=x[3];Uc_mer=Uc; if(MODULACE==0)//sinusovaPWM vektor_regulace(Idwf,Iqwf,Urm_max,Ww,u,Isx,Isy,theta,rychlost,Ucn/2.,Uc_mer,Ucn,REZIM_REGULACE);//rezim=1...reg.rychlosti,rezim=0...reg.momentu //rezim=2...Iqw=sqrt(Imax^2-Idw^2) else vektor_regulace(Idwf,Iqwf,Urm_max,Ww,u,Isx,Isy,theta,rychlost,Ucn/sqrt(3.),Uc_mer,Ucn,REZIM_REGULACE);//rezim=1...reg.rychlosti,rezim=0...reg.momentu //rezim=2...Iqw=sqrt(Imax^2-Idw^2) reg_counter=0; Isx=x[0];Isy=x[1];rychlost=x[2];theta=x[3]+CHYBA_POLOHY;Uc_mer=Uc;//re�ln�vzorkov�n� } hpom_model=0; h_model_counter=0; reg_counter++; } if(t>=t_sense) if(print_counter>199) {fprintf(fw,"%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f\n",t,x[0],x[1],x[6],*ladeni_regulace,x[7],*(ladeni_regulace+2),x[4],x[5],x[3],x[2],Ww,*(ladeni_regulace+1),Urm_max,*(ladeni_regulace+4),*(ladeni_regulace+5),x[10],*(ladeni_regulace+6),Uc,*(ladeni_regulace+7),ladeni_pila,ladeni_ur,*(ladeni_regulace+8),*(ladeni_regulace+9),ekf_estim[0],ekf_estim[1]); //t,Isx,Isy,Isd,Isdw,Isq,Isqw,M,Fs,poloha,rychlost,rychlost_w,Urm,Urm_max, //Fs_model,M_model,zateznyuhel(beta),zateznyuhelvypoctenyznapeti(zbeta),Uc print_counter=0; } if(h_disp>0.2) {printf("%f\n",t); h_disp=0; } t+=h; hpom+=h; hpom_model+=h; h_model_counter++; h_disp+=h; print_counter++; } //printf("***\n\n"); fclose(fw); return0; }