root/applications/pmsm/pmsm_ctrl.h @ 1259

#ifndef PMSM_CTR_H
#define PMSM_CTR_H

#include <design/ctrlbase.h>
#include <design/lq_ctrl.h>
#include "pmsm.h"

#include <fstream>

/*! \defgroup PMSM 
@{
*/

using namespace bdm;


/*! PI Controller*/
class PI_ctrl: public Controller{
public:
        //!integral state
        double S;
        double Pd;
        double Pi;
        double minOut;
        double maxOut;
        
        PI_ctrl(double Pd0, double Pi0, double minOut0, double maxOut0): S(0.0), Pd(Pd0), Pi(Pi0),minOut(minOut0),maxOut(maxOut0){}
        virtual vec ctrlaction ( const vec &cond ) {
                        double tmp;
                        tmp = Pd*cond(0)+S;
                        
                        S += Pi*cond(0);
                        
                        if (tmp>maxOut) tmp=maxOut;
                        if (tmp<minOut) tmp=minOut;
                        return vec_1(tmp);
        }
};

/*! \brief Root class for controllers of PMSM
 * 
 * It defines the interface for controllers that connect to the pmsmDSctrl. 
 * The adapt() function calls bayes() of an internal BM. Note that it is assumed that the estimator is compatible with PMSM system.
 * I.e. it must have rv={ia, ib} and rvc={ua,ub}, any other BM will not work.
 * 
 */
class PMSMCtrl: public Controller{
protected:
        //! Est where rv and rcv are not used!
        shared_ptr<BM> Est;
        //! switch to use or not to use the internal estimator
        bool estim;
        
        // internal quantities filled by PMSMCtrl::ctrlaction()
        double isa;
        double isb;
        double ome;
        double the;
        double Ww;
        
public:

        PMSMCtrl():Controller() {
                rv = RV("{ua ub }");
                rvc = RV("{o_ia o_ib o_ua o_ub o_om o_th Ww }");
        }
        
    void adapt(const itpp::vec& data){
                if (estim){
                        vec y=data.get(0,1);
                        vec u=data.get(2,3);
                        Est->bayes(y,u);
                }
        }
        
        
    virtual vec ctrlaction(const itpp::vec& cond) {
                
                if (estim){
                        vec x_est=Est->posterior().mean();
                        isa=x_est(0);
                        isb=x_est(1);
                        ome=x_est(2);
                        the=x_est(3);
                } else {
                        isa=cond(0);//x_est(0);
                        isb=cond(1);//x_est(1);
                        ome=cond(4);//x[2];//x_est(2);
                        the=cond(5);//x_est(3);
                }
                Ww=cond(6);
                
                return empty_vec; // dummy return
        };
    void from_setting(const libconfig::Setting& set){
                Controller::from_setting(set);
                Est=UI::build<BM>(set,"estim",UI::optional);
                estim = Est; 
        }
        void log_register ( logger &L, const string &prefix ) {
                Controller::log_register(L,prefix);
                if (estim) Est->log_register(L,prefix);
        }
        void log_write() const{
                if (estim) Est->log_write();
        }
};
//UIREGISTER(PMSMCtrl); -- abstract 

class PMSM_PICtrl: public PMSMCtrl{
public:
        PI_ctrl Cwq;
        PI_ctrl Cuq;
        PI_ctrl Cud;
                
        PMSM_PICtrl():PMSMCtrl(),
        Cwq(3.0, 3.0*0.000125/0.1, -1200, 1200), 
        Cuq(20.0, 20.0*0.000125/0.005, -1200, 1200),
        Cud(20.0, 20.0*0.000125/0.005, -1200, 1200) {}
                
        
        virtual vec ctrlaction(const itpp::vec& cond) {
                PMSMCtrl::ctrlaction(cond); // fills isa,isb,om,the
                
                double Isd = isa*cos(the)+isb*sin(the);
                double Isq = isb*cos(the)-isa*sin(the);
                
                double Iqw=(Cwq.ctrlaction(vec_1(Ww-ome)))(0); // conversion from vec
                
                double ud = (Cud.ctrlaction(vec_1(-Isd)))(0);
                double uq = (Cuq.ctrlaction(vec_1(Iqw-Isq)))(0);
                
                const double Ls0=0.003465; // inductance
                const double Fmag0= 0.1989; // Magnetic??
                
                ud-=Ls0*ome*Iqw; // har
                uq+=Fmag0*ome;
                
                double Um = sqrt(ud*ud+uq*uq);
                double beta = atan(uq/ud);
                if (ud<0) beta += M_PI;
                beta +=the;
                
                vec uab(2);
                uab(0)=Um*cos(beta);               // usx = usa
                uab(1)=Um*sin(beta);    // usy
                
                return uab;
        };
};
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