root/applications/pmsm/pmsm_ctrl.h @ 1269

Revision 1261, 6.9 kB (checked in by smidl, 14 years ago)

LQG changes + lyx

Line 
1#ifndef PMSM_CTR_H
2#define PMSM_CTR_H
3
4#include <design/ctrlbase.h>
5#include <design/lq_ctrl.h>
6#include "pmsm.h"
7
8#include <fstream>
9
10/*! \defgroup PMSM
11@{
12*/
13
14using namespace bdm;
15
16
17/*! PI Controller*/
18class PI_ctrl: public Controller{
19public:
20        //!integral state
21        double S;
22        double Pd;
23        double Pi;
24        double minOut;
25        double maxOut;
26       
27        PI_ctrl(double Pd0, double Pi0, double minOut0, double maxOut0): S(0.0), Pd(Pd0), Pi(Pi0),minOut(minOut0),maxOut(maxOut0){}
28        virtual vec ctrlaction ( const vec &cond ) {
29                        double tmp;
30                        tmp = Pd*cond(0)+S;
31                       
32                        S += Pi*cond(0);
33                       
34                        if (tmp>maxOut) tmp=maxOut;
35                        if (tmp<minOut) tmp=minOut;
36                        return vec_1(tmp);
37        }
38};
39
40/*! \brief Root class for controllers of PMSM
41 *
42 * It defines the interface for controllers that connect to the pmsmDSctrl.
43 * The adapt() function calls bayes() of an internal BM. Note that it is assumed that the estimator is compatible with PMSM system.
44 * I.e. it must have rv={ia, ib} and rvc={ua,ub}, any other BM will not work.
45 *
46 */
47class PMSMCtrl: public Controller{
48protected:
49        //! Est where rv and rcv are not used!
50        shared_ptr<BM> Est;
51        //! switch to use or not to use the internal estimator
52        bool estim;
53       
54        // internal quantities filled by PMSMCtrl::ctrlaction()
55        double isa;
56        double isb;
57        double ome;
58        double the;
59        double Ww;
60       
61public:
62
63        PMSMCtrl():Controller() {
64                rv = RV("{ua ub }");
65                rvc = RV("{o_ia o_ib t_ua t_ub o_om o_th Ww }");
66        }
67       
68    void adapt(const itpp::vec& data){
69                if (estim){
70                        vec y=data.get(0,1);
71                        vec u=data.get(2,3);
72                        Est->bayes(y,u);
73                }
74        }
75       
76       
77    virtual vec ctrlaction(const itpp::vec& cond) {
78               
79                if (estim){
80                        vec x_est=Est->posterior().mean();
81                        isa=x_est(0);
82                        isb=x_est(1);
83                        ome=x_est(2);
84                        the=x_est(3);
85                } else {
86                        isa=cond(0);//x_est(0);
87                        isb=cond(1);//x_est(1);
88                        ome=cond(4);//x[2];//x_est(2);
89                        the=cond(5);//x_est(3);
90                }
91                Ww=cond(6);
92               
93                return empty_vec; // dummy return
94        };
95    void from_setting(const libconfig::Setting& set){
96                Controller::from_setting(set);
97                Est=UI::build<BM>(set,"estim",UI::optional);
98                estim = Est; 
99        }
100        void log_register ( logger &L, const string &prefix ) {
101                Controller::log_register(L,prefix);
102                if (estim) Est->log_register(L,prefix);
103        }
104        void log_write() const{
105                if (estim) Est->log_write();
106        }
107};
108//UIREGISTER(PMSMCtrl); -- abstract
109
110class PMSM_PICtrl: public PMSMCtrl{
111public:
112        PI_ctrl Cwq;
113        PI_ctrl Cuq;
114        PI_ctrl Cud;
115               
116        PMSM_PICtrl():PMSMCtrl(),
117        Cwq(3.0, 3.0*0.000125/0.1, -1200, 1200), 
118        Cuq(20.0, 20.0*0.000125/0.005, -1200, 1200),
119        Cud(20.0, 20.0*0.000125/0.005, -1200, 1200) {}
120               
121       
122        virtual vec ctrlaction(const itpp::vec& cond) {
123                PMSMCtrl::ctrlaction(cond); // fills isa,isb,om,the
124               
125                double Isd = isa*cos(the)+isb*sin(the);
126                double Isq = isb*cos(the)-isa*sin(the);
127               
128                double Iqw=(Cwq.ctrlaction(vec_1(Ww-ome)))(0); // conversion from vec
129               
130                double ud = (Cud.ctrlaction(vec_1(-Isd)))(0);
131                double uq = (Cuq.ctrlaction(vec_1(Iqw-Isq)))(0);
132               
133                const double Ls0=0.003465; // inductance
134                const double Fmag0= 0.1989; // Magnetic??
135               
136                ud-=Ls0*ome*Iqw; // har
137                uq+=Fmag0*ome;
138               
139                double Um = sqrt(ud*ud+uq*uq);
140                double beta = atan(uq/ud);
141                if (ud<0) beta += M_PI;
142                beta +=the;
143               
144                vec uab(2);
145                uab(0)=Um*cos(beta);               // usx = usa
146                uab(1)=Um*sin(beta);    // usy
147               
148                return uab;
149        };
150};
151UIREGISTER(PMSM_PICtrl);
152
153class PMSM_LQCtrl: public PMSMCtrl{
154public:
155/*
156PMSMCtrl:
157        double isa;
158        double isb;
159        double ome;
160        double the;
161        double Ww;
162*/     
163
164//PMSM parameters
165        const double a;
166        const double b;
167        const double c;
168        const double d;
169        const double e;
170       
171//input penalty
172        double r;
173       
174//time step
175        const double Dt;
176       
177//receding horizon
178        int rec_hor;
179       
180//system matrices
181        mat A; //5x5
182        mat B; //5x2
183        //mat C; //2x5
184        mat Q; //5x5
185        mat R; //2x2   
186       
187//control matrix
188        mat L;
189        vec uab;
190        vec icond;
191       
192//control maximum
193        const double MAXu;
194       
195//lqg controler class
196        LQG_universal lq;       
197       
198//prediction
199        vec p_isa, p_isb, p_ome, p_the;
200               
201        PMSM_LQCtrl():PMSMCtrl(), a(0.9898), b(0.0072), c(0.0361), d(1.0), e(0.0149),
202                                r(0.005), Dt(0.000125), rec_hor(10),                                    //for r is a default value rewrited by pcp.txt file value
203                                A(5, 5), B(5, 2), Q(5, 5), R(2, 2),
204                                uab(2), icond(6), MAXu(100.0)   {                       
205                //set fix matrices elements & dimensions
206                A.zeros();
207                        A(0, 0) = A(1, 1) = a;
208                        A(2, 2) = d;
209                        A(2, 4) = d - 1;
210                        A(3, 2) = A(3, 4) = Dt;
211                        A(3, 3) = A(4, 4) = 1.0;
212                B.zeros();
213                        B(0, 0) = B(1, 1) = c;
214                //C.zeros();
215                //      C(0, 0) = C(1, 1) = 1.0;
216                Q.zeros();
217                        Q(2, 2) = 1.0;
218                R.zeros();
219                                                               
220        }
221               
222       
223        virtual vec ctrlaction(const itpp::vec& cond) {
224                PMSMCtrl::ctrlaction(cond); // fills isa,isb,ome,the,Ww
225               
226                int i;
227                lq.resetTime();
228               
229                //create prediction
230                p_isa.zeros();
231                p_isb.zeros();
232                p_ome.zeros();
233                p_the.zeros();
234                p_isa(0) = isa;
235                p_isb(0) = isb;
236                p_ome(0) = ome;
237                p_the(0) = the;
238               
239                for(i = 0; i < rec_hor-1; i++){
240                        p_isa(i+1) = a*p_isa(i) + b*p_ome(i)*sin(p_the(i)) + c*0;//uab(0); //use last used control
241                        p_isb(i+1) = a*p_isb(i) - b*p_ome(i)*cos(p_the(i)) + c*0;//uab(1);
242                        p_ome(i+1) = d*p_ome(i) + e*(p_isb(i)*cos(p_the(i)) - p_isa(i)*sin(p_the(i)));
243                        p_the(i+1) = p_the(i) + Dt*p_ome(i);
244                }
245               
246                //create control matrix
247                for(i = rec_hor; i > 0; i--){           
248                        //set variable matrices elements (A matrix only)
249                        A(0, 2) = b*sin(p_the(i));
250                        A(0, 3) = b*(p_ome(i))*cos(p_the(i));
251                        A(0, 4) = b*sin(p_the(i));
252                        A(1, 2) = -b*cos(p_the(i));
253                        A(1, 3) = b*(p_ome(i))*sin(p_the(i));
254                        A(1, 4) = -b*cos(p_the(i));
255                        A(2, 0) = -e*sin(p_the(i));
256                        A(2, 1) = e*cos(p_the(i));
257                        A(2, 3) = -e*(p_isa(i)*cos(p_the(i)) + p_isb(i)*sin(p_the(i)));
258                       
259                        lq.Models(0).A = A;             
260                        lq.redesign();
261                }
262                lq.redesign();
263               
264                L = lq.getL();
265                icond(0) = isa;
266                icond(1) = isb;
267                icond(2) = ome - Ww;
268                icond(3) = the;
269                icond(4) = Ww;
270                icond(5) = 1;
271                vec tmp = L*icond;                 
272               
273                uab = tmp(0,1);
274               
275                if(uab(0) > MAXu) uab(0) = MAXu;
276                else if(uab(0) < -MAXu) uab(0) = -MAXu;
277                if(uab(1) > MAXu) uab(1) = MAXu;
278                else if(uab(1) < -MAXu) uab(1) = -MAXu;
279               
280                return uab;
281        };
282        void from_setting(const Setting &set){
283                PMSMCtrl::from_setting(set);
284                UI::get(r,set, "r", UI::optional);
285                UI::get(rec_hor,set, "h", UI::optional);
286        }
287
288        void validate(){
289                R(0,0)=R(1,1)=r;
290
291                p_isa.set_length(rec_hor+1);
292                p_isb.set_length(rec_hor+1);
293                p_ome.set_length(rec_hor+1);
294                p_the.set_length(rec_hor+1);
295
296                Array<quadraticfn> qloss(2);
297                qloss(0).Q.setCh(Q);
298                qloss(0).rv = RV("x", 5, 1);
299                qloss(1).Q.setCh(R);
300                qloss(1).rv = RV("u", 2, 0);           
301                lq.Losses = qloss;             
302
303                                //set lq
304                lq.rv = RV("u", 2, 0);                 
305                lq.set_rvc(RV("x", 5, 0));
306                lq.horizon = rec_hor;   
307               
308                Array<linfnEx> model(2);
309                model(0).A = A;
310                model(0).B = vec("0 0 0 0 0");
311                model(0).rv = RV("x", 5, 0);
312                model(0).rv_ret = RV("x", 5, 1);
313                model(1).A = B;
314                model(1).B = vec("0 0");
315                model(1).rv = RV("u", 2, 0);
316                model(1).rv_ret = RV("x", 5, 1);
317                lq.Models = model;
318               
319                lq.finalLoss.Q.setCh(Q);
320                lq.finalLoss.rv = RV("x", 5, 1);
321               
322                lq.validate();
323                                               
324                uab.zeros();
325
326        }
327};
328UIREGISTER(PMSM_LQCtrl);
329
330/*!@}*/
331#endif //PMSM_CTR_H
Note: See TracBrowser for help on using the browser.