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

        Vektorove rizeni

        regulacni struktura - plovouci radova carka

        Z. Peroutka

Rev. 15.3.2008

5.6.2007        Doplnen vypocet uhlu beta, ktery doplnuje regulator Isq

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

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


void init_regulace(double *param, 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, unsigned int rezim);
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 void reset_regulatoru(void);

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 Kiqmax, Iqwmax, Iqw_reg;
static double Idw_urm, Idw_reg;

// regulator rychlosti
static double Sw, MAXw_lim;

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 ladeni_regulace[10];

double Treg;

static unsigned int start_reg;
static double Ucf, Isdf, Ukomp;
static double zbeta, zbeta_vyp, zbetaw;		// zatezny uhel vyhodnoceny z polohy pozadovaneho napeti

static unsigned int ALGORITMUS;

// regulator Isq pres zatezny uhel
static double Sbeta;

// regulator Isd - kompenzace vypocteneho napeti
static double SUkomp;

// pomocne promenne
static unsigned int blokace_beta=0;

/////////////////// 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 *param, double TV)
{
  double Kpd;   // pomocna velicina
  double Rs, Ls, kp, p;

  // parametry pro odvazbeni
  Rs=*(param+0);
  Ls=*(param+1);
  Fmag=*(param+2);
  kp=*(param+5);
  p=*(param+4);
  // vyhodit az sem v DSP ////////////////////

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

  // regulator odbuzovani
//  Kpd=Kpurm*2.0/Iref;      // 2.0, protoze Urm je ladeno v pomernych hodnotach Um*2/Ucn a tady je to vztazeno na Ucn
//  Kiurmf=prevod((Kpd*TV/Tiurm),Q_Kiurm);

//  Kt_urm=prevod(TV/Tfurm,Q_Kturm);   // casova konstanta filtru Urm

  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;

  // vypocet modelu
  K_Fs=Ls;
  K_moment=kp*p*Fmag;

  Treg=TV;

  // pocatecni ALGORITMUS
  ALGORITMUS=0;		// implicitne se pouzije klasicke vektorove rizeni

  // regulator Isq prostrednictvim beta
  zbeta=0.;
  Sbeta=0.;
  
  // regulator Isd - kompenzace vypocteneho napeti
  SUkomp=0.;

  // start regulace
  start_reg=1;			// indikace 1. pruchodu regulacni smyckou kvuli nastaveni spravnych hodnot filtru Uc a Isd
}

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);

  // filtrace napeti ss obvodu
  if (start_reg==1)
  { 
    Ucf=Uc;
    Isdf=Isd;
    start_reg=0;
  }
  else
  {
    filtr(Uc,&Ucf,Treg/Tfuc);
    filtr(Isd,&Isdf,Treg/Tfid);
  } 

  // vyber varianty regulace
  if (fabs(rychlost)>prechod_1_2) ALGORITMUS=1;	// prechod z algoritmu 1 na 2
  if (fabs(rychlost)<prechod_2_1) ALGORITMUS=0;	// prechod z algortimu 2 na 1
  ALGORITMUS=0;         // provozovana pouze definovana varianta algoritmu rizeni

  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)

////////////// REGULACE - VARIANTA 1 - klasicke vektorove rizeni ///////////////////
if (ALGORITMUS==0) 
{
  // Regulator odbuzovani
  Idw_urm=pi_reg(Urm_max-Urmf,Kpurm,Kpurm*Treg/Tiurm,0.,MINurm,&Surm);

  // regulace proudu Id, Iq
//  Idw_urm = Idw;      // vyrazena regulace Urm
//  Idw_urm=0;
  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 (MAXw>Iqwmax) MAXw_lim=Iqwmax;else MAXw_lim=MAXw;

  // regulace rychlosti
//  Iqw_reg=pi_reg(Ww-rychlost,Kpw,Kpw*Treg/Tiw,MAXw_lim,-MAXw_lim,&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)
*/
  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 zatezneho uhlu z pozadovaneho napeti
  if (rychlost>=0)
    zbetaw=alfa-M_PI/2.;
  else
  { zbetaw=alfa+M_PI/2.;
    if (ud<0) zbetaw-=2.*M_PI;
  }

  Sbeta=zbetaw;		// inicializace regulatoru Isq prostednictvim beta - sumace pri prepnuti nastavena na aktualni hodnotu zatezneho uhlu!
}
///////////////////// konec kodu VARIANTY 1 //////////////////////////
else 	// (ALGORITMUS == 1)	... regulace zatezneho uhlu
{
///////////////// KOD VARIANTA 2 - regulace zatezneho uhlu ///////////
  // omezeni max. momentu (resp. max. Iqw) s ohledem na max. proud
  tmp_omezeni=Ismaxf2-Isdf*Isdf;
  if (tmp_omezeni<0) Iqwmax=0;
  else Iqwmax=sqrt(tmp_omezeni);
  if (MAXw>Iqwmax) MAXw_lim=Iqwmax;else MAXw_lim=MAXw;
  
  // regulace rychlosti
/*  Iqw_reg=pi_reg(Ww-rychlost,Kpw,Kpw*Treg/Tiw,MAXw_lim,-MAXw_lim,&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)
*/
  if (Iqw_reg>Iqwmax) Iqw_reg=Iqwmax;
  if (Iqw_reg<-Iqwmax) Iqw_reg=-Iqwmax;
  
//  Iqw_reg=Iqw;		// bez omezovani Isqw
  // regulace Isq prostrednictvim zatezneho uhlu
  zbeta=pi_reg(Iqw_reg-Isq,Kpib,Kpib*Treg/Tiib,MAXbeta,-MAXbeta,&Sbeta);

  // test upravy signalu beta
//if ((zbeta>73./180.*M_PI)||(blokace_beta==1)) {zbeta=80./180.*M_PI; blokace_beta=1;}

   // vypocet napeti - jednoducha varianta bez uvazovani odbuzeni
  Um=sqrt(Kodv_uind*Kodv_uind*rychlost*rychlost+Kodv_ud*Kodv_ud*rychlost*rychlost*Iqw_reg*Iqw_reg);     // rychlost musi byt v kvadratu, aby se eliminoval vliv znamenka rychlosti - Um musi byt >=0!
//  Um=rychlost*sqrt(Kodv_uind*Kodv_uind+Kodv_ud*Kodv_ud*Iqw*Iqw);	// neni uvazovano omezeni Isqw

  // vylepseny vypocet napeti - odvazbovaci obvod
/*  ud=Kodv_ud*rychlost*Iqw_reg;
  uq=Kodv_ud*rychlost*Isdf+Kodv_uind*rychlost;
  Um=sqrt(ud*ud+uq*uq);
*/
  
  // korece napeti pomoci regulatoru Isd
  Ukomp=pi_reg(0.-Isd,0,1.*Treg/Tiidb,MAXud,MINud,&SUkomp);		// do zaporu neomezovat!!!  
  Um+=Ukomp;

  // zahrnuti vlivu skutecneho napeti Uc
  Um=Um*Ucn/Ucf;		// v pevne radove carce pak: Um=(long)Um*Qm/Ucf;

  // omezeni na Urm=1
  if (Um>Ucn_2) Um=Ucn_2;

  reset_regulatoru();	// zajistuje pripravenost reg. struktury VAR1

  // doplneni vypoctu zatezneho uhlu ... zbeta_vyp
  if (fabs(rychlost*Kodv_ud*Iqw_reg)>=Um)
    if (Iqw_reg>0)
      if (rychlost>0) zbeta_vyp=M_PI/2.;
      else zbeta_vyp=-M_PI/2.;
    else
      if (rychlost>0) zbeta_vyp=-M_PI/2.;
      else zbeta_vyp=M_PI/2.;
  else
    zbeta_vyp=asin(rychlost*Kodv_ud*Iqw_reg/Um);        // kontrola, zda Kodv_ud=Ls!

//  zbeta=0;      // vyrazeni reg. Isq
  zbeta_vyp=0;  // vyrazeni bloku "vypocet zatezneho uhlu"
  // urceni pozadovaneho zatezneho uhlu a jeho omezeni na <-90st; 90st.>
  if (rychlost<0)
    zbetaw=-zbeta_vyp+zbeta;
  else
    zbetaw=zbeta_vyp+zbeta;

  if (zbetaw>MAXbeta)
    zbetaw=MAXbeta;
  if (zbetaw<-MAXbeta)
    zbetaw=-MAXbeta; /**/

  // urceni polohy vektoru napeti v souradnem systemu (d,q)
  if (rychlost<0)
    alfa=zbetaw-M_PI/2.;
  else
    alfa=zbetaw+M_PI/2.;
}
///////////////////// konec kodu VARIANTY 2 //////////////////////////

// KOD SPOLECNY PRO OBE REGULACNI STRUKTURY
  // vypocet polohy vektoru napeti ve stojicim souradnem systemu
  beta=alfa+theta;

  // vypocet Urmf pro dalsi periodu vzorkovani
  filtr(Um/Ucn_2,&Urmf,Treg/Tfurm);
  
  // vypocet velikosti toku a momentu
  Fs=Fmag+K_Fs*Isd;
  moment=K_moment*Isq;

  // test filtru	- umisteni filtru nema vubec vliv na kvalitu regulace algoritmy var. 2
/*  if (start_reg==0)
  {
	filtr(Uc,&Ucf,Treg/Tfuc);
	filtr(Isd,&Isdf,Treg/Tfid);
  } */

/////// PRECHOD ZPET DO SIMULACE //////////////

// Odblokovat pri pouziti PWM_FULL_NEW()
//  *u=ud*cos(theta)-uq*sin(theta);
//  *(u+1)=ud*sin(theta)+uq*cos(theta);
// Odblokovat pri pouziti PWM_FULL_NEW_3h()
  *u=Um;
  *(u+1)=beta;

  ladeni_regulace[0]=Idw_urm;
  ladeni_regulace[1]=Urmf;
  ladeni_regulace[2]=Iqw_reg;
  ladeni_regulace[3]=Um;
  ladeni_regulace[4]=Fs;
  ladeni_regulace[5]=moment;
  ladeni_regulace[6]=zbetaw;
  ladeni_regulace[7]=Um/Ucn_2;
  ladeni_regulace[8]=Ukomp;
  ladeni_regulace[9]=ALGORITMUS;
}

void reset_regulatoru(void)
{
  Surm=0;
  Sid=0;
  Siq=0;
}