[278] | 1 | /********************************* |
---|
| 2 | |
---|
| 3 | Vektorove rizeni |
---|
| 4 | |
---|
| 5 | regulacni struktura - plovouci radova carka |
---|
| 6 | |
---|
| 7 | Z. Peroutka |
---|
| 8 | |
---|
| 9 | Rev. 15.3.2008 |
---|
| 10 | |
---|
| 11 | 5.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 | |
---|
| 21 | void 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); |
---|
| 23 | 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); |
---|
| 24 | |
---|
| 25 | static void reset_regulatoru(void); |
---|
| 26 | |
---|
| 27 | static double pi_reg(double epsilon, double Kpf, double Kif, double MAX, double MIN, double *S); |
---|
| 28 | static double uhel(double x, double y); |
---|
| 29 | static void filtr(double U, double *Uf, double kt); |
---|
| 30 | |
---|
| 31 | // regulatory proudu |
---|
| 32 | static double ud, uq, alfa, beta; |
---|
| 33 | static double Sid, Siq, Surm; |
---|
| 34 | |
---|
| 35 | // omezeni momentu |
---|
| 36 | static double Kiqmax, Iqwmax, Iqw_reg; |
---|
| 37 | static double Idw_urm, Idw_reg; |
---|
| 38 | |
---|
| 39 | // regulator rychlosti |
---|
| 40 | static double Sw, MAXw_lim; |
---|
| 41 | |
---|
| 42 | static double U; |
---|
| 43 | static double Um, Urmf; // velikost napeti |
---|
| 44 | |
---|
| 45 | // odvazbovaci obvod - blok vypocet napeti |
---|
| 46 | static double Kodv_ud, Kodv_uind; |
---|
| 47 | |
---|
| 48 | static double Isd, Isq, Fs, Fmag, moment, K_Fs, K_moment; |
---|
| 49 | static double Ismaxf2, tmp_omezeni; |
---|
| 50 | |
---|
| 51 | double ladeni_regulace[10]; |
---|
| 52 | |
---|
| 53 | double Treg; |
---|
| 54 | |
---|
| 55 | static unsigned int start_reg; |
---|
| 56 | static double Ucf, Isdf, Ukomp; |
---|
| 57 | static double zbeta, zbeta_vyp, zbetaw; // zatezny uhel vyhodnoceny z polohy pozadovaneho napeti |
---|
| 58 | |
---|
| 59 | static unsigned int ALGORITMUS; |
---|
| 60 | |
---|
| 61 | // regulator Isq pres zatezny uhel |
---|
| 62 | static double Sbeta; |
---|
| 63 | |
---|
| 64 | // regulator Isd - kompenzace vypocteneho napeti |
---|
| 65 | static double SUkomp; |
---|
| 66 | |
---|
| 67 | // pomocne promenne |
---|
| 68 | static unsigned int blokace_beta=0; |
---|
| 69 | |
---|
| 70 | /////////////////// POMOCNE FUNKCE ////////////////////////////////// |
---|
| 71 | double 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 | |
---|
| 88 | double 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 | |
---|
| 101 | void 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 | |
---|
| 112 | void 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 | |
---|
| 167 | 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) |
---|
| 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 /////////////////// |
---|
| 198 | if (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 ////////////////////////// |
---|
| 252 | else // (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 | |
---|
| 371 | void reset_regulatoru(void) |
---|
| 372 | { |
---|
| 373 | Surm=0; |
---|
| 374 | Sid=0; |
---|
| 375 | Siq=0; |
---|
[50] | 376 | } |
---|