/* model synchronniho motoru s permanentnimi magnety

   PMSM v plovouci radove carce - fyzikalni model!
   
   31.5.2007

   Upraven vypocet momentu M = Fmag*Isq!! (nikoliv M = Fs*Isq!!)
   31.5.2007	doplnen vypocet zatezneho uhlu (beta)
*/

#define _USE_MATH_DEFINES
#include <math.h>
#include "pmsm_mod.h"

double x[10]; // (isx,isy,wme,theta_e,M,Fsd,Isd,Isq,Mz,beta)		... beta = zatezny uhel

void init_pmsm(double *param, double *REL);
void pmsm_double(double usx, double usy, double h, unsigned int vypocet);

// pomocne funkce
static double uhel(double x, double y);

static double Isx,Isy,wme,Mz,M,Fs,Isd,Isq;
static double Rs,Ls,Fmag,Bfric,p,kp,J;
// Rs=odpor stator; Ls=statorova indukcnost (synchronni reaktance)
// p=pocet polparu, kp=konst. Parkovy transf.,
// J=moment setrvacnosti (J<0 ... nekonecny)

static double Uref, Iref, Wref, Mref, Fref, Thetaref; // referencni hodnoty

static double dIsx,dIsx1,dIsx2,dIsx3,dIsy,dIsy1,dIsy2,dIsy3;
static double dTheta,dTheta1,dTheta2,dTheta3;
static double dw,dw1,dw2,dw3;

static double A,B,C,D,E,F,G;

static double Fsq;


/////////////////// POMOCNE FUNKCE //////////////////////////////////
double uhel(double x, double y)
{
  double th;

  if (x==0)
    if (y==0) th=0.;
	else if (y>0) th=M_PI/2.;
	     else th=-M_PI/2.;
  else
    th=atan(y/x);

  if (x<0) th+=M_PI;

  return th;
}


void init_pmsm(double *param, double *REL)
{
  unsigned int i;

  Rs=*(param);
  Ls=*(param+1);
  Fmag=*(param+2);
  Bfric=*(param+3);

  p=*(param+4);
  kp=*(param+5);
  J=*(param+6);

  Uref=*REL;
  Iref=*(REL+1);
  Wref=*(REL+2);
  Thetaref=*(REL+3);
  Mref=*(REL+4);
  Fref=*(REL+5);

// definice konstant modelu
  A=Rs/Ls;
  B=Fmag/Ls*Wref/Iref;
  C=Uref/Iref/Ls;
  D=Bfric/J;
  E=kp*p*p*Fmag/J*Iref/Wref;
  F=p/J*Mref/Wref;
  G=Wref/Thetaref;

// definice pocatecnich hodnot
  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;
}

void pmsm_double(double usx, double usy, double h, unsigned int vypocet)
// vypocet<5...Euler, vypocet>4 ... Adams 4.radu
{
// (isx,isy,wme,theta_e,M,Fs,Mz)
  // vypocty diferenci
  dIsx=-A*x[0]+B*x[2]*sin(x[3]*Thetaref)+C*usx;
  dIsy=-A*x[1]-B*x[2]*cos(x[3]*Thetaref)+C*usy;
  dTheta=G*x[2];

  if (J>0)
    dw=E*(x[1]*cos(x[3]*Thetaref)-x[0]*sin(x[3]*Thetaref))-D*x[2]-F*x[8];
  else
    dw=0;

// integrace
  if (vypocet<5)  // Euler
  { x[0]+=dIsx*h;
    x[1]+=dIsy*h;
    x[2]+=dw*h;
    x[3]+=dTheta*h;
  }
  else			// Adams 4.radu
  { 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);
  }

  // uprava theta tak, aby bylo stale v rozmezi (-pi,pi)
  if (x[3]>M_PI/Thetaref) x[3]-=(2*M_PI/Thetaref);
  if (x[3]<-M_PI/Thetaref) x[3]+=(2*M_PI/Thetaref);

  // zamena diferenci
  dIsx3=dIsx2;dIsx2=dIsx1;dIsx1=dIsx;
  dIsy3=dIsy2;dIsy2=dIsy1;dIsy1=dIsy;
  dTheta3=dTheta2;dTheta2=dTheta1;dTheta1=dTheta;
  dw3=dw2;dw2=dw1;dw1=dw;

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

  // vypocet toku statoru Fsd - neni normovany!!
  x[5]=Ls*x[6]*Iref+Fmag;

  // Vypocet momentu
  x[4]=kp*p*Fmag*Iref/Mref*(x[1]*cos(x[3]*Thetaref)-x[0]*sin(x[3]*Thetaref));

  // vypocet zatezneho uhlu
  Fsq=Ls*x[7]*Iref;
  x[10]=uhel(x[5],Fsq);
}