root/applications/pmsm/simulator_zdenek/ekf_example/simul.cpp @ 575

Revision 326, 8.3 kB (checked in by smidl, 17 years ago)

pmsm + UI for EKF

  • 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 "ekf.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 theta;
59static double Uc_mer;
60
61static int k_rampa=1, k_rampa_tmp=0;
62
63static double tmp_uhel=0;
64
65// EKF - vysledek estimace
66static double ekf_estim[2]={0.,0.};     // w_est, theta_est
67
68FILE *fw;
69
70static void sim_init(void);
71
72static void sim_init(void)
73{
74  // parametry troleje a modulator
75  Ucn=600.;Ut=200.;Uc=Ut;It=0.;Iz=0.;
76  fc=4000.;
77
78  // vzorkovani
79  h_reg=1./2./fc;
80  h_model=h_reg;
81  hpom=h;hpom_model=h;
82
83  // vzorkovani pomoci citacu
84  h_model_counter_mez=(int)(h_model/h);
85  h_model_counter=h_model_counter_mez;
86
87  fs=0.;                                        // zadani primo v Hz
88//  fs=700.1*(param[4]/60.);    // zadani pres mechanicke otacky fs=n*pp/60
89
90  // pocatecni stav motoru
91  x[0]=0.;x[1]=0.;x[3]=0.;x[4]=0.;x[5]=0.;x[6]=0.;x[7]=0.;x[8]=0.;
92  x[2]=2.*M_PI*fs;  // konstantni rychlost
93  x[3]=M_PI/10.;
94
95//  REL[0]=600; REL[1]=12.0*sqrt(2); REL[2]=2*M_PI*200; REL[3]=3.14159;
96//  REL[4]=17.8;REL[5]=1.0;       // parametry pro v�kon PMSM 6kW!!!
97
98  Isx=0.;Isy=0.;theta=x[3];rychlost=x[2];Uc_mer=Uc;
99}
100
101int main(void)
102{
103  h=1e-6;
104
105  t_end=9.;    // profil Isq 5s, rozbeh 12s, rozbeh +/- 20s, reverzace 12s
106  t_sense=0.0; // 1.
107  t=h;
108
109  sim_init();
110
111//  pwm_full_new_init(h,fc,DT,0,1,1);
112  pwm_full_new_init_3h(h,fc,DT,0,-1,-1);
113  init_pmsm(param, REL1);
114  init_regulace(param,h_reg);
115  init_ekf(h_reg,param);
116
117  fw=fopen("data/graf1.txt","w");
118
119  Idwf=0.;
120  Ww=0.;
121  Iqwf=0.;
122
123  while (t<=t_end)
124  {
125    pwm_full_new_3h(us,u,&Iz,Ucn,Uc,x,va_char,pocet,MODULACE);          // mod=0 ... sinusova PWM, mod=1 ... 3. harmonicka
126    pmsm_double(*us,*(us+1),h,5);
127
128    if (h_model_counter>=h_model_counter_mez)           // pocatek ISR
129    {
130      if (reg_counter>-1)                                // reg. smycka jede v kazdem ISR => reg_counter>-1, jinak jako u ASM reg_counter>1
131      {
132        if (t>.05)      // 0.05
133        {
134          // skokova zmena Isqw
135//                Iqwf=30.;
136                  // zmena Isqw po rampa - profil
137/*                Iqwf+=k_rampa*0.0125; // 0.055
138                  if (Iqwf>30.) {Iqwf=30.; if (k_rampa_tmp<8000) k_rampa_tmp++; else {k_rampa=-1;k_rampa_tmp=0;}}
139                  if (Iqwf<-30.) {Iqwf=-30.; if (k_rampa_tmp<8000) k_rampa_tmp++; else {k_rampa=1;k_rampa_tmp=0;}}
140                  if ((t>3.) && (Iqwf>0)) Iqwf=0; /* konec profilu Isqw */
141//                if (Iqwf>0) Iqwf=0;
142
143//          if (Ww<2.*M_PI*275.) Ww+=2.*M_PI*0.0125;
144//                if (Ww>-2.*M_PI*275.) Ww-=2.*M_PI*0.0125;
145//                Ww=2.*M_PI*180;
146                // jednoducha reverzace - cely dej cca 11s
147/*              Ww+=k_rampa*2.*M_PI*0.125/2.;    //1000Hz/s
148                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;}}
149                if (Ww<-2.*M_PI*150.) Ww=-2.*M_PI*150.;   /* */
150/**/
151          // skokova zmena napeti troleje
152//          Ut=500;       // nutno zadat konstantni Isdw pri startu
153        }
154
155
156        if (t>0.2) x[8]=1.2;    // 1A
157        if (t>0.4) x[8]=10.8;   // 9A
158        if (t>0.6) x[8]=25.2;  // 21A
159
160        if (t>0.7) Ww=2.*M_PI*10.;
161        if (t>1.0) x[8]=1.2;    // 1A
162        if (t>1.2) x[8]=10.8;   // 9A
163        if (t>1.4) x[8]=25.2;  // 21A
164
165        if (t>1.6) Ww=2.*M_PI*50.;
166        if (t>1.9) x[8]=1.2;    // 1A
167        if (t>2.1) x[8]=10.8;   // 9A
168        if (t>2.3) x[8]=25.2;  // 21A
169
170        if (t>2.5) Ww=2.*M_PI*100;
171        if (t>2.8) x[8]=1.2;    // 1A
172        if (t>3.0) x[8]=10.8;   // 9A
173        if (t>3.2) x[8]=25.2;  // 21A
174
175        if (t>3.4) Ww=2.*M_PI*150;
176        if (t>3.7) x[8]=1.2;    // 1A
177        if (t>3.9) x[8]=10.8;   // 9A
178        if (t>4.1) x[8]=25.2;  // 21A
179
180        if (t>4.3) Ww=2.*M_PI*0;
181        if (t>4.8) x[8]=-1.2;    // 1A
182        if (t>5.0) x[8]=-10.8;   // 9A
183        if (t>5.2) x[8]=-25.2;  // 21A
184
185        if (t>5.4) Ww=2.*M_PI*(-10.);
186        if (t>5.7) x[8]=-1.2;    // 1A
187        if (t>5.9) x[8]=-10.8;   // 9A
188        if (t>6.1) x[8]=-25.2;  // 21A
189
190        if (t>6.3) Ww=2.*M_PI*(-50.);
191        if (t>6.7) x[8]=-1.2;    // 1A
192        if (t>6.9) x[8]=-10.8;   // 9A
193        if (t>7.1) x[8]=-25.2;  // 21A
194
195        if (t>7.3) Ww=2.*M_PI*(-100.);
196        if (t>7.7) x[8]=-1.2;    // 1A
197        if (t>7.9) x[8]=-10.8;   // 9A
198        if (t>8.1) x[8]=-25.2;  // 21A
199        if (t>8.3) x[8]=10.8;   // 9A
200        if (t>8.5) x[8]=25.2;  // 21A
201
202
203        // letmy start s nenulovym momentem
204//        Iqwf=149.*sqrt(2.);
205
206                // pomal� rozjezd 10s do max. rychlosti 700rpm
207/*              Iqwf=149.*sqrt(2.);     // konst. Isqw = Imax
208                x[2]+=2.*M_PI*0.004;
209                if (x[2]>1615) x[2]=1615;
210                if (x[2]<-1615) x[2]=-1615;     /* osetreni pro test rozjezdu do zapornych otacek */
211
212                // rampa rozjezd 5s do max. rychlosti 700rpm a 10s do -700rpm
213/*              Iqwf=149.*sqrt(2.);     // konst. Isqw = Imax
214                x[2]+=k_rampa*2.*M_PI*0.008;
215                if (x[2]>1615) {x[2]=1615; if (k_rampa_tmp<24000) k_rampa_tmp++; else {k_rampa=-1;k_rampa_tmp=0;}}
216                if (x[2]<-1615) x[2]=-1615;  /**/
217
218                // KALMAN
219                ekf(ekf_estim,*u,*(u+1),Ucn,Uc_mer,Isx,Isy);
220
221                // vystup z EKF zaveden do regulatoru
222                // rychlost=ekf_estim[0]; theta=ekf_estim[1]; //VS
223
224                Urm_max=1.0;
225
226                // idealni vzorkovani - test vlivu
227//              Isx=x[0];Isy=x[1];rychlost=x[2];theta=x[3];Uc_mer=Uc;
228
229                if (MODULACE==0)        // sinusova PWM
230                  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
231                                                                                                                        // rezim=2 ... Iqw=sqrt(Imax^2-Idw^2)
232                else
233                  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
234                                                                                                                        // rezim=2 ... Iqw=sqrt(Imax^2-Idw^2)
235
236        reg_counter=0;
237        Isx=x[0];Isy=x[1];rychlost=x[2];theta=x[3]+CHYBA_POLOHY;Uc_mer=Uc;       // re�ln� vzorkov�n�
238      }
239      hpom_model=0;
240      h_model_counter=0;
241      reg_counter++;
242    }
243
244    if (t>=t_sense)
245      if (print_counter>199)
246      { 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]);
247      // t, Isx, Isy, Isd, Isdw, Isq, Isqw, M, Fs, poloha, rychlost, rychlost_w, Urm, Urm_max,
248      // Fs_model, M_model, zatezny uhel (beta), zatezny uhel vypocteny z napeti (zbeta), Uc
249        print_counter=0;
250      }
251
252    if (h_disp>0.2)
253    { printf("%f \n",t);
254      h_disp=0;
255    }
256
257    t+=h;
258    hpom+=h;
259    hpom_model+=h;
260    h_model_counter++;
261    h_disp+=h;
262    print_counter++;
263  }
264
265//  printf("*** \n\n");
266  fclose(fw);
267        return 0;
268 }
Note: See TracBrowser for help on using the browser.