Changeset 1403 for applications/pmsm

Show
Ignore:
Timestamp:
10/17/11 21:00:32 (13 years ago)
Author:
vahalam
Message:
 
Location:
applications/pmsm
Files:
2 modified

Legend:

Unmodified
Added
Removed
  • applications/pmsm/pmsm.h

    r1341 r1403  
    2020//      double Rs, Ls, dt, Ypm, kp, p,  J, Mz; 
    2121// }; 
     22 
     23inline double va_drop_interp(double i){ 
     24        //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}; 
     25        //* 
     26        double si = sign(i); 
     27        i = abs(i); 
     28        double du; 
     29                 
     30        if(i < 0.1) du = 5.0*i; 
     31     else if(i < 0.5) du = 1.0*(i-0.1) + 0.5; 
     32     else if(i < 1.0) du = 0.6*(i-0.5) + 0.9; 
     33     else if(i < 2.0) du = 0.4*(i-1.0) + 1.2; 
     34     else if(i < 3.0) du = 0.3*(i-2.0) + 1.6; 
     35     else if(i < 5.0) du = 0.25*(i-3.0) + 1.9;                           
     36     else du = 0.2*(i - 5.0) + 2.4; 
     37         
     38        return si*du*1.0; 
     39         
     40        /*/ 
     41        double msi = sign(i); 
     42         
     43        i = abs(i) - 0.3; 
     44         
     45        double si = sign(i); 
     46        i = abs(i); 
     47        double du; 
     48         
     49        if(i < 0.0654) du = 5.0*i; 
     50        else if(i < 0.3225) du = 1.0*(i-0.0654) + 0.3225; 
     51        else if(i < 0.654) du = 0.6*(i-0.3225) + 0.5805; 
     52        else if(i < 1.29) du = 0.4*(i-0.654) + 0.774; 
     53        else if(i < 1.935) du = 0.3*(i-1.29) + 1.032; 
     54        else if(i < 3.225) du = 0.25*(i-1.935) + 1.2255;                                 
     55        else du = 0.2*(i - 3.225) + 1.548; 
     56                 
     57        return msi*(si*du+0.558); 
     58        /*/ 
     59} 
    2260 
    2361//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$ 
     
    4482                double u2=0.5* ( -u1+sq3*u0(1) ); 
    4583                double u3=0.5* ( -u1-sq3*u0(1) ); 
    46  
     84                // 
     85                double du1=va_drop_interp(i1); 
     86                double du2=va_drop_interp(i2); 
     87                double du3=va_drop_interp(i3); 
     88                /*/ 
    4789                double du1=1.4* ( double ( i1>0.3 ) - double ( i1<-0.3 ) ) +0.2*i1; 
    4890                double du2=1.4* ( double ( i2>0.3 ) - double ( i2<-0.3 ) ) +0.2*i2; 
    4991                double du3=1.4* ( double ( i3>0.3 ) - double ( i3<-0.3 ) ) +0.2*i3; 
     92                */ 
    5093                ua = ( 2.0* ( u1-du1 )- ( u2-du2 )- ( u3-du3 ) ) /3.0; 
    5194                ub = ( ( u2-du2 )- ( u3-du3 ) ) /sq3; 
  • applications/pmsm/pmsm_ctrl.h

    r1398 r1403  
    99 
    1010#include <fstream> 
     11#include <sstream> 
    1112 
    1213/*! \defgroup PMSM  
     
    533534        mat L; 
    534535        vec uab;         
    535         vec icondpd; 
     536        //vec icondpd; 
    536537        vec u_old; 
    537538         
     
    549550                                r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10),                                  //for r is a default value rewrited by pcp.txt file value 
    550551                                A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4), 
    551                                 uab(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0)  {                        
     552                                uab(2), u_old(2), /*icondpd(8),*/ MAXu(100.0), MAXuflag(0)      {                        
    552553                //set fix matrices elements & dimensions 
    553554                A.zeros(); 
     
    574575                vec udq; 
    575576                vec tmp; 
     577                vec icondpd(8); 
    576578                 
    577579                lq.resetTime(); 
     
    722724        mat L; 
    723725        vec uab,udq; 
    724         vec icondpd; 
     726        //vec icondpd; 
    725727        vec u_old; 
    726728         
     
    739741                                r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10),                                  //for r is a default value rewrited by pcp.txt file value 
    740742                                A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4), 
    741                                 uab(2), udq(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0)  {                        
     743                                uab(2), udq(2), u_old(2), /*icondpd(8),*/ MAXu(100.0), MAXuflag(0)      {                        
    742744                //set fix matrices elements & dimensions 
    743745                A.zeros(); 
     
    763765                int i; 
    764766                vec tmp; 
     767                vec icondpd(8); 
    765768                 
    766769                lq.resetTime(); 
     
    948951        mat L; 
    949952        vec uab,udq; 
    950         vec icondpd; 
     953        //vec icondpd; 
    951954        vec u_old; 
    952955         
     
    971974                                r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10),                                  //for r is a default value rewrited by pcp.txt file value 
    972975                                A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4), //qqq(0.0), 
    973                                 uab(2), udq(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0), bcb(2), bcbv(0.0)       {                        
     976                                uab(2), udq(2), u_old(2), /*icondpd(8),*/ MAXu(100.0), MAXuflag(0), bcb(2), bcbv(0.0)   {                        
    974977                //set fix matrices elements & dimensions 
    975978                A.zeros(); 
     
    9991002                int i; 
    10001003                vec tmp; 
     1004                vec icondpd(8); 
    10011005                 
    10021006                lq.resetTime(); 
     
    11761180        mat L; 
    11771181        vec uab;         
    1178         vec icondpd; 
     1182        //vec icondpd; 
    11791183        vec u_old; 
    11801184         
     
    12031207                                r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10),                                  //for r is a default value rewrited by pcp.txt file value 
    12041208                                A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4), 
    1205                                 uab(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0), 
     1209                                uab(2), u_old(2), /*icondpd(8),*/ MAXu(100.0), MAXuflag(0), 
    12061210                                /*Cwq(3.0, 3.0*0.000125/0.1, -1200, 1200),  
    12071211                                Cuq(20.0, 20.0*0.000125/0.005, -1200, 1200), 
     
    12341238                vec udq; 
    12351239                vec tmp; 
     1240                vec icondpd(8); 
    12361241                 
    12371242                lq.resetTime(); 
     
    14091414        mat L; 
    14101415        vec uab;         
    1411         vec icondpd; 
     1416        //vec icondpd; 
    14121417        vec u_old; 
    14131418         
     
    14481453        double injkon, injome, injphi; 
    14491454        int minindex; 
     1455         
     1456// grid 
     1457        int gsize; //grid matrix size 
     1458        mat qloss, dloss, vomeg, vthet, vdet; //quadratic & dual loss, omega & theta variances, variance mat det 
     1459        double gstep; //grid step size 
     1460        double wwold; // old reference value 
     1461        double kvo; //dual loss koeficient 
     1462        int cntr; 
     1463        int mini, minj; 
    14501464                 
    14511465        PMSM_LQCtrl_bic3():PMSMCtrl(), a(0.9898), b(0.0072), c(0.0361), d(1.0), e(0.0149), 
    14521466                                r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10),                                  //for r is a default value rewrited by pcp.txt file value 
    1453                                 A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4), biver(0), 
    1454                                 uab(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0), 
     1467                                A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4), /*biver(0),*/ 
     1468                                uab(2), u_old(2), /*icondpd(8),*/ MAXu(100.0), MAXuflag(0), 
    14551469                                Ptp(2,2),Kt(2,2),Ared(2,2),Cred(2,2),Qred(2,2),Rred(2,2), 
    14561470                                Pt1(2,2),Pt2(2,2),Pt3(2,2),Pt4(2,2),Pt5(2,2), varPth(5), 
     
    14611475                                Cwq(10.0, 0.0005, -1200, 1200),  
    14621476                                Cuq(0.4, 0.0005, -1200, 1200), 
    1463                                 Cud(0.4, 0.0005, -1200, 1200)   {                        
     1477                                Cud(0.4, 0.0005, -1200, 1200), 
     1478                                gsize(21)       {                        
    14641479                //set fix matrices elements & dimensions 
    14651480                A.zeros(); 
     
    14781493                Rpd.zeros(); 
    14791494                u_old.zeros();   
     1495                uab.zeros(); 
    14801496                 
    14811497                Ared(0,0) = d; 
     
    14841500                Qred(0,0) = 0.1; 
    14851501                Qred(1,1) = 0.01;                
    1486                 Rred(0,0) = Rred(1,1) = 0.5;  
     1502                Rred(0,0) = Rred(1,1) = 0.05;  
    14871503                Pt1(0,0) = Pt2(0,0) = Pt3(0,0) = Pt4(0,0) = Pt5(0,0) = 1.0;      
    14881504                Pt1(1,1) = Pt2(1,1) = Pt3(1,1) = Pt4(1,1) = Pt5(1,1) = 1.0;      
    14891505                 
     1506                biver = 0; 
    14901507                timeid = 0;                      
    14911508                f.open("skf.dat", ios::out); 
     
    15071524                ovA(3,2) = Dt; 
    15081525                ovC(0,0) = ovC(1,1) = 1.0; 
    1509                 ovR(0,0) = ovR(1,1) = 0.00001; 
     1526                ovR(0,0) = ovR(1,1) = 0.05; 
    15101527                ovQ(0,0) = ovQ(1,1) = 0.1; 
    1511                 ovQ(2,2) = 0.1; 
    1512                 ovQ(3,3) = 0.00001; 
     1528                ovQ(2,2) = 0.01; 
     1529                ovQ(3,3) = 0.0001; 
    15131530                varome = 1.0; 
     1531                 
     1532                qloss.set_size(gsize,gsize);  
     1533                dloss.set_size(gsize,gsize);  
     1534                vomeg.set_size(gsize,gsize); 
     1535                vthet.set_size(gsize,gsize); 
     1536                vdet.set_size(gsize,gsize); 
     1537                 
     1538                gstep = 0.5; 
     1539                wwold = 0.0; 
     1540                kvo = 0.00001; 
     1541                cntr = 0; 
    15141542        } 
    15151543         
     
    16561684                 
    16571685                 
    1658                 ovA(0,2) = b*sin(the); 
     1686                /*ovA(0,2) = b*sin(the); 
    16591687                ovA(0,3) = b*ome*cos(the); 
    16601688                ovA(1,2) = -b*cos(the); 
     
    16651693                mat ovPp = ovA*ovP*ovA.T() + ovQ; 
    16661694                ovP = (eye(4) - ovPp*ovC.T()*inv(ovC*ovPp*ovC.T() + ovR )*ovC)*ovPp; 
    1667                 varome = ovP(2,2); 
     1695                varome = ovP(2,2);*/ 
    16681696                 
    16691697                //bcbv = (sqrt( (ome)*(ome) + bcbv0*bcbv0 ) - abs(ome)); 
     
    19241952          //*/ 
    19251953                 
     1954                //verze 13: grid 
     1955          if(biver == 13){ 
     1956                double ia,ib,om,th,ps; 
     1957                int hsize = floor(gsize/2); 
     1958                double dua, dub; 
     1959                double prww = 3.0*Ww - 2.0*wwold; 
     1960                int i,j; 
     1961                double omoo; 
     1962                double minloss = 1.0e300; 
     1963                mini=hsize; 
     1964                minj=hsize; 
     1965                mat ovPp; 
     1966                mat tmpP; 
     1967                                         
     1968                        for(i = 0; i < gsize; i++){ 
     1969                                for(j = 0; j < gsize; j++){ 
     1970                                        dua = (double)(i - hsize)*gstep; 
     1971                                        dub = (double)(j - hsize)*gstep;                                         
     1972                                         
     1973                                        ia = a*isa + b*ome*sin(the) + c*(uab(0) + dua); 
     1974                                        ib = a*isb - b*ome*cos(the) + c*(uab(1) + dub);     
     1975                                        om = d*ome + e*(isb*cos(the) - isa*sin(the)); 
     1976                                        th = the + Dt*ome;       
     1977                                        //ps = th + Dt*om;       
     1978                                        /*Ared(0,1) = -e*(ib*sin(th) + ia*cos(th)); 
     1979                                        Cred(0,0) = b*sin(th); 
     1980                                        Cred(0,1) = b*om*cos(th); 
     1981                                        Cred(1,0) = -b*cos(th); 
     1982                                        Cred(1,1) = b*om*sin(th);*/ 
     1983                                        Ared(0,0) = d+e*Dt*(ib*sin(th)+ia*cos(th)); 
     1984                                        Ared(0,1) = -e*(ib*sin(th)+ia*cos(th)); 
     1985                                        Ared(1,0) = Dt*(d+e*Dt*(ib*sin(th)+ia*cos(th))); 
     1986                                        Ared(1,1) = 1-e*Dt*(ib*sin(th)+ia*cos(th));                                      
     1987                                        Cred(0,0) = b*sin(th)-Dt*b*om*cos(th); 
     1988                                        Cred(0,1) = b*om*cos(th); 
     1989                                        Cred(1,0) = -b*cos(th)-Dt*b*om*sin(th); 
     1990                                        Cred(1,1) = b*om*sin(th);                
     1991                                        Ptp = Ared*Pt1*Ared.T() + Qred; 
     1992                                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);                         
     1993                                        Pt2 = (eye(2) - Kt*Cred)*Ptp; 
     1994                                         
     1995                                         
     1996                                        /*ovA(0,2) = b*sin(th); 
     1997                                        ovA(0,3) = b*om*cos(th); 
     1998                                        ovA(1,2) = -b*cos(th); 
     1999                                        ovA(1,3) = b*om*sin(th); 
     2000                                        ovA(2,0) = -e*sin(th); 
     2001                                        ovA(2,1) = e*cos(th); 
     2002                                        ovA(2,3) = -e*(ib*sin(th)+ia*cos(th));                   
     2003                                        ovPp = ovA*ovP*ovA.T() + ovQ; 
     2004                                        tmpP = (eye(4) - ovPp*ovC.T()*inv(ovC*ovPp*ovC.T() + ovR )*ovC)*ovPp; 
     2005                                        */ 
     2006                                        omoo = d*om + e*(ib*cos(th) - ia*sin(th)); 
     2007                                        qloss(i,j) = (omoo - prww)*(omoo - prww);                                        
     2008                                        vomeg(i,j) = /*tmpP(2,2);/*/Pt2(0,0); 
     2009                                        dloss(i,j) = qloss(i,j) + kvo*vomeg(i,j); 
     2010                                        vthet(i,j) = /*tmpP(3,3);/*/Pt2(1,1); 
     2011                                        vdet(i,j)  = /*det(tmpP);/*/Pt2(0,0)*Pt2(1,1) - Pt2(0,1)*Pt2(1,0); 
     2012                                         
     2013                                        if(vthet(i,j) < minloss){ 
     2014                                                minloss = vthet(i,j); 
     2015                                                mini = i; 
     2016                                                minj = j; 
     2017                                        } 
     2018                                } 
     2019                        }  
     2020                         
     2021                        if((timeid)<1000){ 
     2022                                stringstream ss; 
     2023 
     2024                                ss << "grid/qloss" << cntr << ".txt"; 
     2025                                f.open(ss.str().data(),ios::out); 
     2026                                for(i = 0; i < gsize; i++){ 
     2027                                        for(j = 0; j < gsize; j++){ 
     2028                                                f << (i - hsize)*gstep << "\t" << (j - hsize)*gstep << "\t" << qloss(i,j) << endl; 
     2029                                        } 
     2030                                        f << endl; 
     2031                                }        
     2032                                f.close(); 
     2033                                 
     2034                                ss.str(""); 
     2035                                ss << "grid/dloss" << cntr << ".txt"; 
     2036                                f.open(ss.str().data(),ios::out); 
     2037                                for(i = 0; i < gsize; i++){ 
     2038                                        for(j = 0; j < gsize; j++){ 
     2039                                                f << (i - hsize)*gstep << "\t" << (j - hsize)*gstep << "\t" << dloss(i,j) << endl; 
     2040                                        } 
     2041                                        f << endl; 
     2042                                }        
     2043                                f.close(); 
     2044                                 
     2045                                ss.str(""); 
     2046                                ss << "grid/vomeg" << cntr << ".txt"; 
     2047                                f.open(ss.str().data(),ios::out); 
     2048                                for(i = 0; i < gsize; i++){ 
     2049                                        for(j = 0; j < gsize; j++){ 
     2050                                                f << (i - hsize)*gstep << "\t" << (j - hsize)*gstep << "\t" << vomeg(i,j) << endl; 
     2051                                        } 
     2052                                        f << endl; 
     2053                                }        
     2054                                f.close(); 
     2055                                 
     2056                                ss.str(""); 
     2057                                ss << "grid/vthet" << cntr << ".txt"; 
     2058                                f.open(ss.str().data(),ios::out); 
     2059                                for(i = 0; i < gsize; i++){ 
     2060                                        for(j = 0; j < gsize; j++){ 
     2061                                                f << (i - hsize)*gstep << "\t" << (j - hsize)*gstep << "\t" << vthet(i,j) << endl; 
     2062                                        } 
     2063                                        f << endl; 
     2064                                }        
     2065                                f.close(); 
     2066                                 
     2067                                ss.str(""); 
     2068                                ss << "grid/vdet" << cntr << ".txt"; 
     2069                                f.open(ss.str().data(),ios::out); 
     2070                                for(i = 0; i < gsize; i++){ 
     2071                                        for(j = 0; j < gsize; j++){ 
     2072                                                f << (i - hsize)*gstep << "\t" << (j - hsize)*gstep << "\t" << vdet(i,j) << endl; 
     2073                                        } 
     2074                                        f << endl; 
     2075                                }        
     2076                                f.close();                               
     2077                                cntr++; 
     2078                        } 
     2079                         
     2080                        dua = (double)(mini - hsize)*gstep; 
     2081                        dub = (double)(minj - hsize)*gstep;                                      
     2082                                         
     2083                        uab(0) += dua; 
     2084                        uab(1) += dub; 
     2085                         
     2086                        //save best variance 
     2087                        ia = a*isa + b*ome*sin(the) + c*(uab(0)); 
     2088                        ib = a*isb - b*ome*cos(the) + c*(uab(1));                                
     2089                        //Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                      
     2090                        Ared(0,0) = d+e*Dt*(ib*sin(th)+ia*cos(th)); 
     2091                        Ared(0,1) = -e*(ib*sin(th)+ia*cos(th)); 
     2092                        Ared(1,0) = Dt*(d+e*Dt*(ib*sin(th)+ia*cos(th))); 
     2093                        Ared(1,1) = 1-e*Dt*(ib*sin(th)+ia*cos(th));                              
     2094                        Ptp = Ared*Pt1*Ared.T() + Qred; 
     2095                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred); 
     2096                        Pt1 = (eye(2) - Kt*Cred)*Ptp; 
     2097                        /*ovA(0,2) = b*sin(th); 
     2098                        ovA(0,3) = b*om*cos(th); 
     2099                        ovA(1,2) = -b*cos(th); 
     2100                        ovA(1,3) = b*om*sin(th); 
     2101                        ovA(2,0) = -e*sin(th); 
     2102                        ovA(2,1) = e*cos(th); 
     2103                        ovA(2,3) = -e*(ib*sin(th)+ia*cos(th));                   
     2104                        ovPp = ovA*ovP*ovA.T() + ovQ; 
     2105                        ovP = (eye(4) - ovPp*ovC.T()*inv(ovC*ovPp*ovC.T() + ovR )*ovC)*ovPp; 
     2106                   */ 
     2107                        timeid++;   
     2108                        wwold = Ww;  
     2109                } 
     2110          //*/ 
     2111                 
    19262112                ////////////////////////////////////////////////////////////////////////// 
    19272113                                 
     
    20282214                lq.validate(); 
    20292215                                                 
    2030                 uab.zeros(); 
     2216                 
    20312217                                 
    20322218                //create control matrix 
     
    20442230                PMSMCtrl::log_write(); 
    20452231                vec tmp(2); 
    2046                 tmp(0) = double(minindex); 
    2047                 tmp(1) = double(bcbv); 
     2232                tmp(0) = double(mini);//minindex); 
     2233                tmp(1) = double(minj);//bcbv); 
    20482234//              tmp(1) = double(varome); 
    20492235                log_level.store( logModel , tmp);