/********************************************

        Vector control of PMSM Drive

        Control - floating point

        Z. Peroutka

Rev. 16.3.2008

*********************************************/

#define _USE_MATH_DEFINES

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


void init_regulace(double Ls, double Fpm, double kp, double p, double TV);
void vektor_regulace(double Idw, double Iqw, double Urm_max, double Ww, double *u, double Isx, double Isy, double theta, double rychlost, double Ucn_2, double Uc, double Ucn, unsigned int rezim);

static double pi_reg(double epsilon, double Kpf, double Kif, double MAX, double MIN, double *S);
static double uhel(double x, double y);
static void filtr(double U, double *Uf, double kt);

// regulatory proudu
static double ud, uq, alfa, beta;
static double Sid, Siq, Surm;

// omezeni momentu
static double Iqwmax, Iqw_reg;
static double Idw_urm;

// regulator rychlosti
static double Sw;

static double U;
static double Um, Urmf;          // velikost napeti

// odvazbovaci obvod - blok vypocet napeti
static double Kodv_ud, Kodv_uind;

static double Isd, Isq, Fs, Fmag, moment, K_Fs, K_moment;
static double Ismaxf2, tmp_omezeni;

double Treg;

/////////////////// 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;
}


double pi_reg(double epsilon, double Kpf, double Kif, double MAX, double MIN, double *S)
{
  double out;

  out=Kpf*epsilon+*S;
  if (out>MAX) out=MAX;
  else if (out<MIN) out=MIN;
  else
    *S+=Kif*epsilon;

  return out;
}

void filtr(double U, double *Uf, double kt)
{
  double Ufpom;

  Ufpom=*Uf;
  *Uf=Ufpom+kt*(U-*Uf);
}

//////////////////////////////////////////////////////////////////////////


void init_regulace(double Ls, double Fpm, double kp, double p, double TV)
{
  double Kpd;   // pomocna velicina

  // parametry pro odvazbeni
  Fmag=Fpm;

  Sid=0;        // nulovani integracni slozky
  Siq=0;

  Surm=0;        // nulovani integracni slozky
  Urmf=0;        // filtrovane napeti Urm

  // omezeni momentu kvuli momentu zvratu - omezeni pomoci frmax
  Ismaxf2=Ismax*Ismax;

  // blok VYPOCET NAPETI
  Kodv_ud=Ls;
  Kodv_uind=Fmag;

  // regulator rychlosti
  Sw=0;

  Treg=TV;
}

void vektor_regulace(double Idw, double Iqw, double Urm_max, double Ww, double *u, double Isx, double Isy, double theta, double rychlost, double Ucn_2, double Uc, double Ucn, unsigned int rezim)
{
  // vypocet Isd, Isq
  Isd=Isx*cos(theta)+Isy*sin(theta);
  Isq=Isy*cos(theta)-Isx*sin(theta);

  Iqw_reg=pi_reg(Ww-rychlost,Kpw,Kpw*Treg/Tiw,MAXw,-MAXw,&Sw);
  if (rezim==0)
    Iqw_reg=Iqw;          // vyrazeni reg. rychlosti
  if (rezim==2)           // zkouska max. momentu
    Iqw_reg=Iqwmax;       // Iqw = sqrt(Imax^2-Idw^2)

  // Regulator odbuzovani
  Idw_urm=pi_reg(Urm_max-Urmf,Kpurm,Kpurm*Treg/Tiurm,0.,MINurm,&Surm);

  // regulace proudu Id, Iq
  ud=pi_reg(Idw_urm-Isd,Kpi,Kpi*Treg/Tii,MAXi,-MAXi,&Sid);

  // omezeni max. momentu (resp. max. Iqw) s ohledem na max. proud
  tmp_omezeni=Ismaxf2-Idw_urm*Idw_urm;
  if (tmp_omezeni<0) Iqwmax=0;
  else Iqwmax=sqrt(tmp_omezeni);

  if (Iqw_reg>Iqwmax) Iqw_reg=Iqwmax;
  if (Iqw_reg<-Iqwmax) Iqw_reg=-Iqwmax;
  uq=pi_reg(Iqw_reg-Isq,Kpi,Kpi*Treg/Tii,MAXi,-MAXi,&Siq);

///////// Vypocet napeti - "ODVAZBENI"
  ud-=Kodv_ud*rychlost*Iqw_reg;
  uq+=Kodv_ud*rychlost*Idw_urm+Kodv_uind*rychlost;

  // omezeni - saturace napeti - stejne omezeni jako u reg. proudu
  if (ud>MAXi) ud=MAXi;
  if (ud<-MAXi) ud=-MAXi;
  if (uq>MAXi) uq=MAXi;
  if (uq<-MAXi) uq=-MAXi;

  ////////////// KONEC ODVAZBENI /////////////////////////////////////////

  // velikost a poloha vektoru napeti
  Um=sqrt(ud*ud+uq*uq);
  alfa=uhel(ud,uq);

  // vypocet polohy vektoru napeti ve stojicim souradnem systemu
  beta=alfa+theta;

  // vypocet Urmf pro dalsi periodu vzorkovani
  filtr(Um/Ucn_2,&Urmf,Treg/Tfurm);

// saturation of modulation depth going to PWM
  if (Um>Ucn_2*Urm_max) Um=Ucn_2*Urm_max;  // linear modulation area only - ensure max. modulation depth

/////// PRECHOD ZPET DO SIMULACE //////////////
  *u=Um;
  *(u+1)=beta;

}