Changeset 1289 for applications

Timestamp:

03/10/11 14:03:12 (15 years ago)

Author:

vahalam

Message:

 

Files:

• applications/pmsm/pmsm_ctrl.h (modified) (2 diffs)

applications/pmsm/pmsm_ctrl.h

@@ -408 +408 @@
                 
                 //create prediction
-                p_isa.zeros();
+                /*p_isa.zeros();
                 p_isb.zeros();
                 p_ome.zeros();
@@ -498 +498 @@
 UIREGISTER(PMSM_LQCtrl_dq);
 
+class PMSM_LQCtrl_dq2: public PMSMCtrl{
+public:
+/*
+PMSMCtrl:
+        double isa;
+        double isb;
+        double ome;
+        double the;
+        double Ww;
+*/      
+
+//PMSM parameters
+        const double a;
+        const double b;
+        const double c;
+        const double d;
+        const double e;
+        
+//input penalty
+        double r;
+        
+//time step
+        const double Dt;
+        
+//receding horizon
+        int rec_hor;
+        
+//system matrices
+        mat A; //5x5
+        mat B; //5x2
+        //mat C; //2x5
+        mat Q; //5x5
+        mat R; //2x2    
+        
+//control matrix
+        mat L;
+        vec uab,udq;
+        vec icond;
+        
+//control maximum
+        const double MAXu;
+        
+//lqg controler class
+        LQG_universal lq;       
+        
+//prediction
+        vec p_isd, p_isq, p_ome, p_the;
+                
+        PMSM_LQCtrl_dq2():PMSMCtrl(), a(0.9898), b(0.0072), c(0.0361), d(1.0), e(0.0149),
+                                r(0.005), Dt(0.000125), rec_hor(10),                                    //for r is a default value rewrited by pcp.txt file value
+                                A(5, 5), B(5, 2), Q(5, 5), R(2, 2),
+                                uab(2), udq(2), icond(6), MAXu(100.0)   {                       
+                //set fix matrices elements & dimensions
+                A.zeros();
+                        A(0, 0) = A(1, 1) = a;
+                        A(2, 1) = e;
+                        A(2, 2) = d;
+                        A(2, 4) = d - 1;
+                        A(3, 2) = A(3, 4) = Dt;
+                        A(3, 3) = A(4, 4) = 1.0;
+                B.zeros();
+                        B(0, 0) = B(1, 1) = c;
+                //C.zeros();
+                //      C(0, 0) = C(1, 1) = 1.0;
+                Q.zeros();
+                        Q(2, 2) = 1.0;
+                R.zeros();
+                                                                
+        }
+                
+        
+        virtual vec ctrlaction(const itpp::vec& cond) {
+                PMSMCtrl::ctrlaction(cond); // fills isa,isb,ome,the,Ww
+                
+                int i;
+                
+                lq.resetTime();
+        //cout << "OK" << endl;
+                //create prediction
+                p_isd.zeros();
+                p_isq.zeros();
+                p_ome.zeros();
+                p_the.zeros();
+                p_isd(0) = isa*cos(the) + isb*sin(the);//isa;
+                p_isq(0) = isb*cos(the) - isa*sin(the);//isb;
+                p_ome(0) = ome;
+                p_the(0) = the;
+        //cout << "OKkkk" << endl;      
+                for(i = 0; i < rec_hor-1; i++){
+                        /*p_isa(i+1) = a*p_isa(i) + b*p_ome(i)*sin(p_the(i)) + c*0;//uab(0); //use last used control
+                        p_isb(i+1) = a*p_isb(i) - b*p_ome(i)*cos(p_the(i)) + c*0;//uab(1);
+                        p_ome(i+1) = d*p_ome(i) + e*(p_isb(i)*cos(p_the(i)) - p_isa(i)*sin(p_the(i)));
+                        p_the(i+1) = p_the(i) + Dt*p_ome(i);*/
+                        p_isd(i+1) = a*p_isd(i) + p_isq(i)*p_ome(i)*Dt + c*udq(0);
+                        //cout << "OKaa" << endl;
+                        p_isq(i+1) = -p_isd(i)*p_ome(i)*Dt + a*p_isq(i) - b*p_ome(i) + c*udq(1);
+                        p_ome(i+1) = d*p_ome(i) + e*p_isq(i);
+                        p_the(i+1) = p_the(i) + Dt*p_ome(i);
+                        //cout << "1";
+                }
+                
+                //create control matrix
+                for(i = rec_hor; i > 0; i--){           
+                        //set variable matrices elements (A matrix only)
+                        /*A(0, 2) = b*sin(p_the(i));
+                        A(0, 3) = b*(p_ome(i))*cos(p_the(i));
+                        A(0, 4) = b*sin(p_the(i));
+                        A(1, 2) = -b*cos(p_the(i));
+                        A(1, 3) = b*(p_ome(i))*sin(p_the(i));
+                        A(1, 4) = -b*cos(p_the(i));
+                        A(2, 0) = -e*sin(p_the(i));
+                        A(2, 1) = e*cos(p_the(i));
+                        A(2, 3) = -e*(p_isa(i)*cos(p_the(i)) + p_isb(i)*sin(p_the(i)));*/
+                        A(0, 1) = Dt*p_ome(i);
+                        A(0, 2) = Dt*p_isq(i);
+                        A(0, 4) = Dt*p_isq(i);
+                        A(1, 0) = -Dt*p_ome(i);
+                        A(1, 2) = -Dt*p_isd(i);
+                        A(1, 4) = -Dt*p_isq(i);
+                        
+                        lq.Models(0).A = A;             
+                        lq.redesign();
+                }
+                lq.redesign();
+                
+                L = lq.getL();
+                //icond(0) = isa;
+                //icond(1) = isb;
+                icond(0) = isa*cos(the) + isb*sin(the);
+                icond(1) = isb*cos(the) - isa*sin(the);
+                icond(2) = ome - Ww;
+                icond(3) = the;
+                icond(4) = Ww;
+                icond(5) = 1;
+                vec tmp = L*icond;                  
+                
+                //uab = tmp(0,1);
+                udq = tmp(0,1);
+                
+                uab = udq; //set size
+                uab(0) = udq(0)*cos(the) - udq(1)*sin(the);
+                uab(1) = udq(1)*cos(the) + udq(0)*sin(the);
+                
+                if(uab(0) > MAXu) uab(0) = MAXu;
+                else if(uab(0) < -MAXu) uab(0) = -MAXu;
+                if(uab(1) > MAXu) uab(1) = MAXu;
+                else if(uab(1) < -MAXu) uab(1) = -MAXu;
+                
+                return uab;
+        };
+        void from_setting(const Setting &set){
+                PMSMCtrl::from_setting(set);
+                UI::get(r,set, "r", UI::optional);
+                UI::get(rec_hor,set, "h", UI::optional);
+        }
+
+        void validate(){
+                R(0,0)=R(1,1)=r;
+
+                p_isd.set_length(rec_hor+1);
+                p_isq.set_length(rec_hor+1);
+                p_ome.set_length(rec_hor+1);
+                p_the.set_length(rec_hor+1);
+
+                Array<quadraticfn> qloss(2);
+                qloss(0).Q.setCh(Q);
+                qloss(0).rv = RV("x", 5, 1);
+                qloss(1).Q.setCh(R);
+                qloss(1).rv = RV("u", 2, 0);            
+                lq.Losses = qloss;              
+
+                                //set lq
+                lq.rv = RV("u", 2, 0);                  
+                lq.set_rvc(RV("x", 5, 0));
+                lq.horizon = rec_hor;   
+                
+                Array<linfnEx> model(2);
+                model(0).A = A;
+                model(0).B = vec("0 0 0 0 0");
+                model(0).rv = RV("x", 5, 0);
+                model(0).rv_ret = RV("x", 5, 1);
+                model(1).A = B;
+                model(1).B = vec("0 0");
+                model(1).rv = RV("u", 2, 0);
+                model(1).rv_ret = RV("x", 5, 1);
+                lq.Models = model;
+                
+                lq.finalLoss.Q.setCh(Q);
+                lq.finalLoss.rv = RV("x", 5, 1);
+                
+                lq.validate();
+                                                
+                uab.zeros();
+                udq.zeros();
+
+        }
+};
+UIREGISTER(PMSM_LQCtrl_dq2);
+
 /*!@}*/
 #endif //PMSM_CTR_H