[38] | 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 | } |
---|