root/pmsm/simulator_zdenek/simulator.cpp @ 278

Revision 278, 10.6 kB (checked in by smidl, 16 years ago)

props

  • Property svn:eol-style set to native
RevLine 
[278]1/*
2   Simulator of Vector Controlled PMSM Drive
3
4   This module is background for PMSM drive object design and
5   introduces basic functions ... set_parameters() and eval().
6
7   Z. Peroutka
8
9Rev. 16.3.2008
10
11*/
12
13#define _USE_MATH_DEFINES
14
15#include <math.h>
16#include <stdlib.h> //na linuxu je abs v stdlib
17#include "regulace.h"
18#include "simulator.h"
19
20
21#define REZIM_REGULACE  1       // 0...reg. momentu, 1...reg.rychlosti, 2... Isqw=sqrt(Imax^2-Id^2) - max. moment
22
23void pmsmsim_set_parameters(double Rs0, double Ls0, double Fmag0, double Bf0, double p0, double kp0, double J0, double Uc0, double DT0, double dt0);
24void pmsmsim_step(double Ww);
25
26// local functions
27static void pwm(unsigned int mod);
28static double ubytek(double I);
29static void pmsm_model(unsigned int mod);
30
31
32// simulator properties ///////////////////////
33static double Rs,Ls,Fmag,Bf,p,kp,J;        // parameters of PMSM model
34static double Ucn,Uc,DT,U_modulace;        // dc-link voltage and dead-time
35static double Urm_max;                     // field weakening
36static double h,h_reg,h_reg_real;          // simulation step and sampling of employed loops
37unsigned int h_reg_counter,h_reg_counter_mez;       // emulation of DSP operation
38
39static 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
40static unsigned int pocet=8;            // velikost VA-charky
41
42// system state
43double x[9]; // (isx,isy,wme,theta_e,M,Fsd,Isd,Isq,Mz)
44
45// internal variables of PWM module
46static int smer, smer2, citac, citac2, citac_PR, modulace;
47
48// internal variables of PMSM model
49static double dIsx,dIsx1,dIsx2,dIsx3,dIsy,dIsy1,dIsy2,dIsy3;
50static double dTheta,dTheta1,dTheta2,dTheta3;
51static double dw,dw1,dw2,dw3;
52
53// system measures
54static double Isx, Isy, theta, speed;
55
56// control
57static double u[2]={0.,0.};             // format u={Um, beta}
58static double us[2]={0.,0.};    // format us={us_alfa, us_beta}
59
60// variables for calculation of mean values of stator voltage components
61static double usx_av=0., usy_av=0.,sum_usx_av=0.,sum_usy_av=0.;
62
63// variables for calculation of mean values of stator current components - (alfa, beta)
64static double isx_av=0., isy_av=0.,sum_isx_av=0.,sum_isy_av=0.;
65
66// stator voltage components filtering
67static double usxf=0.,usyf=0.,Tf=0.01;
68static unsigned int start_filter=1;
69
70// output for EKF (voltages and measured currents, which are fed to KalmanObs)
71double KalmanObs[10]={0.,0.,0.,0.,0.,0.,0.,0.,0.,0.};      // usx, usy, Isx, Isy, usx_av, usy_av
72
73// real-time
74double t=0.; //VS removed static due to clash with export in .h
75
76// stator voltage components in alfa beta (inluding impact of the real dc-link voltage)
77static double ualfa=0., ubeta=0.;
78
79
80/////////////////// POMOCNE FUNKCE //////////////////////////////////
81double uhel(double x, double y)
82{
83  double th;
84
85  if (x==0)
86    if (y==0) th=0.;
87        else if (y>0) th=M_PI/2.;
88             else th=-M_PI/2.;
89  else
90    th=atan(y/x);
91
92  if (x<0) th+=M_PI;
93
94  return th;
95}
96/////////////////////////////////////////////////////////////////////
97
98
99void pmsmsim_set_parameters(double Rs0, double Ls0, double Fmag0, double Bf0, double p0, double kp0, double J0, double Uc0, double DT0, double dt0)
100{
101  int tmp_i;
102
103  // simulator parameters setup
104  Rs=Rs0;
105  Ls=Ls0;
106  Fmag=Fmag0;
107  Bf=Bf0;
108  p=p0;
109  kp=kp0;
110  J=J0;
111  Ucn=600.;
112  Uc=Uc0;
113  DT=DT0;
114
115  // control setup
116  Urm_max=0.95;
117
118  // simulator sampling - fixed setup
119  h=dt0;
120  h_reg=125e-6;         // fpwm = 4kHz
121  h_reg_counter_mez=(int)(h_reg/h);         // emulation of operation of DSP timer
122  //h_reg_counter=h_reg_counter_mez;
123  h_reg_counter=1;
124  h_reg_real=h_reg_counter_mez*h;           // real sampling period
125
126  // reset of the system state variables
127  for (tmp_i=0;tmp_i<9;tmp_i++)
128    x[tmp_i]=0.;
129
130  // emulation of the first measure
131  Isx=0.;Isy=0.;theta=x[3];speed=x[2];
132
133// === init of particular modules of simulator ===
134  // PWM init
135  smer=-1; smer2=-1;
136  citac=0;
137  citac2=abs(0-(int)(DT/h)); //VS: oprava, je to spravne?
138  citac_PR=h_reg_counter_mez;
139
140  // first interrupt occur after first period match => add 1 to both counter registers
141  citac++;smer=1;
142  citac2--;
143
144  modulace=1;           // THIPWM
145  if (modulace==1)
146    U_modulace=Ucn/sqrt(3.);
147  else
148    U_modulace=Ucn/2.;
149
150  // PMSM model init
151  dIsx=0;dIsx1=0;dIsx2=0;dIsx3=0;dIsy=0;dIsy1=0;dIsy2=0;dIsy3=0;
152  dTheta=0;dTheta1=0;dTheta2=0;dTheta3=0;
153  dw=0;dw1=0;dw2=0;dw3=0;
154
155  init_regulace(Ls,Fmag,kp,p,h_reg_real);
156}
157
158
159static void pwm(unsigned int mod)
160// mod ... mod=0 - sinusoidal PWM; mod=1 - PWM with injected 3rd harmonic
161{
162  unsigned int i;
163  double iabc[3], ur[3],ustr[3],ua,ub,uc;
164  double dtr[3],dd[3];
165  double Um, beta;
166  double U3;
167  double up, up2;
168
169  Um=*u;
170  beta=*(u+1);
171
172  // emulation of carrier - timer
173  up=((double)citac/citac_PR-0.5)*Ucn;
174  up2=((double)citac2/citac_PR-0.5)*Ucn;
175
176  iabc[0]=*x;
177  iabc[1]=(-*x+sqrt(3.)**(x+1))/2.;
178  iabc[2]=(-*x-sqrt(3.)**(x+1))/2.;
179
180  if (mod==0)   // sin. PWM
181  {
182    ur[0]=Um*cos(beta);
183    ur[1]=Um*cos(beta-2./3.*M_PI);
184    ur[2]=Um*cos(beta+2./3.*M_PI);
185  }
186  else                  // PWM with injected 3rd harmonic
187  {
188    U3=0.17*cos(3.*beta);
189    ur[0]=Um*(cos(beta)-U3);
190    ur[1]=Um*(cos(beta-2./3.*M_PI)-U3);
191    ur[2]=Um*(cos(beta+2./3.*M_PI)-U3);
192  }
193
194  for (i=0;i<3;i++)
195  { dtr[i]=ubytek(fabs(iabc[i]));
196    dd[i]=dtr[i]*.73;
197  }
198
199  // implementation of voltage drops and dead-times
200  for (i=0;i<3;i++)
201    if (iabc[i]>=0)
202      if ((ur[i]>up) && (ur[i]>up2))
203        ustr[i]=Uc/2-dtr[i];
204      else
205        ustr[i]=-(Uc/2+dd[i]);
206    else
207      if ((ur[i]<up) && (ur[i]<up2))
208        ustr[i]=-(Uc/2-dtr[i]);
209      else
210        ustr[i]=Uc/2+dd[i];
211
212// phase voltages
213  ua=(2.*ustr[0]-ustr[1]-ustr[2])/3.;
214  ub=(2.*ustr[1]-ustr[0]-ustr[2])/3.;
215  uc=(2.*ustr[2]-ustr[0]-ustr[1])/3.;
216
217// voltage vector in stationary reference frame (x,y)
218  *us=(2.*ua-ub-uc)/3.;
219  *(us+1)=(ub-uc)/sqrt(3.);
220
221  // emulation of DSP timers
222  if ((citac==citac_PR)||(citac==0))
223  {
224    smer*=-1;
225    // calculation of stator voltage components mean values
226    usx_av=h/h_reg*sum_usx_av;
227    usy_av=h/h_reg*sum_usy_av;
228    // reset of sum accumulators
229    sum_usx_av=0.;
230    sum_usy_av=0.;
231
232    // stator current components mean values - reference frame (alfa, beta)
233    isx_av=h/h_reg*sum_isx_av;
234    isy_av=h/h_reg*sum_isy_av;
235    // reset of sum accumulators
236    sum_isx_av=0.;
237    sum_isy_av=0.;
238  }
239  if ((citac2==citac_PR)||(citac2==0)) smer2*=-1;
240  citac+=smer;
241  citac2+=smer2;
242
243  // calculation of stator voltage components mean values - sum
244  sum_usx_av+=*us;
245  sum_usy_av+=*(us+1);
246
247  // stator voltage components filtering
248  //if (start_filter==1)
249  usxf+=(*us-usxf)*h/h_reg;
250  usyf+=(*(us+1)-usyf)*h/h_reg;
251
252  // stator current components mean values - reference frame (alfa, beta)
253  sum_isx_av+=*x;
254  sum_isy_av+=*(x+1);
255}
256
257static double ubytek(double I)
258{
259  unsigned int ii;
260  double delta_u;
261
262  ii=0;
263  while ((*(va_char+ii)<I) && (ii<(pocet-1)))
264    ii++;
265
266  if (ii==(pocet-1))
267    delta_u=*(va_char+ii+pocet);
268  else
269    if (ii==0)
270      delta_u=0;
271    else
272      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));
273
274  return delta_u;
275}
276
277
278static void pmsm_model(unsigned int mod)
279// mod<5...Euler, mod>4 ... Adams of 4th order
280{
281  double usx, usy;
282
283  usx=*us;
284  usy=*(us+1);
285
286  dIsx=-Rs/Ls*x[0]+Fmag/Ls*x[2]*sin(x[3])+usx/Ls;
287  dIsy=-Rs/Ls*x[1]-Fmag/Ls*x[2]*cos(x[3])+usy/Ls;
288  dTheta=x[2];
289
290  if (J>0)
291    dw=kp*p*p*Fmag/J*(x[1]*cos(x[3])-x[0]*sin(x[3]))-Bf/J*x[2]-p/J*x[8];
292  else
293    dw=0;
294
295  // integration
296  if (mod<5)  // Euler
297  { x[0]+=dIsx*h;
298    x[1]+=dIsy*h;
299    x[2]+=dw*h;
300    x[3]+=dTheta*h;
301  }
302  else                  // Adams (4th order)
303  { x[0]+=h/24.*(55.*dIsx-59.*dIsx1+37.*dIsx2-9.*dIsx3);
304    x[1]+=h/24.*(55.*dIsy-59.*dIsy1+37.*dIsy2-9.*dIsy3);
305    x[2]+=h/24.*(55.*dw-59.*dw1+37.*dw2-9.*dw3);
306    x[3]+=h/24.*(55.*dTheta-59.*dTheta1+37.*dTheta2-9.*dTheta3);
307  }
308
309  // saturation of theta to (-pi,pi)
310  if (x[3]>M_PI) x[3]-=(2*M_PI);
311  if (x[3]<-M_PI) x[3]+=(2*M_PI);
312
313  // diff. shift - Adams
314  dIsx3=dIsx2;dIsx2=dIsx1;dIsx1=dIsx;
315  dIsy3=dIsy2;dIsy2=dIsy1;dIsy1=dIsy;
316  dTheta3=dTheta2;dTheta2=dTheta1;dTheta1=dTheta;
317  dw3=dw2;dw2=dw1;dw1=dw;
318
319  // calculation of Isd, Isq
320  x[6]=x[0]*cos(x[3])+x[1]*sin(x[3]);         // Isd
321  x[7]=x[1]*cos(x[3])-x[0]*sin(x[3]);         // Isq
322
323  // Fsd ... d-component of stator flux
324  x[5]=Ls*x[6]+Fmag;
325
326  // Torque
327  x[4]=kp*p*Fmag*(x[1]*cos(x[3])-x[0]*sin(x[3]));
328}
329
330//////////////////////////////////////////////////////////////////////////////////////////////////////
331void pmsmsim_step(double Ww)            // you must link array KalmanObs[] to EKF modul
332{
333  double Umk, ub, uc;
334
335//  while (t<=t_end)
336  {
337    pwm(modulace);
338//    *us=KalmanObs[0]; *(us+1)=KalmanObs[1];
339//     *us=ualfa; *(us+1)=ubeta;
340    pmsm_model(5);
341
342    if (h_reg_counter>=h_reg_counter_mez)           // pocatek ISR
343    {
344      // voltages and measured currents for EKF
345//       Umk=*u*Uc/Ucn;
346//       ualfa=Umk*cos(*(u+1));
347//       ub=Umk*cos(*(u+1)-2./3.*M_PI);
348      KalmanObs[0]=ualfa;                     // usx
349      //KalmanObs[1]=(ualfa+2.*ub)/sqrt(3.);    // usy
350      KalmanObs[1]=ubeta;    // usy
351     
352      // real sampling - considered transport delay equal to the sampling period
353/*     KalmanObs[2]=Isx;
354     KalmanObs[3]=Isy;*/
355      // ideal sampling
356      KalmanObs[2]=x[0];
357      KalmanObs[3]=x[1];
358     
359      // diagnostic - mean values of stator voltage components - pwm()
360      KalmanObs[4]=usx_av;
361      KalmanObs[5]=usy_av;
362      KalmanObs[6]=usxf;
363      KalmanObs[7]=usyf;
364      KalmanObs[8]=isx_av;
365      KalmanObs[9]=isy_av;     
366
367      vektor_regulace(0,0,Urm_max,Ww,u,Isx,Isy,theta,speed,U_modulace,Uc,Ucn,REZIM_REGULACE);   // rezim=1 ... reg. rychlosti, rezim=0 ... reg. momentu
368                                                                                                                // rezim=2 ... Iqw=sqrt(Imax^2-Idw^2)
369      // emulation of the real sampling of A/D converter
370      Isx=x[0];Isy=x[1];speed=x[2];theta=x[3];
371
372      // include ideal commanded stator voltage
373      Umk=*u*Uc/Ucn;
374      ualfa=Umk*cos(*(u+1));               // usx = usa
375      ub=Umk*cos(*(u+1)-2./3.*M_PI);
376      ubeta=(ualfa+2.*ub)/sqrt(3.);    // usy
377//       uc=-ualfa-ub;
378//       ubeta=(ub-uc)/sqrt(3.);
379
380      h_reg_counter=0;
381    }
382
383    t+=h;
384    h_reg_counter++;
385  }
386}
387
388void pmsmsim_noreg_step(double ua, double ub)            // you must link array KalmanObs[] to EKF modul
389{
390  double kor_Uc;
391
392  *u=sqrt(ua*ua+ub*ub);
393  *(u+1)=uhel(ua,ub);
394
395  // uprava velikosti vzhledem k Uc!=Ucn
396  kor_Uc=Ucn/230.;
397  *u*=kor_Uc; /**/
398
399  pwm(modulace);
400
401//  *us=*u*cos(*(u+1));
402//  *(us+1)=*u*sin(*(u+1));;
403
404  pmsm_model(5);
405}
406//////////////////////////////////////////////////////////////////////////////////////////////////////
407//////////////////////////////////////////////////////////////////////////////////////////////////////
Note: See TracBrowser for help on using the browser.