root/applications/pmsm/simulator_zdenek/demo_example/regulace.CPP @ 1440

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