Changeset 1259

Timestamp:

12/08/10 19:47:57 (15 years ago)

Author:

vahalam

Message:

 

Files:

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

applications/pmsm/pmsm_ctrl.h

@@ -3 +3 @@
 
 #include <design/ctrlbase.h>
+#include <design/lq_ctrl.h>
 #include "pmsm.h"
+
+#include <fstream>
 
 /*! \defgroup PMSM 
@@ -148 +151 @@
 UIREGISTER(PMSM_PICtrl);
 
+class PMSM_LQCtrl: 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
+        const 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;
+        vec icond;
+        
+//control maximum
+        const double MAXu;
+        
+//lqg controler class
+        LQG_universal lq;       
+        
+//prediction
+        vec p_isa, p_isb, p_ome, p_the;
+                
+        PMSM_LQCtrl():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), icond(6), MAXu(100.0),
+                                p_isa(rec_hor+1), p_isb(rec_hor+1), p_ome(rec_hor+1), p_the(rec_hor+1)
+        {                       
+                //set fix matrices elements & dimensions
+                A.zeros();
+                        A(0, 0) = A(1, 1) = a;
+                        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();
+                        //load input penalty r from file
+                        ifstream pcpfile;
+                        double pcp;
+                        
+                        pcpfile.open("pcp.txt");
+                        if(pcpfile){
+                                if(pcpfile >> pcp) r = pcp;     
+                                pcpfile.close();
+                        }
+                        R(0, 0) = R(1, 1) = r;
+                                                                
+                //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;
+                
+                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;
+                
+                lq.finalLoss.Q.setCh(Q);
+                lq.finalLoss.rv = RV("x", 5, 1);
+                
+                lq.validate();
+                                                
+                uab.zeros();
+        }
+                
+        
+        virtual vec ctrlaction(const itpp::vec& cond) {
+                PMSMCtrl::ctrlaction(cond); // fills isa,isb,ome,the,Ww
+                
+                int i;
+                lq.resetTime();
+                
+                //create prediction
+                p_isa.zeros();
+                p_isb.zeros();
+                p_ome.zeros();
+                p_the.zeros();
+                p_isa(0) = isa;
+                p_isb(0) = isb;
+                p_ome(0) = ome;
+                p_the(0) = the;
+                
+                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);
+                }
+                
+                //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)));
+                        
+                        lq.Models(0).A = A;             
+                        lq.redesign();
+                }
+                lq.redesign();
+                
+                L = lq.getL();
+                icond(0) = isa;
+                icond(1) = isb;
+                icond(2) = ome - Ww;
+                icond(3) = the;
+                icond(4) = Ww;
+                icond(5) = 0;
+                vec tmp = L*icond;                  
+                
+                uab = tmp(0,1);
+                
+                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;
+        };
+};
+UIREGISTER(PMSM_LQCtrl);
+
 /*!@}*/
 #endif //PMSM_CTR_H