/* Hlavni simulacni program Simulace tramvaje s PMSM v plovouci radove carce Presne parametry tramvaje Skoda 15T 31.5.2007 REV. 31.5. 2007 Doplnena moznost provozu PWM se 3. harmonickou - konstanta MODULACE nastavuje typ modulace 25.6. 2007 Vyrazen ss obvod a trolej (uvazovano konst. napeti na troleji) */ #include #include #include "pmsm_mod.h" #include "pwm.h" #include "regulace.h" #include "ekf.h" #define Qm 13 #define CHYBA_POLOHY (0./180.*M_PI) // chyba +/-8 stupnu elektrickych pro senzorovany loziskovy stit #define MODULACE 0 // 0...sinusova PWM, 1...PWM se 3.harmonickou #define REZIM_REGULACE 1 // 0...reg. momentu, 1...reg.rychlosti, 2... Isqw=sqrt(Imax^2-Id^2) - max. moment static double param[7]={0.28,0.003465,0.1989,0.0,4,1.5,0.04}; // [Rs, Ls, Fmag, Bf, p, kp, J = 0.04?]; static double REL[6], REL1[6]={1.,1.,1.,1.,1.,1.}; // REL = [Ur, Ir, wr, thetar, Mr, Fr]; static double DT=3e-6; // mrtve casy static double va_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 static unsigned int pocet=8; // velikost VA-charky // trolejovy filtr //static double Rf=22e-3,Lf=1.8e-3,Cf=10e-3; // filtr VEKTRA static double Rf=5e-3,Lf=1e-3,Cf=1e-3; // filtr 15T // parametry ss obvodu static double Ut,Uc,Ucn,It,Iz; // modulace a skalarni rizeni double fc,fr,fs; double t, t_end, t_sense; double h,hx,h_disp=0; double h_reg,h_model,hpom,hpom_model; unsigned int h_model_counter,h_model_counter_mez; static double u[2]={0.,0.}; // format u={Um, beta} static double us[2]={0.,0.}; // format us={us_alfa, us_beta} double Isd, Isq, Fr, Fs; static int reg_counter=1, print_counter=0; static double Idwf,Iqwf,Urm_max,Ww; static double rychlost; static double Isx, Isy; static double theta; static double Uc_mer; static int k_rampa=1, k_rampa_tmp=0; static double tmp_uhel=0; // EKF - vysledek estimace static double ekf_estim[2]={0.,0.}; // w_est, theta_est FILE *fw; static void sim_init(void); static void sim_init(void) { // parametry troleje a modulator 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; // vzorkovani pomoci citacu h_model_counter_mez=(int)(h_model/h); h_model_counter=h_model_counter_mez; fs=0.; // zadani primo v Hz // fs=700.1*(param[4]/60.); // zadani pres mechanicke otacky fs=n*pp/60 // pocatecni stav motoru 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; // konstantni rychlost 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; // parametry pro v�kon PMSM 6kW!!! Isx=0.;Isy=0.;theta=x[3];rychlost=x[2];Uc_mer=Uc; } int main(void) { h=1e-6; t_end=9.; // profil Isq 5s, rozbeh 12s, rozbeh +/- 20s, reverzace 12s 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 ... sinusova PWM, mod=1 ... 3. harmonicka pmsm_double(*us,*(us+1),h,5); if (h_model_counter>=h_model_counter_mez) // pocatek ISR { if (reg_counter>-1) // reg. smycka jede v kazdem ISR => reg_counter>-1, jinak jako u ASM reg_counter>1 { if (t>.05) // 0.05 { // skokova zmena Isqw // Iqwf=30.; // zmena Isqw po rampa - 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; /* konec profilu Isqw */ // 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; // jednoducha reverzace - cely dej cca 11s /* 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.; /* */ /**/ // skokova zmena napeti troleje // Ut=500; // nutno zadat konstantni Isdw pri startu } 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 // letmy start s nenulovym momentem // Iqwf=149.*sqrt(2.); // pomal� rozjezd 10s do max. rychlosti 700rpm /* 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; /* osetreni pro test rozjezdu do zapornych otacek */ // rampa rozjezd 5s do max. rychlosti 700rpm a 10s do -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); // vystup z EKF zaveden do regulatoru // rychlost=ekf_estim[0]; theta=ekf_estim[1]; //VS Urm_max=1.0; // idealni vzorkovani - test vlivu // Isx=x[0];Isy=x[1];rychlost=x[2];theta=x[3];Uc_mer=Uc; if (MODULACE==0) // sinusova PWM 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>125) { 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, zatezny uhel (beta), zatezny uhel vypocteny z napeti (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); return 0; }