root/simulator_zdenek/ekf_example/simul.cpp @ 52

/* 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 "ekf.h"

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

#define MODULACE        1       // 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 theta;
static double Uc_mer;

static int k_rampa=1, k_rampa_tmp=0;

static double tmp_uhel=0;

// EKF - vysledek estimace
static double ekf_estim[2]={0.,0.};     // w_est, theta_est

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=5.;    // 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);
  init_ekf(h_reg,param);

  fw=fopen("data/graf1.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
        }

        // 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
                ekf(ekf_estim,*u,*(u+1),Ucn,Uc_mer,Isx,Isy);

                // 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>199)
      { fprintf(fw,"%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f \n",t,x[0],x[1],x[6],*ladeni_regulace,x[7],*(ladeni_regulace+2),x[4],x[5],x[3],x[2],Ww,*(ladeni_regulace+1),Urm_max,*(ladeni_regulace+4),*(ladeni_regulace+5),x[10],*(ladeni_regulace+6),Uc, *(ladeni_regulace+7),ladeni_pila,ladeni_ur,*(ladeni_regulace+8),*(ladeni_regulace+9),ekf_estim[0],ekf_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;
 }