root/simulator_zdenek/ekf_example/PWM.CPP @ 50

Revision 50, 8.7 kB (checked in by peroutka, 18 years ago)
Line 
1/* modul PWM modulace
2   ubytky a mrtve casy
3
4   shodna verze s PWM_FULL_LC
5
6   31.5. 2007
7
8   VER. zmenena metoda vypoctu pily (nove funkce)
9   31.5.2007    Doplnena PWM se treti harmonickou - fce pwm_full_new_3h(...) - prepinani mezi sinusovou PWM a 3. harmonickou pomoci priznaku mod!
10*/
11
12#define _USE_MATH_DEFINES
13#include <math.h>
14#include "pwm.h"
15
16void pwm_full_init(double Ucn, double fc, double DT);
17void pwm_full(double *us, double *u, double *Iz,double Ucn, double Uc, double *i, double fc, double *va_char,unsigned int pocet, double h);
18
19void pwm_full_new_init(double h, double fc, double DT, int citac_init, int D_smer, int D_smer2);
20void pwm_full_new(double *us, double *u, double *Iz, double Ucn, double Uc, double *is, double *va_char, unsigned int pocet);
21
22// PWM se treti harmonickou
23void pwm_full_new_init_3h(double h, double fc, double DT, int citac_init, int D_smer, int D_smer2);
24void pwm_full_new_3h(double *us, double *u, double *Iz, double Ucn, double Uc, double *is, double *va_char, unsigned int pocet, unsigned int mod);
25
26static double ubytek(double *va_char, double I, unsigned int pocet);
27
28static double up, up2, dup;
29static int smer, smer2;
30
31static int citac, citac2, citac_PR;
32
33double ladeni_pila, ladeni_ur;
34
35void pwm_full_init(double Ucn, double fc, double DT)
36/*
37Ucn ... jmenovite napeti kondenzatoru v ss obvodu
38fc  ... spinaci frekvence
39h   ... krok simulace
40DT  ... velikost mrtveho casu
41*/
42{
43  smer=-1; smer2=1;
44  up=Ucn/2;
45//  up=0.;
46  up2=up-2*Ucn*fc*DT;
47}
48
49void pwm_full(double *us, double *u, double *Iz,double Ucn, double Uc, double *is, double fc, double *va_char,unsigned int pocet, double h)
50/*
51u    ... pozadovane napeti
52us   ... [usx, usy]...vystupni napeti stridace v (x,y)
53Iz   ... proud v ss obvodu
54Ucn  ... jmenovite nap. v ss obvodu
55Uc   ... skutecne napeti na C
56is   ... [isx, isy]...proud motorem
57va_char ... VA-char vykonovych spinacu
58pocet   ... pocet bodu VA-charky
59h    ... krok simulace
60*/
61{
62  unsigned int i;
63  double iabc[3], ur[3],ustr[3],ua,ub,uc;
64  double dtr[3],dd[3];        // ubytky na polovodicich
65
66  ladeni_pila=up;
67
68  iabc[0]=*is;
69  iabc[1]=(-*is+sqrt(3.)**(is+1))/2.;
70  iabc[2]=(-*is-sqrt(3.)**(is+1))/2.;
71
72  ur[0]=*u;
73  ur[1]=(-*u+sqrt(3.)**(u+1))/2.;
74  ur[2]=(-*u-sqrt(3.)**(u+1))/2.;
75
76  for (i=0;i<3;i++)
77  { dtr[i]=ubytek(va_char,fabs(iabc[i]),pocet);
78    dd[i]=dtr[i]*.73;
79  }
80
81  // implementace ubytku a mrtvych casu
82  for (i=0;i<3;i++)
83    if (iabc[i]>=0)
84      if ((ur[i]>up) && (ur[i]>up2))
85        ustr[i]=Uc/2-dtr[i];
86      else
87        ustr[i]=-(Uc/2+dd[i]);
88    else
89      if ((ur[i]<up) && (ur[i]<up2))
90        ustr[i]=-(Uc/2-dtr[i]);
91      else
92        ustr[i]=Uc/2+dd[i];
93
94// fazova napeti zateze
95  ua=(2*ustr[0]-ustr[1]-ustr[2])/3;
96  ub=(2*ustr[1]-ustr[0]-ustr[2])/3;
97  uc=(2*ustr[2]-ustr[0]-ustr[1])/3;
98
99// vystupni napeti stridace v (x,y)
100  *us=(2.*ua-ub-uc)/3.;
101  *(us+1)=(ub-uc)/sqrt(3.);
102
103// vypocet pily pro PWM a pily pro mrtve casy
104  dup=2.*Ucn*fc*h;
105  up+=smer*dup;
106  if (up>Ucn/2)
107  { up-=2*dup;
108    smer=-1;
109  }
110  if (up<-Ucn/2)
111  { up+=2.*dup;
112    smer=1;
113  }
114
115  up2+=smer2*dup;
116  if (up2>Ucn/2)
117  { up2-=2.*dup;
118    smer2=-1;
119  }
120  if (up2<-Ucn/2)
121  { up2+=2.*dup;
122    smer2=1;
123  }
124
125  *Iz=(ustr[0]>0)*iabc[0]+(ustr[1]>0)*iabc[1]+(ustr[2]>0)*iabc[2];
126}
127
128static double ubytek(double *va_char, double I, unsigned int pocet)
129{
130  unsigned int ii;
131  double delta_u;
132
133  ii=0;
134  while ((*(va_char+ii)<I) && (ii<(pocet-1)))
135    ii++;
136
137  if (ii==(pocet-1))  // konec va_char - predpoklad nasyceni charky
138    delta_u=*(va_char+ii+pocet);
139  else
140    if (ii==0)
141      delta_u=0;
142    else
143      delta_u=*(va_char+ii-1+pocet)+(I-*(va_char+ii-1))/(*(va_char+ii)-*(va_char+ii-1))*(*(va_char+ii+pocet)-*(va_char+ii-1+pocet));
144
145  return delta_u;
146}
147
148
149void pwm_full_new(double *us, double *u, double *Iz, double Ucn, double Uc, double *is, double *va_char, unsigned int pocet)
150/*
151u    ... pozadovane napeti
152us   ... [usx, usy]...vystupni napeti stridace v (x,y)
153Iz   ... proud v ss obvodu
154Ucn  ... jmenovite nap. v ss obvodu
155Uc   ... skutecne napeti na C
156is   ... [isx, isy]...proud motorem
157va_char ... VA-char vykonovych spinacu
158pocet   ... pocet bodu VA-charky
159t    ... aktualni cas simulace
160*/
161{
162  unsigned int i;
163  double iabc[3], ur[3],ustr[3],ua,ub,uc;
164  double dtr[3],dd[3];        // ubytky na polovodicich
165
166  // vypocet pily
167  up=(double)citac/citac_PR*Ucn/2.;
168  up2=(double)citac2/citac_PR*Ucn/2.;
169
170  ladeni_pila=up;
171
172  iabc[0]=*is;
173  iabc[1]=(-*is+sqrt(3.)**(is+1))/2.;
174  iabc[2]=(-*is-sqrt(3.)**(is+1))/2.;
175
176  ur[0]=*u;
177  ur[1]=(-*u+sqrt(3.)**(u+1))/2.;
178  ur[2]=(-*u-sqrt(3.)**(u+1))/2.;
179
180  for (i=0;i<3;i++)
181  { dtr[i]=ubytek(va_char,fabs(iabc[i]),pocet);
182    dd[i]=dtr[i]*.73;
183  }
184
185  // implementace ubytku a mrtvych casu
186  for (i=0;i<3;i++)
187    if (iabc[i]>=0)
188      if ((ur[i]>up) && (ur[i]>up2))
189        ustr[i]=Uc/2-dtr[i];
190      else
191        ustr[i]=-(Uc/2+dd[i]);
192    else
193      if ((ur[i]<up) && (ur[i]<up2))
194        ustr[i]=-(Uc/2-dtr[i]);
195      else
196        ustr[i]=Uc/2+dd[i];
197
198// fazova napeti zateze
199  ua=(2*ustr[0]-ustr[1]-ustr[2])/3;
200  ub=(2*ustr[1]-ustr[0]-ustr[2])/3;
201  uc=(2*ustr[2]-ustr[0]-ustr[1])/3;
202
203// vystupni napeti stridace v (x,y)
204  *us=(2.*ua-ub-uc)/3.;
205  *(us+1)=(ub-uc)/sqrt(3.);
206
207  *Iz=(ustr[0]>0)*iabc[0]+(ustr[1]>0)*iabc[1]+(ustr[2]>0)*iabc[2];
208
209  // posun pily ... inkrement citacu
210  if ((citac==citac_PR)||(citac==-citac_PR)) smer*=-1;
211  if ((citac2==citac_PR)||(citac2==-citac_PR)) smer2*=-1;
212  citac+=smer;
213  citac2+=smer2;
214}
215
216void pwm_full_new_init(double h, double fc, double DT, int citac_init, int D_smer, int D_smer2)
217/*
218fc  ... spinaci frekvence
219h   ... krok simulace
220DT  ... velikost mrtveho casu
221citac_init ... pocatecni nastaveni citace
222D_smer, D_smer1 ... pocatecni nastaveni smeru citani
223*/
224{
225  smer=D_smer; smer2=D_smer2;
226  citac=citac_init;
227  citac2=citac_init-(DT/h);
228  citac_PR=1./4./fc/h;
229}
230
231
232void pwm_full_new_init_3h(double h, double fc, double DT, int citac_init, int D_smer, int D_smer2)
233/*
234fc  ... spinaci frekvence
235h   ... krok simulace
236DT  ... velikost mrtveho casu
237citac_init ... pocatecni nastaveni citace
238D_smer, D_smer1 ... pocatecni nastaveni smeru citani
239*/
240{
241  smer=D_smer; smer2=D_smer2;
242  citac=citac_init;
243  citac2=abs(citac_init-(DT/h));
244  citac_PR=1./2./fc/h;
245}
246
247
248void pwm_full_new_3h(double *us, double *u, double *Iz, double Ucn, double Uc, double *is, double *va_char, unsigned int pocet, unsigned int mod)
249/*
250u    ... pozadovane napeti ve formatu u={Um, beta}
251us   ... [usx, usy]...vystupni napeti stridace v (x,y)
252Iz   ... proud v ss obvodu
253Ucn  ... jmenovite nap. v ss obvodu
254Uc   ... skutecne napeti na C
255is   ... [isx, isy]...proud motorem
256va_char ... VA-char vykonovych spinacu
257pocet   ... pocet bodu VA-charky
258t    ... aktualni cas simulace
259mod ... mod= - sinusova PWM; mod=1 - PWM se 3. harmonickou
260*/
261{
262  unsigned int i;
263  double iabc[3], ur[3],ustr[3],ua,ub,uc;
264  double dtr[3],dd[3];        // ubytky na polovodicich
265  double Um, beta;
266  double U3;                              // korekce 3. harmonickou
267
268
269  Um=*u;
270  beta=*(u+1);
271
272  // vypocet pily
273//  up=(double)citac/citac_PR*Ucn/2.;
274//  up2=(double)citac2/citac_PR*Ucn/2.;
275  // vypocet pily presne jako v procesoru
276  up=((double)citac/citac_PR-0.5)*Ucn;
277  up2=((double)citac2/citac_PR-0.5)*Ucn;
278
279  ladeni_pila=up;
280
281  iabc[0]=*is;
282  iabc[1]=(-*is+sqrt(3.)**(is+1))/2.;
283  iabc[2]=(-*is-sqrt(3.)**(is+1))/2.;
284
285  if (mod==0)   // sinusova PWM
286  {
287    ur[0]=Um*cos(beta);
288    ur[1]=Um*cos(beta-2./3.*M_PI);
289    ur[2]=Um*cos(beta+2./3.*M_PI);
290  }
291  else                  // PWM se 3. harmonickou
292  {
293    // dle Karla
294//    U3=0.13*cos(3.*beta);
295        // moje
296        U3=0.17*cos(3.*beta);
297    ur[0]=Um*(cos(beta)-U3);
298    ur[1]=Um*(cos(beta-2./3.*M_PI)-U3);
299    ur[2]=Um*(cos(beta+2./3.*M_PI)-U3);
300  }
301
302  ladeni_ur=ur[0];
303
304  for (i=0;i<3;i++)
305  { dtr[i]=ubytek(va_char,fabs(iabc[i]),pocet);
306    dd[i]=dtr[i]*.73;
307  }
308
309  // implementace ubytku a mrtvych casu
310  for (i=0;i<3;i++)
311    if (iabc[i]>=0)
312      if ((ur[i]>up) && (ur[i]>up2))
313        ustr[i]=Uc/2-dtr[i];
314      else
315        ustr[i]=-(Uc/2+dd[i]);
316    else
317      if ((ur[i]<up) && (ur[i]<up2))
318        ustr[i]=-(Uc/2-dtr[i]);
319      else
320        ustr[i]=Uc/2+dd[i];
321
322// fazova napeti zateze
323  ua=(2.*ustr[0]-ustr[1]-ustr[2])/3.;
324  ub=(2.*ustr[1]-ustr[0]-ustr[2])/3.;
325  uc=(2.*ustr[2]-ustr[0]-ustr[1])/3.;
326
327// vystupni napeti stridace v (x,y)
328  *us=(2.*ua-ub-uc)/3.;
329  *(us+1)=(ub-uc)/sqrt(3.);
330
331  *Iz=(ustr[0]>0)*iabc[0]+(ustr[1]>0)*iabc[1]+(ustr[2]>0)*iabc[2];
332
333  // posun pily ... inkrement citacu
334//  if ((citac==citac_PR)||(citac==-citac_PR)) smer*=-1;
335//  if ((citac2==citac_PR)||(citac2==-citac_PR)) smer2*=-1;
336  if ((citac==citac_PR)||(citac==0)) smer*=-1;
337  if ((citac2==citac_PR)||(citac2==0)) smer2*=-1;
338  citac+=smer;
339  citac2+=smer2;
340}
Note: See TracBrowser for help on using the browser.