Changeset 1398 for applications/pmsm

Show
Ignore:
Timestamp:
10/03/11 23:35:36 (13 years ago)
Author:
vahalam
Message:
 
Files:
1 modified

Legend:

Unmodified
Added
Removed
  • applications/pmsm/pmsm_ctrl.h

    r1391 r1398  
    14401440        int biver; 
    14411441         
     1442        double bcbv0; 
     1443         
     1444        mat ovA, ovC, ovP, ovQ, ovR; 
     1445        double varome; 
     1446         
    14421447// inj 
    14431448        double injkon, injome, injphi; 
     
    14491454                                uab(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0), 
    14501455                                Ptp(2,2),Kt(2,2),Ared(2,2),Cred(2,2),Qred(2,2),Rred(2,2), 
    1451                                 Pt1(2,2),Pt2(2,2),Pt3(2,2),Pt4(2,2),Pt5(2,2), varPth(5),                                 
     1456                                Pt1(2,2),Pt2(2,2),Pt3(2,2),Pt4(2,2),Pt5(2,2), varPth(5), 
     1457                                ovA(4,4),       ovC(2,4), ovP(4,4), ovQ(4,4), ovR(2,2),                  
    14521458                                /*Cwq(3.0, 3.0*0.000125/0.1, -1200, 1200),  
    14531459                                Cuq(20.0, 20.0*0.000125/0.005, -1200, 1200), 
     
    14881494                 
    14891495                srand(time(NULL)); 
     1496                 
     1497                bcbv0 = bcbv; 
     1498                 
     1499                ovP=eye(4); 
     1500                ovA.zeros(); 
     1501                ovC.zeros(); 
     1502                ovQ.zeros(); 
     1503                ovR.zeros(); 
     1504                ovA(0,0) = ovA(1,1) = a; 
     1505                ovA(2,2) = d; 
     1506                ovA(3,3) = 1.0; 
     1507                ovA(3,2) = Dt; 
     1508                ovC(0,0) = ovC(1,1) = 1.0; 
     1509                ovR(0,0) = ovR(1,1) = 0.00001; 
     1510                ovQ(0,0) = ovQ(1,1) = 0.1; 
     1511                ovQ(2,2) = 0.1; 
     1512                ovQ(3,3) = 0.00001; 
     1513                varome = 1.0; 
    14901514        } 
    14911515         
     
    14961520        virtual vec ctrlaction(const itpp::vec& cond) { 
    14971521                PMSMCtrl::ctrlaction(cond); // fills isa,isb,ome,the,Ww 
    1498                                  
     1522                                                 
    14991523                vec udq(2); 
    15001524                                 
     
    15021526                double Isq = isb*cos(the)-isa*sin(the); 
    15031527                 
    1504                 double Iqw=(Cwq.ctrlaction(vec_1(Ww-ome)))(0); // conversion from vec 
     1528                double www = Ww; 
     1529                if(biver == 12){ 
     1530                        www += (double)rand() / (double)RAND_MAX * 3.0 - 1.5; 
     1531                } 
     1532                 
     1533                double Iqw=(Cwq.ctrlaction(vec_1(www-ome)))(0); // conversion from vec 
    15051534                 
    15061535                udq(0) = (Cud.ctrlaction(vec_1(-Isd)))(0); 
     
    15151544                 
    15161545                 
    1517                 //bicriterial///////////////////////////////////////////////////////////// 
     1546                //bicriterial/////////////////////////////////////////////////////////////               
     1547                 
     1548                 
    15181549                //verze 1: signum 
    15191550                if(biver == 1){ 
     
    16231654          //verze 4: 5KF           
    16241655          if((biver == 4)||(biver == 40)){ 
    1625                         double ia = a*isa + b*ome*sin(the) + c*(uab(0) + bcbv); 
    1626                         double ib = a*isb - b*ome*cos(the) + c*(uab(1) + bcbv);     
     1656                 
     1657                 
     1658                ovA(0,2) = b*sin(the); 
     1659                ovA(0,3) = b*ome*cos(the); 
     1660                ovA(1,2) = -b*cos(the); 
     1661                ovA(1,3) = b*ome*sin(the); 
     1662                ovA(2,0) = -e*sin(the); 
     1663                ovA(2,1) = e*cos(the); 
     1664                ovA(2,3) = -e*(isb*sin(the)+isa*cos(the));               
     1665                mat ovPp = ovA*ovP*ovA.T() + ovQ; 
     1666                ovP = (eye(4) - ovPp*ovC.T()*inv(ovC*ovPp*ovC.T() + ovR )*ovC)*ovPp; 
     1667                varome = ovP(2,2); 
     1668                 
     1669                //bcbv = (sqrt( (ome)*(ome) + bcbv0*bcbv0 ) - abs(ome)); 
     1670                //double tmp = (ome-Ww)*(ome-Ww) + bcbv0*bcbv0*e*e*c*c*0.5*(1.0+(varome-10.0)/(4.0)); 
     1671                //if(tmp < 0.0) tmp = 0.0; 
     1672                //bcbv = (-abs(ome-Ww) + sqrt( tmp )) /e/c; 
     1673                           
     1674                        double ia = a*isa + b*ome*sin(the) + c*(uab(0) + bcbv0); 
     1675                        double ib = a*isb - b*ome*cos(the) + c*(uab(1) + bcbv0);     
    16271676                        double om = d*ome + e*(isb*cos(the) - isa*sin(the)); 
    16281677                        double th = the + Dt*ome;                
     
    16381687                        if(biver == 40) varPth(0) = Pt1(0,0)*Pt1(1,1) - Pt1(0,1)*Pt1(1,0); 
    16391688                 
    1640                         ia = a*isa + b*ome*sin(the) + c*(uab(0) + bcbv); 
    1641                         ib = a*isb - b*ome*cos(the) + c*(uab(1) - bcbv);                                 
     1689                        ia = a*isa + b*ome*sin(the) + c*(uab(0) + bcbv0); 
     1690                        ib = a*isb - b*ome*cos(the) + c*(uab(1) - bcbv0);                                
    16421691                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                                
    16431692                        Ptp = Ared*Pt2*Ared.T() + Qred; 
     
    16471696                        if(biver == 40) varPth(1) = Pt2(0,0)*Pt2(1,1) - Pt2(0,1)*Pt2(1,0); 
    16481697                 
    1649                         ia = a*isa + b*ome*sin(the) + c*(uab(0) - bcbv); 
    1650                         ib = a*isb - b*ome*cos(the) + c*(uab(1) + bcbv);                                 
     1698                        ia = a*isa + b*ome*sin(the) + c*(uab(0) - bcbv0); 
     1699                        ib = a*isb - b*ome*cos(the) + c*(uab(1) + bcbv0);                                
    16511700                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                                
    16521701                        Ptp = Ared*Pt3*Ared.T() + Qred; 
     
    16561705                        if(biver == 40) varPth(2) = Pt3(0,0)*Pt3(1,1) - Pt3(0,1)*Pt3(1,0); 
    16571706                 
    1658                         ia = a*isa + b*ome*sin(the) + c*(uab(0) - bcbv); 
    1659                         ib = a*isb - b*ome*cos(the) + c*(uab(1) - bcbv);                                 
     1707                        ia = a*isa + b*ome*sin(the) + c*(uab(0) - bcbv0); 
     1708                        ib = a*isb - b*ome*cos(the) + c*(uab(1) - bcbv0);                                
    16601709                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                                
    16611710                        Ptp = Ared*Pt4*Ared.T() + Qred; 
     
    17041753                                        break; 
    17051754                        } 
     1755                } 
     1756          //*/ 
     1757           
     1758          //verze 44: 5KF           
     1759          if((biver == 44)){ 
     1760                        double ia = a*isa + b*ome*sin(the) + c*(uab(0) + bcbv); 
     1761                        double ib = a*isb - b*ome*cos(the) + c*(uab(1) + bcbv);     
     1762                        double om = d*ome + e*(isb*cos(the) - isa*sin(the)); 
     1763                        double th = the + Dt*ome;                
     1764                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th)); 
     1765                        Cred(0,0) = b*sin(th); 
     1766                        Cred(0,1) = b*om*cos(th); 
     1767                        Cred(1,0) = -b*cos(th); 
     1768                        Cred(1,1) = b*om*sin(th);                
     1769                        Ptp = Ared*Pt1*Ared.T() + Qred; 
     1770                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);                         
     1771                        Pt2 = (eye(2) - Kt*Cred)*Ptp; 
     1772                        varPth(0) = Pt2(0,0)*Pt2(1,1) - Pt2(0,1)*Pt2(1,0); 
     1773                 
     1774                        ia = a*isa + b*ome*sin(the) + c*(uab(0) + bcbv); 
     1775                        ib = a*isb - b*ome*cos(the) + c*(uab(1) - bcbv);                                 
     1776                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                                
     1777                        Ptp = Ared*Pt1*Ared.T() + Qred; 
     1778                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred); 
     1779                        Pt2 = (eye(2) - Kt*Cred)*Ptp; 
     1780                        varPth(0) = Pt2(0,0)*Pt2(1,1) - Pt2(0,1)*Pt2(1,0); 
     1781                 
     1782                        ia = a*isa + b*ome*sin(the) + c*(uab(0) - bcbv); 
     1783                        ib = a*isb - b*ome*cos(the) + c*(uab(1) + bcbv);                                 
     1784                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                                
     1785                        Ptp = Ared*Pt1*Ared.T() + Qred; 
     1786                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred); 
     1787                        Pt2 = (eye(2) - Kt*Cred)*Ptp; 
     1788                        varPth(0) = Pt2(0,0)*Pt2(1,1) - Pt2(0,1)*Pt2(1,0); 
     1789                 
     1790                        ia = a*isa + b*ome*sin(the) + c*(uab(0) - bcbv); 
     1791                        ib = a*isb - b*ome*cos(the) + c*(uab(1) - bcbv);                                 
     1792                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                                
     1793                        Ptp = Ared*Pt1*Ared.T() + Qred; 
     1794                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred); 
     1795                        Pt2 = (eye(2) - Kt*Cred)*Ptp; 
     1796                        varPth(0) = Pt2(0,0)*Pt2(1,1) - Pt2(0,1)*Pt2(1,0); 
     1797                 
     1798                        ia = a*isa + b*ome*sin(the) + c*(uab(0)); 
     1799                        ib = a*isb - b*ome*cos(the) + c*(uab(1));                                
     1800                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                                
     1801                        Ptp = Ared*Pt1*Ared.T() + Qred; 
     1802                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred); 
     1803                        Pt2 = (eye(2) - Kt*Cred)*Ptp; 
     1804                        varPth(0) = Pt2(0,0)*Pt2(1,1) - Pt2(0,1)*Pt2(1,0); 
     1805                                 
     1806                        minindex = 0; 
     1807                        for(int i = 1; i < 5; i++){ 
     1808                                if(varPth(i) < varPth(minindex)){                                
     1809                                        minindex = i; 
     1810                                } 
     1811                        } 
     1812                 
     1813                        f << timeid << "\t" << minindex << endl; 
     1814                        timeid++; 
     1815                 
     1816                        switch(minindex){ 
     1817                                case 0: 
     1818                                        uab(0) += bcbv; 
     1819                                        uab(1) += bcbv; 
     1820                                        break; 
     1821                                case 1: 
     1822                                        uab(0) += bcbv; 
     1823                                        uab(1) -= bcbv; 
     1824                                        break; 
     1825                                case 2: 
     1826                                        uab(0) -= bcbv; 
     1827                                        uab(1) += bcbv; 
     1828                                        break; 
     1829                                case 3: 
     1830                                        uab(0) -= bcbv; 
     1831                                        uab(1) -= bcbv; 
     1832                                        break; 
     1833                                case 4:                          
     1834                                        break; 
     1835                        } 
     1836                         
     1837                        ia = a*isa + b*ome*sin(the) + c*(uab(0)); 
     1838                        ib = a*isb - b*ome*cos(the) + c*(uab(1));                                
     1839                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                                
     1840                        Ptp = Ared*Pt1*Ared.T() + Qred; 
     1841                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred); 
     1842                        Pt1 = (eye(2) - Kt*Cred)*Ptp; 
    17061843                } 
    17071844          //*/ 
     
    18441981                Cud.set_params(Pd,Pi); 
    18451982                Cuq.set_params(Pd,Pi); 
    1846  
     1983                 
     1984                bcbv0 = bcbv; 
    18471985        } 
    18481986 
     
    19012039        void log_register ( logger &L, const string &prefix ) { 
    19022040                PMSMCtrl::log_register(L,prefix); 
    1903                 L.add_vector ( log_level, logModel, RV ( 1), prefix ); 
     2041                L.add_vector ( log_level, logModel, RV ( 2), prefix ); 
    19042042        } 
    19052043        void log_write() const{ 
    19062044                PMSMCtrl::log_write(); 
    1907                 log_level.store( logModel , double(minindex));  
     2045                vec tmp(2); 
     2046                tmp(0) = double(minindex); 
     2047                tmp(1) = double(bcbv); 
     2048//              tmp(1) = double(varome); 
     2049                log_level.store( logModel , tmp);  
    19082050                 
    19092051        }