root/applications/pmsm/simulator_zdenek/demo_example/simulator.cpp @ 1292

/*
   Simulator of Vector Controlled PMSM Drive

   This module is background for PMSM drive object design and
   introduces basic functions ... set_parameters() and eval().

   Z. Peroutka

Rev. 16.3.2008

*/

#include <math.h>
#include "regulace.h"
#include "simulator.h"

#define REZIM_REGULACE  1       // 0...reg. momentu, 1...reg.rychlosti, 2... Isqw=sqrt(Imax^2-Id^2) - max. moment

void set_parameters(double Rs0, double Ls0, double Fmag0, double Bf0, double p0, double kp0, double J0, double Uc0, double DT0, double dt0);
void eval(double Ww);

// local functions
static void pwm(unsigned int mod);
static double ubytek(double I);
static void pmsm_model(unsigned int mod);


// simulator properties ///////////////////////
static double Rs,Ls,Fmag,Bf,p,kp,J;        // parameters of PMSM model
static double Ucn,Uc,DT,U_modulace;        // dc-link voltage and dead-time
static double Urm_max;                     // field weakening
static double h,h_reg,h_reg_real;          // simulation step and sampling of employed loops
unsigned int h_reg_counter,h_reg_counter_mez;       // emulation of DSP operation

static 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
static unsigned int pocet=8;            // velikost VA-charky

// system state
double x[9]; // (isx,isy,wme,theta_e,M,Fsd,Isd,Isq,Mz)

// internal variables of PWM module
static int smer, smer2, citac, citac2, citac_PR, modulace;

// internal variables of PMSM model
static double dIsx,dIsx1,dIsx2,dIsx3,dIsy,dIsy1,dIsy2,dIsy3;
static double dTheta,dTheta1,dTheta2,dTheta3;
static double dw,dw1,dw2,dw3;

// system measures
static double Isx, Isy, theta, speed;

// control
static double u[2]={0.,0.};             // format u={Um, beta}
static double us[2]={0.,0.};    // format us={us_alfa, us_beta}

// output for EKF (voltages and measured currents, which are fed to KalmanObs)
static double KalmanObs[4]={0.,0.,0.,0.};      // usx, usy, Isx, Isy

// real-time
static double t=0.;

void set_parameters(double Rs0, double Ls0, double Fmag0, double Bf0, double p0, double kp0, double J0, double Uc0, double DT0, double dt0)
{
  int tmp_i;

  // simulator parameters setup
  Rs=Rs0;
  Ls=Ls0;
  Fmag=Fmag0;
  Bf=Bf0;
  p=p0;
  kp=kp0;
  J=J0;
  Ucn=600.;
  Uc=Uc0;
  DT=DT0;

  // control setup
  Urm_max=0.95;

  // simulator sampling - fixed setup
  h=dt0;
  h_reg=125e-6;         // fpwm = 4kHz
  h_reg_counter_mez=(int)(h_reg/h);         // emulation of operation of DSP timer
  h_reg_counter=h_reg_counter_mez;
  h_reg_real=h_reg_counter_mez*h;           // real sampling period

  // reset of the system state variables
  for (tmp_i=0;tmp_i<9;tmp_i++)
    x[tmp_i]=0.;

  // emulation of the first measure
  Isx=0.;Isy=0.;theta=x[3];speed=x[2];

// === init of particular modules of simulator ===
  // PWM init
  smer=-1; smer2=-1;
  citac=0;
  citac2=abs(0-(DT/h));
  citac_PR=h_reg_counter_mez;

  modulace=1;           // THIPWM
  if (modulace==1)
    U_modulace=Ucn/sqrt(3.);
  else
    U_modulace=Ucn/2.;

  // PMSM model init
  dIsx=0;dIsx1=0;dIsx2=0;dIsx3=0;dIsy=0;dIsy1=0;dIsy2=0;dIsy3=0;
  dTheta=0;dTheta1=0;dTheta2=0;dTheta3=0;
  dw=0;dw1=0;dw2=0;dw3=0;

  init_regulace(Ls,Fmag,kp,p,h_reg_real);
}


static void pwm(unsigned int mod)
// mod ... mod=0 - sinusoidal PWM; mod=1 - PWM with injected 3rd harmonic
{
  unsigned int i;
  double iabc[3], ur[3],ustr[3],ua,ub,uc;
  double dtr[3],dd[3];
  double Um, beta;
  double U3;
  double up, up2;

  Um=*u;
  beta=*(u+1);

  // emulation of carrier - timer
  up=((double)citac/citac_PR-0.5)*Ucn;
  up2=((double)citac2/citac_PR-0.5)*Ucn;

  iabc[0]=*x;
  iabc[1]=(-*x+sqrt(3.)**(x+1))/2.;
  iabc[2]=(-*x-sqrt(3.)**(x+1))/2.;

  if (mod==0)   // sin. PWM
  {
    ur[0]=Um*cos(beta);
    ur[1]=Um*cos(beta-2./3.*M_PI);
    ur[2]=Um*cos(beta+2./3.*M_PI);
  }
  else                  // PWM with injected 3rd harmonic
  {
    U3=0.17*cos(3.*beta);
    ur[0]=Um*(cos(beta)-U3);
    ur[1]=Um*(cos(beta-2./3.*M_PI)-U3);
    ur[2]=Um*(cos(beta+2./3.*M_PI)-U3);
  }

  for (i=0;i<3;i++)
  { dtr[i]=ubytek(fabs(iabc[i]));
    dd[i]=dtr[i]*.73;
  }

  // implementation of voltage drops and dead-times
  for (i=0;i<3;i++)
    if (iabc[i]>=0)
      if ((ur[i]>up) && (ur[i]>up2))
        ustr[i]=Uc/2-dtr[i];
      else
        ustr[i]=-(Uc/2+dd[i]);
    else
      if ((ur[i]<up) && (ur[i]<up2))
        ustr[i]=-(Uc/2-dtr[i]);
      else
        ustr[i]=Uc/2+dd[i];

// phase voltages
  ua=(2.*ustr[0]-ustr[1]-ustr[2])/3.;
  ub=(2.*ustr[1]-ustr[0]-ustr[2])/3.;
  uc=(2.*ustr[2]-ustr[0]-ustr[1])/3.;

// voltage vector in stationary reference frame (x,y)
  *us=(2.*ua-ub-uc)/3.;
  *(us+1)=(ub-uc)/sqrt(3.);

  // emulation of DSP timers
  if ((citac==citac_PR)||(citac==0)) smer*=-1;
  if ((citac2==citac_PR)||(citac2==0)) smer2*=-1;
  citac+=smer;
  citac2+=smer2;
}

static double ubytek(double I)
{
  unsigned int ii;
  double delta_u;

  ii=0;
  while ((*(va_char+ii)<I) && (ii<(pocet-1)))
    ii++;

  if (ii==(pocet-1))
    delta_u=*(va_char+ii+pocet);
  else
    if (ii==0)
      delta_u=0;
    else
      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));

  return delta_u;
}


static void pmsm_model(unsigned int mod)
// mod<5...Euler, mod>4 ... Adams of 4th order
{
  double usx, usy;

  usx=*us;
  usy=*(us+1);

  dIsx=-Rs/Ls*x[0]+Fmag/Ls*x[2]*sin(x[3])+usx/Ls;
  dIsy=-Rs/Ls*x[1]-Fmag/Ls*x[2]*cos(x[3])+usy/Ls;
  dTheta=x[2];

  if (J>0)
    dw=kp*p*p*Fmag/J*(x[1]*cos(x[3])-x[0]*sin(x[3]))-Bf/J*x[2]-p/J*x[8];
  else
    dw=0;

  // integration
  if (mod<5)  // Euler
  { x[0]+=dIsx*h;
    x[1]+=dIsy*h;
    x[2]+=dw*h;
    x[3]+=dTheta*h;
  }
  else                  // Adams (4th order)
  { x[0]+=h/24.*(55.*dIsx-59.*dIsx1+37.*dIsx2-9.*dIsx3);
    x[1]+=h/24.*(55.*dIsy-59.*dIsy1+37.*dIsy2-9.*dIsy3);
    x[2]+=h/24.*(55.*dw-59.*dw1+37.*dw2-9.*dw3);
    x[3]+=h/24.*(55.*dTheta-59.*dTheta1+37.*dTheta2-9.*dTheta3);
  }

  // saturation of theta to (-pi,pi)
  if (x[3]>M_PI) x[3]-=(2*M_PI);
  if (x[3]<-M_PI) x[3]+=(2*M_PI);

  // diff. shift - Adams
  dIsx3=dIsx2;dIsx2=dIsx1;dIsx1=dIsx;
  dIsy3=dIsy2;dIsy2=dIsy1;dIsy1=dIsy;
  dTheta3=dTheta2;dTheta2=dTheta1;dTheta1=dTheta;
  dw3=dw2;dw2=dw1;dw1=dw;

  // calculation of Isd, Isq
  x[6]=x[0]*cos(x[3])+x[1]*sin(x[3]);         // Isd
  x[7]=x[1]*cos(x[3])-x[0]*sin(x[3]);         // Isq

  // Fsd ... d-component of stator flux
  x[5]=Ls*x[6]+Fmag;

  // Torque
  x[4]=kp*p*Fmag*(x[1]*cos(x[3])-x[0]*sin(x[3]));
}

//////////////////////////////////////////////////////////////////////////////////////////////////////
void eval(double Ww)            // you must link array KalmanObs[] to EKF modul
{
  double Umk, ua, ub;

//  while (t<=t_end)
  {
    pwm(modulace);
    pmsm_model(5);

    if (h_reg_counter>=h_reg_counter_mez)           // pocatek ISR
    {
      // voltages and measured currents for EKF
      Umk=*u*Uc/Ucn;
      ua=Umk*cos(*(u+1));
      ub=Umk*cos(*(u+1)-2./3.*M_PI);
      KalmanObs[0]=ua;                     // usx
      KalmanObs[1]=(ua+2.*ub)/sqrt(3.);    // usy
      KalmanObs[2]=Isx;
      KalmanObs[3]=Isy;

      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
                                                                                                                // rezim=2 ... Iqw=sqrt(Imax^2-Idw^2)
      // emulation of the real sampling of A/D converter
      Isx=x[0];Isy=x[1];speed=x[2];theta=x[3];

      h_reg_counter=0;
    }

    t+=h;
    h_reg_counter++;
  }
}
//////////////////////////////////////////////////////////////////////////////////////////////////////