root/pmsm/simulator_zdenek/simulator.cpp @ 169

Revision 135, 10.2 kB (checked in by smidl, 16 years ago)

oprava simulatoru

Line 
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
79void pmsmsim_set_parameters(double Rs0, double Ls0, double Fmag0, double Bf0, double p0, double kp0, double J0, double Uc0, double DT0, double dt0)
80{
81  int tmp_i;
82
83  // simulator parameters setup
84  Rs=Rs0;
85  Ls=Ls0;
86  Fmag=Fmag0;
87  Bf=Bf0;
88  p=p0;
89  kp=kp0;
90  J=J0;
91  Ucn=600.;
92  Uc=Uc0;
93  DT=DT0;
94
95  // control setup
96  Urm_max=0.95;
97
98  // simulator sampling - fixed setup
99  h=dt0;
100  h_reg=125e-6;         // fpwm = 4kHz
101  h_reg_counter_mez=(int)(h_reg/h);         // emulation of operation of DSP timer
102  //h_reg_counter=h_reg_counter_mez;
103  h_reg_counter=1;
104  h_reg_real=h_reg_counter_mez*h;           // real sampling period
105
106  // reset of the system state variables
107  for (tmp_i=0;tmp_i<9;tmp_i++)
108    x[tmp_i]=0.;
109
110  // emulation of the first measure
111  Isx=0.;Isy=0.;theta=x[3];speed=x[2];
112
113// === init of particular modules of simulator ===
114  // PWM init
115  smer=-1; smer2=-1;
116  citac=0;
117  citac2=abs(0-(int)(DT/h)); //VS: oprava, je to spravne?
118  citac_PR=h_reg_counter_mez;
119
120  // first interrupt occur after first period match => add 1 to both counter registers
121  citac++;smer=1;
122  citac2--;
123
124  modulace=1;           // THIPWM
125  if (modulace==1)
126    U_modulace=Ucn/sqrt(3.);
127  else
128    U_modulace=Ucn/2.;
129
130  // PMSM model init
131  dIsx=0;dIsx1=0;dIsx2=0;dIsx3=0;dIsy=0;dIsy1=0;dIsy2=0;dIsy3=0;
132  dTheta=0;dTheta1=0;dTheta2=0;dTheta3=0;
133  dw=0;dw1=0;dw2=0;dw3=0;
134
135  init_regulace(Ls,Fmag,kp,p,h_reg_real);
136}
137
138
139static void pwm(unsigned int mod)
140// mod ... mod=0 - sinusoidal PWM; mod=1 - PWM with injected 3rd harmonic
141{
142  unsigned int i;
143  double iabc[3], ur[3],ustr[3],ua,ub,uc;
144  double dtr[3],dd[3];
145  double Um, beta;
146  double U3;
147  double up, up2;
148
149  Um=*u;
150  beta=*(u+1);
151
152  // emulation of carrier - timer
153  up=((double)citac/citac_PR-0.5)*Ucn;
154  up2=((double)citac2/citac_PR-0.5)*Ucn;
155
156  iabc[0]=*x;
157  iabc[1]=(-*x+sqrt(3.)**(x+1))/2.;
158  iabc[2]=(-*x-sqrt(3.)**(x+1))/2.;
159
160  if (mod==0)   // sin. PWM
161  {
162    ur[0]=Um*cos(beta);
163    ur[1]=Um*cos(beta-2./3.*M_PI);
164    ur[2]=Um*cos(beta+2./3.*M_PI);
165  }
166  else                  // PWM with injected 3rd harmonic
167  {
168    U3=0.17*cos(3.*beta);
169    ur[0]=Um*(cos(beta)-U3);
170    ur[1]=Um*(cos(beta-2./3.*M_PI)-U3);
171    ur[2]=Um*(cos(beta+2./3.*M_PI)-U3);
172  }
173
174  for (i=0;i<3;i++)
175  { dtr[i]=ubytek(fabs(iabc[i]))*0.;
176    dd[i]=dtr[i]*.73;
177  }
178
179  // implementation of voltage drops and dead-times
180  for (i=0;i<3;i++)
181    if (iabc[i]>=0)
182      if ((ur[i]>up) && (ur[i]>up2))
183        ustr[i]=Uc/2-dtr[i];
184      else
185        ustr[i]=-(Uc/2+dd[i]);
186    else
187      if ((ur[i]<up) && (ur[i]<up2))
188        ustr[i]=-(Uc/2-dtr[i]);
189      else
190        ustr[i]=Uc/2+dd[i];
191
192// phase voltages
193  ua=(2.*ustr[0]-ustr[1]-ustr[2])/3.;
194  ub=(2.*ustr[1]-ustr[0]-ustr[2])/3.;
195  uc=(2.*ustr[2]-ustr[0]-ustr[1])/3.;
196
197// voltage vector in stationary reference frame (x,y)
198  *us=(2.*ua-ub-uc)/3.;
199  *(us+1)=(ub-uc)/sqrt(3.);
200
201  // emulation of DSP timers
202  if ((citac==citac_PR)||(citac==0))
203  {
204    smer*=-1;
205    // calculation of stator voltage components mean values
206    usx_av=h/h_reg*sum_usx_av;
207    usy_av=h/h_reg*sum_usy_av;
208    // reset of sum accumulators
209    sum_usx_av=0.;
210    sum_usy_av=0.;
211   
212    // stator current components mean values - reference frame (alfa, beta)
213    isx_av=h/h_reg*sum_isx_av;
214    isy_av=h/h_reg*sum_isy_av;
215    // reset of sum accumulators
216    sum_isx_av=0.;
217    sum_isy_av=0.;
218  }
219  if ((citac2==citac_PR)||(citac2==0)) smer2*=-1;
220  citac+=smer;
221  citac2+=smer2;
222 
223  // calculation of stator voltage components mean values - sum
224  sum_usx_av+=*us;
225  sum_usy_av+=*(us+1);
226 
227  // stator voltage components filtering
228  //if (start_filter==1)
229  usxf+=(*us-usxf)*h/h_reg;
230  usyf+=(*(us+1)-usyf)*h/h_reg;
231 
232  // stator current components mean values - reference frame (alfa, beta)
233  sum_isx_av+=*x;
234  sum_isy_av+=*(x+1);
235}
236
237static double ubytek(double I)
238{
239  unsigned int ii;
240  double delta_u;
241
242  ii=0;
243  while ((*(va_char+ii)<I) && (ii<(pocet-1)))
244    ii++;
245
246  if (ii==(pocet-1))
247    delta_u=*(va_char+ii+pocet);
248  else
249    if (ii==0)
250      delta_u=0;
251    else
252      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));
253
254  return delta_u;
255}
256
257
258static void pmsm_model(unsigned int mod)
259// mod<5...Euler, mod>4 ... Adams of 4th order
260{
261  double usx, usy;
262
263  usx=*us;
264  usy=*(us+1);
265
266  dIsx=-Rs/Ls*x[0]+Fmag/Ls*x[2]*sin(x[3])+usx/Ls;
267  dIsy=-Rs/Ls*x[1]-Fmag/Ls*x[2]*cos(x[3])+usy/Ls;
268  dTheta=x[2];
269
270  if (J>0)
271    dw=kp*p*p*Fmag/J*(x[1]*cos(x[3])-x[0]*sin(x[3]))-Bf/J*x[2]-p/J*x[8];
272  else
273    dw=0;
274
275  // integration
276  if (mod<5)  // Euler
277  { x[0]+=dIsx*h;
278    x[1]+=dIsy*h;
279    x[2]+=dw*h;
280    x[3]+=dTheta*h;
281  }
282  else                  // Adams (4th order)
283  { x[0]+=h/24.*(55.*dIsx-59.*dIsx1+37.*dIsx2-9.*dIsx3);
284    x[1]+=h/24.*(55.*dIsy-59.*dIsy1+37.*dIsy2-9.*dIsy3);
285    x[2]+=h/24.*(55.*dw-59.*dw1+37.*dw2-9.*dw3);
286    x[3]+=h/24.*(55.*dTheta-59.*dTheta1+37.*dTheta2-9.*dTheta3);
287  }
288
289  // saturation of theta to (-pi,pi)
290  if (x[3]>M_PI) x[3]-=(2*M_PI);
291  if (x[3]<-M_PI) x[3]+=(2*M_PI);
292
293  // diff. shift - Adams
294  dIsx3=dIsx2;dIsx2=dIsx1;dIsx1=dIsx;
295  dIsy3=dIsy2;dIsy2=dIsy1;dIsy1=dIsy;
296  dTheta3=dTheta2;dTheta2=dTheta1;dTheta1=dTheta;
297  dw3=dw2;dw2=dw1;dw1=dw;
298
299  // calculation of Isd, Isq
300  x[6]=x[0]*cos(x[3])+x[1]*sin(x[3]);         // Isd
301  x[7]=x[1]*cos(x[3])-x[0]*sin(x[3]);         // Isq
302
303  // Fsd ... d-component of stator flux
304  x[5]=Ls*x[6]+Fmag;
305
306  // Torque
307  x[4]=kp*p*Fmag*(x[1]*cos(x[3])-x[0]*sin(x[3]));
308}
309
310//////////////////////////////////////////////////////////////////////////////////////////////////////
311void pmsmsim_step(double Ww)            // you must link array KalmanObs[] to EKF modul
312{
313  double Umk, ub, uc;
314
315//  while (t<=t_end)
316  {
317    pwm(modulace);
318//    *us=KalmanObs[0]; *(us+1)=KalmanObs[1];
319//     *us=ualfa; *(us+1)=ubeta;
320    pmsm_model(5);
321
322    if (h_reg_counter>=h_reg_counter_mez)           // pocatek ISR
323    {
324      // voltages and measured currents for EKF
325//       Umk=*u*Uc/Ucn;
326//       ualfa=Umk*cos(*(u+1));
327//       ub=Umk*cos(*(u+1)-2./3.*M_PI);
328      KalmanObs[0]=ualfa;                     // usx
329      //KalmanObs[1]=(ualfa+2.*ub)/sqrt(3.);    // usy
330      KalmanObs[1]=ubeta;    // usy
331     
332      // real sampling - considered transport delay equal to the sampling period
333/*     KalmanObs[2]=Isx;
334     KalmanObs[3]=Isy;*/
335      // ideal sampling
336      KalmanObs[2]=x[0];
337      KalmanObs[3]=x[1];
338     
339      // diagnostic - mean values of stator voltage components - pwm()
340      KalmanObs[4]=usx_av;
341      KalmanObs[5]=usy_av;
342      KalmanObs[6]=usxf;
343      KalmanObs[7]=usyf;
344      KalmanObs[8]=isx_av;
345      KalmanObs[9]=isy_av;     
346
347      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
348                                                                                                                // rezim=2 ... Iqw=sqrt(Imax^2-Idw^2)
349      // emulation of the real sampling of A/D converter
350      Isx=x[0];Isy=x[1];speed=x[2];theta=x[3];
351
352      // include ideal commanded stator voltage
353      Umk=*u*Uc/Ucn;
354      ualfa=Umk*cos(*(u+1));               // usx = usa
355      ub=Umk*cos(*(u+1)-2./3.*M_PI);
356      ubeta=(ualfa+2.*ub)/sqrt(3.);    // usy
357//       uc=-ualfa-ub;
358//       ubeta=(ub-uc)/sqrt(3.);
359
360      h_reg_counter=0;
361    }
362
363    t+=h;
364    h_reg_counter++;
365  }
366}
367//////////////////////////////////////////////////////////////////////////////////////////////////////
Note: See TracBrowser for help on using the browser.