1 | /******************************************** |
---|
2 | |
---|
3 | Vector control of PMSM Drive |
---|
4 | |
---|
5 | Control - floating point |
---|
6 | |
---|
7 | Z. Peroutka |
---|
8 | |
---|
9 | Rev. 16.3.2008 |
---|
10 | |
---|
11 | *********************************************/ |
---|
12 | |
---|
13 | #include <math.h> |
---|
14 | #include "nastaveni_regulatoru_float.h" |
---|
15 | #include "regulace.h" |
---|
16 | |
---|
17 | |
---|
18 | void init_regulace(double Ls, double Fpm, double kp, double p, double TV); |
---|
19 | void vektor_regulace(double Idw, double Iqw, double Urm_max, double Ww, double *u, double Isx, double Isy, double theta, double rychlost, double Ucn_2, double Uc, double Ucn, unsigned int rezim); |
---|
20 | |
---|
21 | static double pi_reg(double epsilon, double Kpf, double Kif, double MAX, double MIN, double *S); |
---|
22 | static double uhel(double x, double y); |
---|
23 | static void filtr(double U, double *Uf, double kt); |
---|
24 | |
---|
25 | // regulatory proudu |
---|
26 | static double ud, uq, alfa, beta; |
---|
27 | static double Sid, Siq, Surm; |
---|
28 | |
---|
29 | // omezeni momentu |
---|
30 | static double Iqwmax, Iqw_reg; |
---|
31 | static double Idw_urm; |
---|
32 | |
---|
33 | // regulator rychlosti |
---|
34 | static double Sw; |
---|
35 | |
---|
36 | static double U; |
---|
37 | static double Um, Urmf; // velikost napeti |
---|
38 | |
---|
39 | // odvazbovaci obvod - blok vypocet napeti |
---|
40 | static double Kodv_ud, Kodv_uind; |
---|
41 | |
---|
42 | static double Isd, Isq, Fs, Fmag, moment, K_Fs, K_moment; |
---|
43 | static double Ismaxf2, tmp_omezeni; |
---|
44 | |
---|
45 | double Treg; |
---|
46 | |
---|
47 | /////////////////// POMOCNE FUNKCE ////////////////////////////////// |
---|
48 | double uhel(double x, double y) |
---|
49 | { |
---|
50 | double th; |
---|
51 | |
---|
52 | if (x==0) |
---|
53 | if (y==0) th=0.; |
---|
54 | else if (y>0) th=M_PI/2.; |
---|
55 | else th=-M_PI/2.; |
---|
56 | else |
---|
57 | th=atan(y/x); |
---|
58 | |
---|
59 | if (x<0) th+=M_PI; |
---|
60 | |
---|
61 | return th; |
---|
62 | } |
---|
63 | |
---|
64 | |
---|
65 | double pi_reg(double epsilon, double Kpf, double Kif, double MAX, double MIN, double *S) |
---|
66 | { |
---|
67 | double out; |
---|
68 | |
---|
69 | out=Kpf*epsilon+*S; |
---|
70 | if (out>MAX) out=MAX; |
---|
71 | else if (out<MIN) out=MIN; |
---|
72 | else |
---|
73 | *S+=Kif*epsilon; |
---|
74 | |
---|
75 | return out; |
---|
76 | } |
---|
77 | |
---|
78 | void filtr(double U, double *Uf, double kt) |
---|
79 | { |
---|
80 | double Ufpom; |
---|
81 | |
---|
82 | Ufpom=*Uf; |
---|
83 | *Uf=Ufpom+kt*(U-*Uf); |
---|
84 | } |
---|
85 | |
---|
86 | ////////////////////////////////////////////////////////////////////////// |
---|
87 | |
---|
88 | |
---|
89 | void init_regulace(double Ls, double Fpm, double kp, double p, double TV) |
---|
90 | { |
---|
91 | double Kpd; // pomocna velicina |
---|
92 | |
---|
93 | // parametry pro odvazbeni |
---|
94 | Fmag=Fpm; |
---|
95 | |
---|
96 | Sid=0; // nulovani integracni slozky |
---|
97 | Siq=0; |
---|
98 | |
---|
99 | Surm=0; // nulovani integracni slozky |
---|
100 | Urmf=0; // filtrovane napeti Urm |
---|
101 | |
---|
102 | // omezeni momentu kvuli momentu zvratu - omezeni pomoci frmax |
---|
103 | Ismaxf2=Ismax*Ismax; |
---|
104 | |
---|
105 | // blok VYPOCET NAPETI |
---|
106 | Kodv_ud=Ls; |
---|
107 | Kodv_uind=Fmag; |
---|
108 | |
---|
109 | // regulator rychlosti |
---|
110 | Sw=0; |
---|
111 | |
---|
112 | Treg=TV; |
---|
113 | } |
---|
114 | |
---|
115 | void vektor_regulace(double Idw, double Iqw, double Urm_max, double Ww, double *u, double Isx, double Isy, double theta, double rychlost, double Ucn_2, double Uc, double Ucn, unsigned int rezim) |
---|
116 | { |
---|
117 | // vypocet Isd, Isq |
---|
118 | Isd=Isx*cos(theta)+Isy*sin(theta); |
---|
119 | Isq=Isy*cos(theta)-Isx*sin(theta); |
---|
120 | |
---|
121 | Iqw_reg=pi_reg(Ww-rychlost,Kpw,Kpw*Treg/Tiw,MAXw,-MAXw,&Sw); |
---|
122 | if (rezim==0) |
---|
123 | Iqw_reg=Iqw; // vyrazeni reg. rychlosti |
---|
124 | if (rezim==2) // zkouska max. momentu |
---|
125 | Iqw_reg=Iqwmax; // Iqw = sqrt(Imax^2-Idw^2) |
---|
126 | |
---|
127 | // Regulator odbuzovani |
---|
128 | Idw_urm=pi_reg(Urm_max-Urmf,Kpurm,Kpurm*Treg/Tiurm,0.,MINurm,&Surm); |
---|
129 | |
---|
130 | // regulace proudu Id, Iq |
---|
131 | ud=pi_reg(Idw_urm-Isd,Kpi,Kpi*Treg/Tii,MAXi,-MAXi,&Sid); |
---|
132 | |
---|
133 | // omezeni max. momentu (resp. max. Iqw) s ohledem na max. proud |
---|
134 | tmp_omezeni=Ismaxf2-Idw_urm*Idw_urm; |
---|
135 | if (tmp_omezeni<0) Iqwmax=0; |
---|
136 | else Iqwmax=sqrt(tmp_omezeni); |
---|
137 | |
---|
138 | if (Iqw_reg>Iqwmax) Iqw_reg=Iqwmax; |
---|
139 | if (Iqw_reg<-Iqwmax) Iqw_reg=-Iqwmax; |
---|
140 | uq=pi_reg(Iqw_reg-Isq,Kpi,Kpi*Treg/Tii,MAXi,-MAXi,&Siq); |
---|
141 | |
---|
142 | ///////// Vypocet napeti - "ODVAZBENI" |
---|
143 | ud-=Kodv_ud*rychlost*Iqw_reg; |
---|
144 | uq+=Kodv_ud*rychlost*Idw_urm+Kodv_uind*rychlost; |
---|
145 | |
---|
146 | // omezeni - saturace napeti - stejne omezeni jako u reg. proudu |
---|
147 | if (ud>MAXi) ud=MAXi; |
---|
148 | if (ud<-MAXi) ud=-MAXi; |
---|
149 | if (uq>MAXi) uq=MAXi; |
---|
150 | if (uq<-MAXi) uq=-MAXi; |
---|
151 | |
---|
152 | ////////////// KONEC ODVAZBENI ///////////////////////////////////////// |
---|
153 | |
---|
154 | // velikost a poloha vektoru napeti |
---|
155 | Um=sqrt(ud*ud+uq*uq); |
---|
156 | alfa=uhel(ud,uq); |
---|
157 | |
---|
158 | // vypocet polohy vektoru napeti ve stojicim souradnem systemu |
---|
159 | beta=alfa+theta; |
---|
160 | |
---|
161 | // vypocet Urmf pro dalsi periodu vzorkovani |
---|
162 | filtr(Um/Ucn_2,&Urmf,Treg/Tfurm); |
---|
163 | |
---|
164 | /////// PRECHOD ZPET DO SIMULACE ////////////// |
---|
165 | *u=Um; |
---|
166 | *(u+1)=beta; |
---|
167 | |
---|
168 | } |
---|