| 1 | #ifndef PMSM_CTR_H |
|---|
| 2 | #define PMSM_CTR_H |
|---|
| 3 | |
|---|
| 4 | #include <design/ctrlbase.h> |
|---|
| 5 | #include "pmsm.h" |
|---|
| 6 | |
|---|
| 7 | /*! \defgroup PMSM |
|---|
| 8 | @{ |
|---|
| 9 | */ |
|---|
| 10 | |
|---|
| 11 | using namespace bdm; |
|---|
| 12 | |
|---|
| 13 | |
|---|
| 14 | /*! PI Controller*/ |
|---|
| 15 | class PI_ctrl: public Controller{ |
|---|
| 16 | public: |
|---|
| 17 | //!integral state |
|---|
| 18 | double S; |
|---|
| 19 | double Pd; |
|---|
| 20 | double Pi; |
|---|
| 21 | double minOut; |
|---|
| 22 | double maxOut; |
|---|
| 23 | |
|---|
| 24 | PI_ctrl(double Pd0, double Pi0, double minOut0, double maxOut0): S(0.0), Pd(Pd0), Pi(Pi0),minOut(minOut0),maxOut(maxOut0){} |
|---|
| 25 | virtual vec ctrlaction ( const vec &cond ) { |
|---|
| 26 | double tmp; |
|---|
| 27 | tmp = Pd*cond(0)+S; |
|---|
| 28 | |
|---|
| 29 | S += Pi*cond(0); |
|---|
| 30 | |
|---|
| 31 | if (tmp>maxOut) tmp=maxOut; |
|---|
| 32 | if (tmp<minOut) tmp=minOut; |
|---|
| 33 | return vec_1(tmp); |
|---|
| 34 | } |
|---|
| 35 | }; |
|---|
| 36 | |
|---|
| 37 | class PMSM_PICtrl: public Controller{ |
|---|
| 38 | protected: |
|---|
| 39 | shared_ptr<BM> Est; |
|---|
| 40 | vec state; |
|---|
| 41 | public: |
|---|
| 42 | PI_ctrl Cwq; |
|---|
| 43 | PI_ctrl Cuq; |
|---|
| 44 | PI_ctrl Cud; |
|---|
| 45 | |
|---|
| 46 | PMSM_PICtrl(): Cwq(3.0, 3.0*0.000125/0.1, -1200, 1200), |
|---|
| 47 | Cuq(20.0, 20.0*0.000125/0.005, -1200, 1200), |
|---|
| 48 | Cud(20.0, 20.0*0.000125/0.005, -1200, 1200) |
|---|
| 49 | { |
|---|
| 50 | rv = RV("{ua ub }"); |
|---|
| 51 | rvc = RV("{o_ia o_ib o_ua o_ub Ww o_om o_th }"); |
|---|
| 52 | } |
|---|
| 53 | |
|---|
| 54 | void adapt(const itpp::vec& data){ |
|---|
| 55 | vec y=data.get(0,1); |
|---|
| 56 | vec u=data.get(2,3); |
|---|
| 57 | Est->bayes(y,u); |
|---|
| 58 | |
|---|
| 59 | } |
|---|
| 60 | |
|---|
| 61 | |
|---|
| 62 | virtual vec ctrlaction(const itpp::vec& cond) { |
|---|
| 63 | vec x_est=Est->posterior().mean(); |
|---|
| 64 | double isa=cond(0);//x_est(0); |
|---|
| 65 | double isb=cond(1);//x_est(1); |
|---|
| 66 | double ome=cond(5);//x[2];//x_est(2); |
|---|
| 67 | double the=cond(6);//x_est(3); |
|---|
| 68 | double Ww=cond(4); |
|---|
| 69 | |
|---|
| 70 | double Isd = isa*cos(the)+isb*sin(the); |
|---|
| 71 | double Isq = isb*cos(the)-isa*sin(the); |
|---|
| 72 | |
|---|
| 73 | double Iqw=(Cwq.ctrlaction(vec_1(Ww-ome)))(0); // conversion from vec |
|---|
| 74 | |
|---|
| 75 | double ud = (Cud.ctrlaction(vec_1(-Isd)))(0); |
|---|
| 76 | double uq = (Cuq.ctrlaction(vec_1(Iqw-Isq)))(0); |
|---|
| 77 | |
|---|
| 78 | const double Ls0=0.003465; // inductance |
|---|
| 79 | const double Fmag0= 0.1989; // Magnetic?? |
|---|
| 80 | |
|---|
| 81 | ud-=Ls0*ome*Iqw; // har |
|---|
| 82 | uq+=Fmag0*ome; |
|---|
| 83 | |
|---|
| 84 | double Um = sqrt(ud*ud+uq*uq); |
|---|
| 85 | double beta = atan(uq/ud); |
|---|
| 86 | if (ud<0) beta += M_PI; |
|---|
| 87 | beta +=the; |
|---|
| 88 | |
|---|
| 89 | vec uab(2); |
|---|
| 90 | uab(0)=Um*cos(beta); // usx = usa |
|---|
| 91 | uab(1)=Um*sin(beta); // usy |
|---|
| 92 | |
|---|
| 93 | return uab; |
|---|
| 94 | }; |
|---|
| 95 | void from_setting(const libconfig::Setting& set){ |
|---|
| 96 | Controller::from_setting(set); |
|---|
| 97 | Est=UI::build<BM>(set,"estim",UI::compulsory); |
|---|
| 98 | } |
|---|
| 99 | }; |
|---|
| 100 | UIREGISTER(PMSM_PICtrl); |
|---|
| 101 | |
|---|
| 102 | /*!@}*/ |
|---|
| 103 | #endif //PMSM_CTR_H |
|---|
