00001
00013 #include <stat/loggers.h>
00014 #include "simulator.h"
00015 #include "pmsm.h"
00016
00018 class pmsmDS : public DS {
00019
00020 protected:
00022 int L_x, L_ou, L_oy, L_iu, L_optu;
00024 vec profileWw;
00026 double dt_prof;
00028 int Dt;
00031 string opt;
00032 public:
00034 pmsmDS (string opt0="" ) : Dt ( 125 ), opt(opt0) {}
00035 void set_parameters ( double Rs0, double Ls0, double Fmag0, double Bf0, double p0, double kp0, double J0, double Uc0, double DT0, double dt0 ) {
00036 pmsmsim_set_parameters ( Rs0, Ls0, Fmag0, Bf0, p0, kp0, J0, Uc0, DT0, dt0 );
00037 }
00038 void getdata ( vec &dt ) {dt=vec ( KalmanObs,6 );}
00039 void write ( vec &ut ) {}
00040
00041 void step() {
00042 static int ind=0;
00043 static double dW;
00044 static double Ww;
00045 if ( t>=dt_prof*ind ) {
00046 ind++;
00047 if ( ind<profileWw.length() ) {
00048
00049 if ( profileWw.length() ==1 ) {
00050 Ww=profileWw ( 0 ); dW=0.0;}
00051 else {
00052 dW = profileWw ( ind )-profileWw ( ind-1 );
00053 dW *=125e-6/dt_prof;
00054 }
00055 }
00056 else {
00057 dW = 0;
00058 }
00059 }
00060 Ww += dW;
00061
00062 for ( int i=0;i<Dt;i++ ) { pmsmsim_step ( Ww );}
00063 };
00064
00065 void log_add ( logger &L ) {
00066 L_x = L.add ( rx, "x" );
00067 L_oy = L.add ( ry, "obs" );
00068 L_ou = L.add ( ru, "obs" );
00069 L_iu = L.add ( ru, "true" );
00070
00071 if (opt.find("u")==string::npos){
00072 L_optu = L.add(ru, "model");
00073 }
00074 }
00075
00076 void logit ( logger &L ) {
00077 L.logit ( L_x, vec ( x,4 ) );
00078 L.logit ( L_oy, vec_2 ( KalmanObs[2],KalmanObs[3] ) );
00079 L.logit ( L_ou, vec_2 ( KalmanObs[0],KalmanObs[1] ) );
00080 L.logit ( L_iu, vec_2 ( KalmanObs[4],KalmanObs[5] ) );
00081 if (opt.find("u")==string::npos){
00082 double sq3=sqrt(3.0);
00083 double ua,ub;
00084 double i1=x[0];
00085 double i2=0.5*(-i1+sq3*x[1]);
00086 double i3=0.5*(-i1-sq3*x[1]);
00087 double u1=KalmanObs[0];
00088 double u2=0.5*(-u1+sq3*KalmanObs[1]);
00089 double u3=0.5*(-u1-sq3*KalmanObs[1]);
00090
00091 double du1=0.7*(double(i1>0.1) - double(i1<-0.1))+0.05*i1;
00092 double du2=0.7*(double(i2>0.1) - double(i2<-0.1))+0.05*i2;
00093 double du3=0.7*(double(i3>0.1) - double(i3<-0.1))+0.05*i3;
00094 ua = (2.0*(u1-du1)-(u2-du2)-(u3-du3))/3.0;
00095 ub = ((u2-du2)-(u3-du3))/sq3;
00096 L.logit( L_optu , vec_2 (ua,ub));
00097 }
00098 }
00099
00100 void set_profile ( double dt, const vec &Ww ) {dt_prof=dt; profileWw=Ww;}
00101 };