/* Hlavni simulacni program

   Simulace tramvaje s PMSM v plovouci radove carce

   Presne parametry tramvaje Skoda 15T

   31.5.2007

REV.   
31.5. 2007	Doplnena moznost provozu PWM se 3. harmonickou - konstanta MODULACE nastavuje typ modulace
25.6. 2007      Vyrazen ss obvod a trolej (uvazovano konst. napeti na troleji)       
*/

#include <stdio.h>
#include <math.h>
#include "pmsm_mod.h"
#include "pwm.h"
#include "regulace.h"
#include "mpf_double.h"

#define Qm      13
#define CHYBA_POLOHY	(0./180.*M_PI)	// chyba +/-8 stupnu elektrickych pro senzorovany loziskovy stit

#define MODULACE	0       // 0...sinusova PWM, 1...PWM se 3.harmonickou
#define REZIM_REGULACE  1       // 0...reg. momentu, 1...reg.rychlosti, 2... Isqw=sqrt(Imax^2-Id^2) - max. moment

static double param[7]={0.28,0.003465,0.1989,0.0,4,1.5,0.04};
// [Rs, Ls, Fmag, Bf, p, kp, J = 0.04?];
static double REL[6], REL1[6]={1.,1.,1.,1.,1.,1.};
// REL = [Ur, Ir, wr, thetar, Mr, Fr];
static double DT=3e-6;			// mrtve casy
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

// trolejovy filtr
//static double Rf=22e-3,Lf=1.8e-3,Cf=10e-3;	// filtr VEKTRA
static double Rf=5e-3,Lf=1e-3,Cf=1e-3;		// filtr 15T
// parametry ss obvodu
static double Ut,Uc,Ucn,It,Iz;
// modulace a skalarni rizeni
double fc,fr,fs;

double t, t_end, t_sense;
double h,hx,h_disp=0;
double h_reg,h_model,hpom,hpom_model;
unsigned int h_model_counter,h_model_counter_mez;

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

static int reg_counter=1, print_counter=0;
static double Idwf,Iqwf,Urm_max,Ww;

static double rychlost;

static double Isx, Isy;
static double Usx, Usy, ua, ub, Umk;
static double theta;
static double Uc_mer;

static int k_rampa=1, k_rampa_tmp=0;

static double tmp_uhel=0;

// EKF - vysledek estimace
static floatx mpf_estim[2]={0.,0.};     // w_est, theta_est
static floatx Esinth, Ecosth;

FILE *fw;

static void sim_init(void);

static void sim_init(void)
{
  // parametry troleje a modulator
  Ucn=600.;Ut=200.;Uc=Ut;It=0.;Iz=0.;
  fc=4000.;

  // vzorkovani
  h_reg=1./2./fc;
  h_model=h_reg;
  hpom=h;hpom_model=h;

  // vzorkovani pomoci citacu
  h_model_counter_mez=(int)(h_model/h);
  h_model_counter=h_model_counter_mez;

  fs=0.;					// zadani primo v Hz
//  fs=700.1*(param[4]/60.);	// zadani pres mechanicke otacky fs=n*pp/60

  // pocatecni stav motoru
  x[0]=0.;x[1]=0.;x[3]=0.;x[4]=0.;x[5]=0.;x[6]=0.;x[7]=0.;x[8]=0.;
  x[2]=2.*M_PI*fs;  // konstantni rychlost
  x[3]=M_PI/10.;

//  REL[0]=600; REL[1]=12.0*sqrt(2); REL[2]=2*M_PI*200; REL[3]=3.14159;
//  REL[4]=17.8;REL[5]=1.0;       // parametry pro v�kon PMSM 6kW!!!

  Isx=0.;Isy=0.;theta=x[3];rychlost=x[2];Uc_mer=Uc;
}

int main(void)
{
  h=1e-6;

  t_end=9.;    // profil Isq 5s, rozbeh 12s, rozbeh +/- 20s, reverzace 12s
  t_sense=0.0; // 1.
  t=h;

  sim_init();

//  pwm_full_new_init(h,fc,DT,0,1,1);
  pwm_full_new_init_3h(h,fc,DT,0,-1,-1);
  init_pmsm(param, REL1);
  init_regulace(param,h_reg);
  mpf_init(1e0, 1e-3, 1e0);//VS:

  fw=fopen("data/graf2.txt","w");

  Idwf=0.;
  Ww=0.;
  Iqwf=0.;

  while (t<=t_end)
  {
    pwm_full_new_3h(us,u,&Iz,Ucn,Uc,x,va_char,pocet,MODULACE);		// mod=0 ... sinusova PWM, mod=1 ... 3. harmonicka
    pmsm_double(*us,*(us+1),h,5);

    if (h_model_counter>=h_model_counter_mez)           // pocatek ISR
    {
      if (reg_counter>-1)                                // reg. smycka jede v kazdem ISR => reg_counter>-1, jinak jako u ASM reg_counter>1
      {
        if (t>.05)	// 0.05
        {
          // skokova zmena Isqw
//		  Iqwf=30.;
		  // zmena Isqw po rampa - profil
/*		  Iqwf+=k_rampa*0.0125;	// 0.055
		  if (Iqwf>30.) {Iqwf=30.; if (k_rampa_tmp<8000) k_rampa_tmp++; else {k_rampa=-1;k_rampa_tmp=0;}}
		  if (Iqwf<-30.) {Iqwf=-30.; if (k_rampa_tmp<8000) k_rampa_tmp++; else {k_rampa=1;k_rampa_tmp=0;}}
		  if ((t>3.) && (Iqwf>0)) Iqwf=0; /* konec profilu Isqw */
//		  if (Iqwf>0) Iqwf=0;

//          if (Ww<2.*M_PI*275.) Ww+=2.*M_PI*0.0125;
//		  if (Ww>-2.*M_PI*275.) Ww-=2.*M_PI*0.0125;
//		  Ww=2.*M_PI*180;
		// jednoducha reverzace - cely dej cca 11s
/* 	        Ww+=k_rampa*2.*M_PI*0.125/2.;    //1000Hz/s
		if (Ww>2.*M_PI*150.) {Ww=2.*M_PI*150.; if (k_rampa_tmp<8000) k_rampa_tmp++; else {k_rampa=-1;k_rampa_tmp=0;}}
		if (Ww<-2.*M_PI*150.) Ww=-2.*M_PI*150.;   /* */
/**/
          // skokova zmena napeti troleje
//          Ut=500;       // nutno zadat konstantni Isdw pri startu
        }


        if (t>0.2) x[8]=1.2;    // 1A
        if (t>0.4) x[8]=10.8;   // 9A
        if (t>0.6) x[8]=25.2;  // 21A

        if (t>0.7) Ww=2.*M_PI*10.;
//         if (t>1.0) x[8]=1.2;    // 1A
//         if (t>1.2) x[8]=10.8;   // 9A
//         if (t>1.4) x[8]=25.2;  // 21A

        if (t>1.6) Ww=2.*M_PI*50.;
//         if (t>1.9) x[8]=1.2;    // 1A
//         if (t>2.1) x[8]=10.8;   // 9A
//         if (t>2.3) x[8]=25.2;  // 21A

        if (t>2.5) Ww=2.*M_PI*100;
//         if (t>2.8) x[8]=1.2;    // 1A
//         if (t>3.0) x[8]=10.8;   // 9A
//         if (t>3.2) x[8]=25.2;  // 21A

        if (t>3.4) Ww=2.*M_PI*150;
//         if (t>3.7) x[8]=1.2;    // 1A
//         if (t>3.9) x[8]=10.8;   // 9A
//         if (t>4.1) x[8]=25.2;  // 21A

        if (t>4.3) Ww=2.*M_PI*0;
//         if (t>4.8) x[8]=-1.2;    // 1A
//         if (t>5.0) x[8]=-10.8;   // 9A
//         if (t>5.2) x[8]=-25.2;  // 21A

        if (t>5.4) Ww=2.*M_PI*(-10.);
//         if (t>5.7) x[8]=-1.2;    // 1A
//         if (t>5.9) x[8]=-10.8;   // 9A
//         if (t>6.1) x[8]=-25.2;  // 21A

        if (t>6.3) Ww=2.*M_PI*(-50.);
//         if (t>6.7) x[8]=-1.2;    // 1A
//         if (t>6.9) x[8]=-10.8;   // 9A
//         if (t>7.1) x[8]=-25.2;  // 21A

        if (t>7.3) Ww=2.*M_PI*(-100.);
//         if (t>7.7) x[8]=-1.2;    // 1A
//         if (t>7.9) x[8]=-10.8;   // 9A
//         if (t>8.1) x[8]=-25.2;  // 21A
//         if (t>8.3) x[8]=10.8;   // 9A
//         if (t>8.5) x[8]=25.2;  // 21A


        // letmy start s nenulovym momentem
//        Iqwf=149.*sqrt(2.);

		// pomal� rozjezd 10s do max. rychlosti 700rpm
/*     	        Iqwf=149.*sqrt(2.);     // konst. Isqw = Imax
                x[2]+=2.*M_PI*0.004;
		if (x[2]>1615) x[2]=1615;
                if (x[2]<-1615) x[2]=-1615;     /* osetreni pro test rozjezdu do zapornych otacek */

		// rampa rozjezd 5s do max. rychlosti 700rpm a 10s do -700rpm
/*     	        Iqwf=149.*sqrt(2.);     // konst. Isqw = Imax
                x[2]+=k_rampa*2.*M_PI*0.008;
		if (x[2]>1615) {x[2]=1615; if (k_rampa_tmp<24000) k_rampa_tmp++; else {k_rampa=-1;k_rampa_tmp=0;}}
		if (x[2]<-1615) x[2]=-1615;  /**/
			
       
      // KALMAN (pouzij stara Usx, Usy!!
	  if (0){
		mpf_bayes((floatx)Isx,(floatx)Isy,(floatx)Usx,(floatx)Usy);
		mpf_mean(&Esinth,&Ecosth,&mpf_estim[0]);
		mpf_estim[1]=atan2((floatx)Esinth,(floatx)Ecosth);
	  } else {
		  mpf_estim[0]=kalman_om((floatx)Isx,(floatx)Isy,(floatx)Usx,(floatx)Usy,(floatx)x[3]);
	  }

      Umk=*u*Uc/Ucn;
      ua=Umk*cos(*(u+1));
      ub=Umk*cos(*(u+1)-2./3.*M_PI);
      Usx=ua;                     // usx
      Usy=(ua+2.*ub)/sqrt(3.);    // usy
                // vystup z EKF zaveden do regulatoru
                // rychlost=ekf_estim[0]; theta=ekf_estim[1]; //VS

		Urm_max=1.0;

		// idealni vzorkovani - test vlivu
//		Isx=x[0];Isy=x[1];rychlost=x[2];theta=x[3];Uc_mer=Uc;

		if (MODULACE==0)	// sinusova PWM
		  vektor_regulace(Idwf,Iqwf,Urm_max,Ww,u,Isx,Isy,theta,rychlost,Ucn/2.,Uc_mer,Ucn,REZIM_REGULACE);	// rezim=1 ... reg. rychlosti, rezim=0 ... reg. momentu
													                // rezim=2 ... Iqw=sqrt(Imax^2-Idw^2)
		else
		  vektor_regulace(Idwf,Iqwf,Urm_max,Ww,u,Isx,Isy,theta,rychlost,Ucn/sqrt(3.),Uc_mer,Ucn,REZIM_REGULACE);// rezim=1 ... reg. rychlosti, rezim=0 ... reg. momentu
														        // rezim=2 ... Iqw=sqrt(Imax^2-Idw^2)

        reg_counter=0;
        Isx=x[0];Isy=x[1];rychlost=x[2];theta=x[3]+CHYBA_POLOHY;Uc_mer=Uc;       // re�ln� vzorkov�n�
      }
      hpom_model=0;
      h_model_counter=0;
      reg_counter++;
    }

    if (t>=t_sense)
      if (print_counter>125)//sampling period
      { fprintf(fw,"%f %f %f %f %f %f %f \n", t,x[0],x[1],Usx,Usy,mpf_estim[0],mpf_estim[1]); // t, Isx, Isy, Isd, Isdw, Isq, Isqw, M, Fs, poloha, rychlost, rychlost_w, Urm, Urm_max,
      // Fs_model, M_model, zatezny uhel (beta), zatezny uhel vypocteny z napeti (zbeta), Uc
        print_counter=0;
      }

    if (h_disp>0.2)
    { printf("%f \n",t);
      h_disp=0;
    }

    t+=h;
    hpom+=h;
    hpom_model+=h;
    h_model_counter++;
    h_disp+=h;
    print_counter++;
  }

//  printf("*** \n\n");
  fclose(fw);
	return 0;
 }