root/pmsm/simulator_zdenek/ekf_example/regulace.cpp @ 249

Revision 52, 10.6 kB (checked in by smidl, 18 years ago)

zprovozneni na linuxu

Line 
1/*********************************
2
3        Vektorove rizeni
4
5        regulacni struktura - plovouci radova carka
6
7        Z. Peroutka
8
9Rev. 15.3.2008
10
115.6.2007        Doplnen vypocet uhlu beta, ktery doplnuje regulator Isq
12
13**********************************/
14
15#define _USE_MATH_DEFINES
16#include <math.h>
17#include "nastaveni_regulatoru_float.h"
18#include "regulace.h"
19
20
21void init_regulace(double *param, double TV);
22//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, unsigned int rezim);
23void 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);
24
25static void reset_regulatoru(void);
26
27static double pi_reg(double epsilon, double Kpf, double Kif, double MAX, double MIN, double *S);
28static double uhel(double x, double y);
29static void filtr(double U, double *Uf, double kt);
30
31// regulatory proudu
32static double ud, uq, alfa, beta;
33static double Sid, Siq, Surm;
34
35// omezeni momentu
36static double Kiqmax, Iqwmax, Iqw_reg;
37static double Idw_urm, Idw_reg;
38
39// regulator rychlosti
40static double Sw, MAXw_lim;
41
42static double U;
43static double Um, Urmf;          // velikost napeti
44
45// odvazbovaci obvod - blok vypocet napeti
46static double Kodv_ud, Kodv_uind;
47
48static double Isd, Isq, Fs, Fmag, moment, K_Fs, K_moment;
49static double Ismaxf2, tmp_omezeni;
50
51double ladeni_regulace[10];
52
53double Treg;
54
55static unsigned int start_reg;
56static double Ucf, Isdf, Ukomp;
57static double zbeta, zbeta_vyp, zbetaw;         // zatezny uhel vyhodnoceny z polohy pozadovaneho napeti
58
59static unsigned int ALGORITMUS;
60
61// regulator Isq pres zatezny uhel
62static double Sbeta;
63
64// regulator Isd - kompenzace vypocteneho napeti
65static double SUkomp;
66
67// pomocne promenne
68static unsigned int blokace_beta=0;
69
70/////////////////// POMOCNE FUNKCE //////////////////////////////////
71double uhel(double x, double y)
72{
73  double th;
74
75  if (x==0)
76    if (y==0) th=0.;
77        else if (y>0) th=M_PI/2.;
78             else th=-M_PI/2.;
79  else
80    th=atan(y/x);
81
82  if (x<0) th+=M_PI;
83
84  return th;
85}
86
87
88double pi_reg(double epsilon, double Kpf, double Kif, double MAX, double MIN, double *S)
89{
90  double out;
91
92  out=Kpf*epsilon+*S;
93  if (out>MAX) out=MAX;
94  else if (out<MIN) out=MIN;
95  else
96    *S+=Kif*epsilon;
97
98  return out;
99}
100
101void filtr(double U, double *Uf, double kt)
102{
103  double Ufpom;
104
105  Ufpom=*Uf;
106  *Uf=Ufpom+kt*(U-*Uf);
107}
108
109//////////////////////////////////////////////////////////////////////////
110
111
112void init_regulace(double *param, double TV)
113{
114  double Kpd;   // pomocna velicina
115  double Rs, Ls, kp, p;
116
117  // parametry pro odvazbeni
118  Rs=*(param+0);
119  Ls=*(param+1);
120  Fmag=*(param+2);
121  kp=*(param+5);
122  p=*(param+4);
123  // vyhodit az sem v DSP ////////////////////
124
125  Sid=0;        // nulovani integracni slozky
126  Siq=0;
127
128  // regulator odbuzovani
129//  Kpd=Kpurm*2.0/Iref;      // 2.0, protoze Urm je ladeno v pomernych hodnotach Um*2/Ucn a tady je to vztazeno na Ucn
130//  Kiurmf=prevod((Kpd*TV/Tiurm),Q_Kiurm);
131
132//  Kt_urm=prevod(TV/Tfurm,Q_Kturm);   // casova konstanta filtru Urm
133
134  Surm=0;        // nulovani integracni slozky
135  Urmf=0;        // filtrovane napeti Urm
136
137  // omezeni momentu kvuli momentu zvratu - omezeni pomoci frmax
138  Ismaxf2=Ismax*Ismax;
139
140  // blok VYPOCET NAPETI
141  Kodv_ud=Ls;
142  Kodv_uind=Fmag;
143
144  // regulator rychlosti
145  Sw=0;
146
147  // vypocet modelu
148  K_Fs=Ls;
149  K_moment=kp*p*Fmag;
150
151  Treg=TV;
152
153  // pocatecni ALGORITMUS
154  ALGORITMUS=0;         // implicitne se pouzije klasicke vektorove rizeni
155
156  // regulator Isq prostrednictvim beta
157  zbeta=0.;
158  Sbeta=0.;
159 
160  // regulator Isd - kompenzace vypocteneho napeti
161  SUkomp=0.;
162
163  // start regulace
164  start_reg=1;                  // indikace 1. pruchodu regulacni smyckou kvuli nastaveni spravnych hodnot filtru Uc a Isd
165}
166
167void 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)
168{
169  // vypocet Isd, Isq
170  Isd=Isx*cos(theta)+Isy*sin(theta);
171  Isq=Isy*cos(theta)-Isx*sin(theta);
172
173  // filtrace napeti ss obvodu
174  if (start_reg==1)
175  { 
176    Ucf=Uc;
177    Isdf=Isd;
178    start_reg=0;
179  }
180  else
181  {
182    filtr(Uc,&Ucf,Treg/Tfuc);
183    filtr(Isd,&Isdf,Treg/Tfid);
184  } 
185
186  // vyber varianty regulace
187  if (fabs(rychlost)>prechod_1_2) ALGORITMUS=1; // prechod z algoritmu 1 na 2
188  if (fabs(rychlost)<prechod_2_1) ALGORITMUS=0; // prechod z algortimu 2 na 1
189  ALGORITMUS=0;         // provozovana pouze definovana varianta algoritmu rizeni
190
191  Iqw_reg=pi_reg(Ww-rychlost,Kpw,Kpw*Treg/Tiw,MAXw,-MAXw,&Sw);
192  if (rezim==0)
193    Iqw_reg=Iqw;          // vyrazeni reg. rychlosti
194  if (rezim==2)           // zkouska max. momentu
195    Iqw_reg=Iqwmax;       // Iqw = sqrt(Imax^2-Idw^2)
196
197////////////// REGULACE - VARIANTA 1 - klasicke vektorove rizeni ///////////////////
198if (ALGORITMUS==0) 
199{
200  // Regulator odbuzovani
201  Idw_urm=pi_reg(Urm_max-Urmf,Kpurm,Kpurm*Treg/Tiurm,0.,MINurm,&Surm);
202
203  // regulace proudu Id, Iq
204//  Idw_urm = Idw;      // vyrazena regulace Urm
205//  Idw_urm=0;
206  ud=pi_reg(Idw_urm-Isd,Kpi,Kpi*Treg/Tii,MAXi,-MAXi,&Sid);
207
208  // omezeni max. momentu (resp. max. Iqw) s ohledem na max. proud
209  tmp_omezeni=Ismaxf2-Idw_urm*Idw_urm;
210  if (tmp_omezeni<0) Iqwmax=0;
211  else Iqwmax=sqrt(tmp_omezeni);
212  if (MAXw>Iqwmax) MAXw_lim=Iqwmax;else MAXw_lim=MAXw;
213
214  // regulace rychlosti
215//  Iqw_reg=pi_reg(Ww-rychlost,Kpw,Kpw*Treg/Tiw,MAXw_lim,-MAXw_lim,&Sw);
216/*  if (rezim==0)
217    Iqw_reg=Iqw;          // vyrazeni reg. rychlosti
218  if (rezim==2)           // zkouska max. momentu
219    Iqw_reg=Iqwmax;       // Iqw = sqrt(Imax^2-Idw^2)
220*/
221  if (Iqw_reg>Iqwmax) Iqw_reg=Iqwmax;
222  if (Iqw_reg<-Iqwmax) Iqw_reg=-Iqwmax;
223  uq=pi_reg(Iqw_reg-Isq,Kpi,Kpi*Treg/Tii,MAXi,-MAXi,&Siq);
224
225///////// Vypocet napeti - "ODVAZBENI"
226  ud-=Kodv_ud*rychlost*Iqw_reg;
227  uq+=Kodv_ud*rychlost*Idw_urm+Kodv_uind*rychlost;
228
229  // omezeni - saturace napeti - stejne omezeni jako u reg. proudu
230  if (ud>MAXi) ud=MAXi;
231  if (ud<-MAXi) ud=-MAXi;
232  if (uq>MAXi) uq=MAXi;
233  if (uq<-MAXi) uq=-MAXi;
234
235  ////////////// KONEC ODVAZBENI /////////////////////////////////////////
236
237  // velikost a poloha vektoru napeti
238  Um=sqrt(ud*ud+uq*uq);
239  alfa=uhel(ud,uq);
240
241  // vypocet zatezneho uhlu z pozadovaneho napeti
242  if (rychlost>=0)
243    zbetaw=alfa-M_PI/2.;
244  else
245  { zbetaw=alfa+M_PI/2.;
246    if (ud<0) zbetaw-=2.*M_PI;
247  }
248
249  Sbeta=zbetaw;         // inicializace regulatoru Isq prostednictvim beta - sumace pri prepnuti nastavena na aktualni hodnotu zatezneho uhlu!
250}
251///////////////////// konec kodu VARIANTY 1 //////////////////////////
252else    // (ALGORITMUS == 1)    ... regulace zatezneho uhlu
253{
254///////////////// KOD VARIANTA 2 - regulace zatezneho uhlu ///////////
255  // omezeni max. momentu (resp. max. Iqw) s ohledem na max. proud
256  tmp_omezeni=Ismaxf2-Isdf*Isdf;
257  if (tmp_omezeni<0) Iqwmax=0;
258  else Iqwmax=sqrt(tmp_omezeni);
259  if (MAXw>Iqwmax) MAXw_lim=Iqwmax;else MAXw_lim=MAXw;
260 
261  // regulace rychlosti
262/*  Iqw_reg=pi_reg(Ww-rychlost,Kpw,Kpw*Treg/Tiw,MAXw_lim,-MAXw_lim,&Sw);
263  if (rezim==0)
264    Iqw_reg=Iqw;          // vyrazeni reg. rychlosti
265  if (rezim==2)           // zkouska max. momentu
266    Iqw_reg=Iqwmax;       // Iqw = sqrt(Imax^2-Idw^2)
267*/
268  if (Iqw_reg>Iqwmax) Iqw_reg=Iqwmax;
269  if (Iqw_reg<-Iqwmax) Iqw_reg=-Iqwmax;
270 
271//  Iqw_reg=Iqw;                // bez omezovani Isqw
272  // regulace Isq prostrednictvim zatezneho uhlu
273  zbeta=pi_reg(Iqw_reg-Isq,Kpib,Kpib*Treg/Tiib,MAXbeta,-MAXbeta,&Sbeta);
274
275  // test upravy signalu beta
276//if ((zbeta>73./180.*M_PI)||(blokace_beta==1)) {zbeta=80./180.*M_PI; blokace_beta=1;}
277
278   // vypocet napeti - jednoducha varianta bez uvazovani odbuzeni
279  Um=sqrt(Kodv_uind*Kodv_uind*rychlost*rychlost+Kodv_ud*Kodv_ud*rychlost*rychlost*Iqw_reg*Iqw_reg);     // rychlost musi byt v kvadratu, aby se eliminoval vliv znamenka rychlosti - Um musi byt >=0!
280//  Um=rychlost*sqrt(Kodv_uind*Kodv_uind+Kodv_ud*Kodv_ud*Iqw*Iqw);      // neni uvazovano omezeni Isqw
281
282  // vylepseny vypocet napeti - odvazbovaci obvod
283/*  ud=Kodv_ud*rychlost*Iqw_reg;
284  uq=Kodv_ud*rychlost*Isdf+Kodv_uind*rychlost;
285  Um=sqrt(ud*ud+uq*uq);
286*/
287 
288  // korece napeti pomoci regulatoru Isd
289  Ukomp=pi_reg(0.-Isd,0,1.*Treg/Tiidb,MAXud,MINud,&SUkomp);             // do zaporu neomezovat!!! 
290  Um+=Ukomp;
291
292  // zahrnuti vlivu skutecneho napeti Uc
293  Um=Um*Ucn/Ucf;                // v pevne radove carce pak: Um=(long)Um*Qm/Ucf;
294
295  // omezeni na Urm=1
296  if (Um>Ucn_2) Um=Ucn_2;
297
298  reset_regulatoru();   // zajistuje pripravenost reg. struktury VAR1
299
300  // doplneni vypoctu zatezneho uhlu ... zbeta_vyp
301  if (fabs(rychlost*Kodv_ud*Iqw_reg)>=Um)
302    if (Iqw_reg>0)
303      if (rychlost>0) zbeta_vyp=M_PI/2.;
304      else zbeta_vyp=-M_PI/2.;
305    else
306      if (rychlost>0) zbeta_vyp=-M_PI/2.;
307      else zbeta_vyp=M_PI/2.;
308  else
309    zbeta_vyp=asin(rychlost*Kodv_ud*Iqw_reg/Um);        // kontrola, zda Kodv_ud=Ls!
310
311//  zbeta=0;      // vyrazeni reg. Isq
312  zbeta_vyp=0;  // vyrazeni bloku "vypocet zatezneho uhlu"
313  // urceni pozadovaneho zatezneho uhlu a jeho omezeni na <-90st; 90st.>
314  if (rychlost<0)
315    zbetaw=-zbeta_vyp+zbeta;
316  else
317    zbetaw=zbeta_vyp+zbeta;
318
319  if (zbetaw>MAXbeta)
320    zbetaw=MAXbeta;
321  if (zbetaw<-MAXbeta)
322    zbetaw=-MAXbeta; /**/
323
324  // urceni polohy vektoru napeti v souradnem systemu (d,q)
325  if (rychlost<0)
326    alfa=zbetaw-M_PI/2.;
327  else
328    alfa=zbetaw+M_PI/2.;
329}
330///////////////////// konec kodu VARIANTY 2 //////////////////////////
331
332// KOD SPOLECNY PRO OBE REGULACNI STRUKTURY
333  // vypocet polohy vektoru napeti ve stojicim souradnem systemu
334  beta=alfa+theta;
335
336  // vypocet Urmf pro dalsi periodu vzorkovani
337  filtr(Um/Ucn_2,&Urmf,Treg/Tfurm);
338 
339  // vypocet velikosti toku a momentu
340  Fs=Fmag+K_Fs*Isd;
341  moment=K_moment*Isq;
342
343  // test filtru        - umisteni filtru nema vubec vliv na kvalitu regulace algoritmy var. 2
344/*  if (start_reg==0)
345  {
346        filtr(Uc,&Ucf,Treg/Tfuc);
347        filtr(Isd,&Isdf,Treg/Tfid);
348  } */
349
350/////// PRECHOD ZPET DO SIMULACE //////////////
351
352// Odblokovat pri pouziti PWM_FULL_NEW()
353//  *u=ud*cos(theta)-uq*sin(theta);
354//  *(u+1)=ud*sin(theta)+uq*cos(theta);
355// Odblokovat pri pouziti PWM_FULL_NEW_3h()
356  *u=Um;
357  *(u+1)=beta;
358
359  ladeni_regulace[0]=Idw_urm;
360  ladeni_regulace[1]=Urmf;
361  ladeni_regulace[2]=Iqw_reg;
362  ladeni_regulace[3]=Um;
363  ladeni_regulace[4]=Fs;
364  ladeni_regulace[5]=moment;
365  ladeni_regulace[6]=zbetaw;
366  ladeni_regulace[7]=Um/Ucn_2;
367  ladeni_regulace[8]=Ukomp;
368  ladeni_regulace[9]=ALGORITMUS;
369}
370
371void reset_regulatoru(void)
372{
373  Surm=0;
374  Sid=0;
375  Siq=0;
376}
Note: See TracBrowser for help on using the browser.