root/simulator_zdenek/regulace.CPP @ 38

Revision 38, 4.2 kB (checked in by peroutka, 17 years ago)
Line 
1/********************************************
2
3        Vector control of PMSM Drive
4
5        Control - floating point
6
7        Z. Peroutka
8
9Rev. 16.3.2008
10
11*********************************************/
12
13#include <math.h>
14#include "nastaveni_regulatoru_float.h"
15#include "regulace.h"
16
17
18void init_regulace(double Ls, double Fpm, double kp, double p, double TV);
19void 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
21static double pi_reg(double epsilon, double Kpf, double Kif, double MAX, double MIN, double *S);
22static double uhel(double x, double y);
23static void filtr(double U, double *Uf, double kt);
24
25// regulatory proudu
26static double ud, uq, alfa, beta;
27static double Sid, Siq, Surm;
28
29// omezeni momentu
30static double Iqwmax, Iqw_reg;
31static double Idw_urm;
32
33// regulator rychlosti
34static double Sw;
35
36static double U;
37static double Um, Urmf;          // velikost napeti
38
39// odvazbovaci obvod - blok vypocet napeti
40static double Kodv_ud, Kodv_uind;
41
42static double Isd, Isq, Fs, Fmag, moment, K_Fs, K_moment;
43static double Ismaxf2, tmp_omezeni;
44
45double Treg;
46
47/////////////////// POMOCNE FUNKCE //////////////////////////////////
48double 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
65double 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
78void 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
89void 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
115void 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}
Note: See TracBrowser for help on using the browser.