root/applications/pmsm/simulator_zdenek/ekf_example/simulmpf.cpp @ 1400

Revision 1381, 8.3 kB (checked in by smidl, 13 years ago)

simulator

  • Property svn:eol-style set to native
Line 
1/* Hlavni simulacni program
2
3   Simulace tramvaje s PMSM v plovouci radove carce
4
5   Presne parametry tramvaje Skoda 15T
6
7   31.5.2007
8
9REV.   
1031.5. 2007      Doplnena moznost provozu PWM se 3. harmonickou - konstanta MODULACE nastavuje typ modulace
1125.6. 2007      Vyrazen ss obvod a trolej (uvazovano konst. napeti na troleji)       
12*/
13
14#include <stdio.h>
15#include <math.h>
16#include "pmsm_mod.h"
17#include "pwm.h"
18#include "regulace.h"
19#include "mpf_double.h"
20
21#define Qm      13
22#define CHYBA_POLOHY    (0./180.*M_PI)  // chyba +/-8 stupnu elektrickych pro senzorovany loziskovy stit
23
24#define MODULACE        0       // 0...sinusova PWM, 1...PWM se 3.harmonickou
25#define REZIM_REGULACE  1       // 0...reg. momentu, 1...reg.rychlosti, 2... Isqw=sqrt(Imax^2-Id^2) - max. moment
26
27static double param[7]={0.28,0.003465,0.1989,0.0,4,1.5,0.04};
28// [Rs, Ls, Fmag, Bf, p, kp, J = 0.04?];
29static double REL[6], REL1[6]={1.,1.,1.,1.,1.,1.};
30// REL = [Ur, Ir, wr, thetar, Mr, Fr];
31static double DT=3e-6;                  // mrtve casy
32static 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
33static unsigned int pocet=8;            // velikost VA-charky
34
35// trolejovy filtr
36//static double Rf=22e-3,Lf=1.8e-3,Cf=10e-3;    // filtr VEKTRA
37static double Rf=5e-3,Lf=1e-3,Cf=1e-3;          // filtr 15T
38// parametry ss obvodu
39static double Ut,Uc,Ucn,It,Iz;
40// modulace a skalarni rizeni
41double fc,fr,fs;
42
43double t, t_end, t_sense;
44double h,hx,h_disp=0;
45double h_reg,h_model,hpom,hpom_model;
46unsigned int h_model_counter,h_model_counter_mez;
47
48static double u[2]={0.,0.};             // format u={Um, beta}
49static double us[2]={0.,0.};    // format us={us_alfa, us_beta}
50double Isd, Isq, Fr, Fs;
51
52static int reg_counter=1, print_counter=0;
53static double Idwf,Iqwf,Urm_max,Ww;
54
55static double rychlost;
56
57static double Isx, Isy;
58static double Usx, Usy, ua, ub, Umk;
59static double theta;
60static double Uc_mer;
61
62static int k_rampa=1, k_rampa_tmp=0;
63
64static double tmp_uhel=0;
65
66// EKF - vysledek estimace
67static double mpf_estim[2]={0.,0.};     // w_est, theta_est
68static double Esinth, Ecosth;
69
70FILE *fw;
71
72static void sim_init(void);
73
74static void sim_init(void)
75{
76  // parametry troleje a modulator
77  Ucn=600.;Ut=200.;Uc=Ut;It=0.;Iz=0.;
78  fc=4000.;
79
80  // vzorkovani
81  h_reg=1./2./fc;
82  h_model=h_reg;
83  hpom=h;hpom_model=h;
84
85  // vzorkovani pomoci citacu
86  h_model_counter_mez=(int)(h_model/h);
87  h_model_counter=h_model_counter_mez;
88
89  fs=0.;                                        // zadani primo v Hz
90//  fs=700.1*(param[4]/60.);    // zadani pres mechanicke otacky fs=n*pp/60
91
92  // pocatecni stav motoru
93  x[0]=0.;x[1]=0.;x[3]=0.;x[4]=0.;x[5]=0.;x[6]=0.;x[7]=0.;x[8]=0.;
94  x[2]=2.*M_PI*fs;  // konstantni rychlost
95  x[3]=M_PI/10.;
96
97//  REL[0]=600; REL[1]=12.0*sqrt(2); REL[2]=2*M_PI*200; REL[3]=3.14159;
98//  REL[4]=17.8;REL[5]=1.0;       // parametry pro v�kon PMSM 6kW!!!
99
100  Isx=0.;Isy=0.;theta=x[3];rychlost=x[2];Uc_mer=Uc;
101}
102
103int main(void)
104{
105  h=1e-6;
106
107  t_end=9.;    // profil Isq 5s, rozbeh 12s, rozbeh +/- 20s, reverzace 12s
108  t_sense=0.0; // 1.
109  t=h;
110
111  sim_init();
112
113//  pwm_full_new_init(h,fc,DT,0,1,1);
114  pwm_full_new_init_3h(h,fc,DT,0,-1,-1);
115  init_pmsm(param, REL1);
116  init_regulace(param,h_reg);
117  mpf_init(1e0, 1e-5, 1e0);//VS:
118
119  fw=fopen("data/graf2.txt","w");
120
121  Idwf=0.;
122  Ww=0.;
123  Iqwf=0.;
124
125  while (t<=t_end)
126  {
127    pwm_full_new_3h(us,u,&Iz,Ucn,Uc,x,va_char,pocet,MODULACE);          // mod=0 ... sinusova PWM, mod=1 ... 3. harmonicka
128    pmsm_double(*us,*(us+1),h,5);
129
130    if (h_model_counter>=h_model_counter_mez)           // pocatek ISR
131    {
132      if (reg_counter>-1)                                // reg. smycka jede v kazdem ISR => reg_counter>-1, jinak jako u ASM reg_counter>1
133      {
134        if (t>.05)      // 0.05
135        {
136          // skokova zmena Isqw
137//                Iqwf=30.;
138                  // zmena Isqw po rampa - profil
139/*                Iqwf+=k_rampa*0.0125; // 0.055
140                  if (Iqwf>30.) {Iqwf=30.; if (k_rampa_tmp<8000) k_rampa_tmp++; else {k_rampa=-1;k_rampa_tmp=0;}}
141                  if (Iqwf<-30.) {Iqwf=-30.; if (k_rampa_tmp<8000) k_rampa_tmp++; else {k_rampa=1;k_rampa_tmp=0;}}
142                  if ((t>3.) && (Iqwf>0)) Iqwf=0; /* konec profilu Isqw */
143//                if (Iqwf>0) Iqwf=0;
144
145//          if (Ww<2.*M_PI*275.) Ww+=2.*M_PI*0.0125;
146//                if (Ww>-2.*M_PI*275.) Ww-=2.*M_PI*0.0125;
147//                Ww=2.*M_PI*180;
148                // jednoducha reverzace - cely dej cca 11s
149/*              Ww+=k_rampa*2.*M_PI*0.125/2.;    //1000Hz/s
150                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;}}
151                if (Ww<-2.*M_PI*150.) Ww=-2.*M_PI*150.;   /* */
152/**/
153          // skokova zmena napeti troleje
154//          Ut=500;       // nutno zadat konstantni Isdw pri startu
155        }
156
157
158        if (t>0.2) x[8]=1.2;    // 1A
159        if (t>0.4) x[8]=10.8;   // 9A
160        if (t>0.6) x[8]=25.2;  // 21A
161
162        if (t>0.7) Ww=2.*M_PI*10.;
163        if (t>1.0) x[8]=1.2;    // 1A
164        if (t>1.2) x[8]=10.8;   // 9A
165        if (t>1.4) x[8]=25.2;  // 21A
166
167        if (t>1.6) Ww=2.*M_PI*50.;
168        if (t>1.9) x[8]=1.2;    // 1A
169        if (t>2.1) x[8]=10.8;   // 9A
170        if (t>2.3) x[8]=25.2;  // 21A
171
172        if (t>2.5) Ww=2.*M_PI*100;
173        if (t>2.8) x[8]=1.2;    // 1A
174        if (t>3.0) x[8]=10.8;   // 9A
175        if (t>3.2) x[8]=25.2;  // 21A
176
177        if (t>3.4) Ww=2.*M_PI*150;
178        if (t>3.7) x[8]=1.2;    // 1A
179        if (t>3.9) x[8]=10.8;   // 9A
180        if (t>4.1) x[8]=25.2;  // 21A
181
182        if (t>4.3) Ww=2.*M_PI*0;
183        if (t>4.8) x[8]=-1.2;    // 1A
184        if (t>5.0) x[8]=-10.8;   // 9A
185        if (t>5.2) x[8]=-25.2;  // 21A
186
187        if (t>5.4) Ww=2.*M_PI*(-10.);
188        if (t>5.7) x[8]=-1.2;    // 1A
189        if (t>5.9) x[8]=-10.8;   // 9A
190        if (t>6.1) x[8]=-25.2;  // 21A
191
192        if (t>6.3) Ww=2.*M_PI*(-50.);
193        if (t>6.7) x[8]=-1.2;    // 1A
194        if (t>6.9) x[8]=-10.8;   // 9A
195        if (t>7.1) x[8]=-25.2;  // 21A
196
197        if (t>7.3) Ww=2.*M_PI*(-100.);
198        if (t>7.7) x[8]=-1.2;    // 1A
199        if (t>7.9) x[8]=-10.8;   // 9A
200        if (t>8.1) x[8]=-25.2;  // 21A
201        if (t>8.3) x[8]=10.8;   // 9A
202        if (t>8.5) x[8]=25.2;  // 21A
203
204
205        // letmy start s nenulovym momentem
206//        Iqwf=149.*sqrt(2.);
207
208                // pomal� rozjezd 10s do max. rychlosti 700rpm
209/*              Iqwf=149.*sqrt(2.);     // konst. Isqw = Imax
210                x[2]+=2.*M_PI*0.004;
211                if (x[2]>1615) x[2]=1615;
212                if (x[2]<-1615) x[2]=-1615;     /* osetreni pro test rozjezdu do zapornych otacek */
213
214                // rampa rozjezd 5s do max. rychlosti 700rpm a 10s do -700rpm
215/*              Iqwf=149.*sqrt(2.);     // konst. Isqw = Imax
216                x[2]+=k_rampa*2.*M_PI*0.008;
217                if (x[2]>1615) {x[2]=1615; if (k_rampa_tmp<24000) k_rampa_tmp++; else {k_rampa=-1;k_rampa_tmp=0;}}
218                if (x[2]<-1615) x[2]=-1615;  /**/
219                       
220                // KALMAN
221               Umk=*u*Uc/Ucn;
222      ua=Umk*cos(*(u+1));
223      ub=Umk*cos(*(u+1)-2./3.*M_PI);
224      Usx=ua;                     // usx
225      Usy=(ua+2.*ub)/sqrt(3.);    // usy
226       
227                mpf_bayes(Isx,Isy,Usx,Usy);
228                mpf_mean(&Esinth,&Ecosth,&mpf_estim[0]);
229                mpf_estim[1]=atan(Esinth/Ecosth);
230
231                // vystup z EKF zaveden do regulatoru
232                // rychlost=ekf_estim[0]; theta=ekf_estim[1]; //VS
233
234                Urm_max=1.0;
235
236                // idealni vzorkovani - test vlivu
237//              Isx=x[0];Isy=x[1];rychlost=x[2];theta=x[3];Uc_mer=Uc;
238
239                if (MODULACE==0)        // sinusova PWM
240                  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
241                                                                                                                        // rezim=2 ... Iqw=sqrt(Imax^2-Idw^2)
242                else
243                  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
244                                                                                                                        // rezim=2 ... Iqw=sqrt(Imax^2-Idw^2)
245
246        reg_counter=0;
247        Isx=x[0];Isy=x[1];rychlost=x[2];theta=x[3]+CHYBA_POLOHY;Uc_mer=Uc;       // re�ln� vzorkov�n�
248      }
249      hpom_model=0;
250      h_model_counter=0;
251      reg_counter++;
252    }
253
254    if (t>=t_sense)
255      if (print_counter>125)//sampling period
256      { fprintf(fw,"%f %f %f %f %f %f %f \n", t,x[0],x[1],Usx,Usy,mpf_estim[0],mpf_estim[1]); // t, Isx, Isy, Isd, Isdw, Isq, Isqw, M, Fs, poloha, rychlost, rychlost_w, Urm, Urm_max,
257      // Fs_model, M_model, zatezny uhel (beta), zatezny uhel vypocteny z napeti (zbeta), Uc
258        print_counter=0;
259      }
260
261    if (h_disp>0.2)
262    { printf("%f \n",t);
263      h_disp=0;
264    }
265
266    t+=h;
267    hpom+=h;
268    hpom_model+=h;
269    h_model_counter++;
270    h_disp+=h;
271    print_counter++;
272  }
273
274//  printf("*** \n\n");
275  fclose(fw);
276        return 0;
277 }
Note: See TracBrowser for help on using the browser.