regulace.cpp math.h nastaveni_regulatoru_float.h regulace.h work/git/mixpp/pmsm/simulator_zdenek/ekf_example/regulace.cpp math.h regulace.h nastaveni_regulatoru_float.h _USE_MATH_DEFINES MINud (-600.) double double ud ud double double uq uq double double alfa alfa double double beta beta double double Sid Sid double double Siq Siq double double Surm Surm double double Kiqmax Kiqmax double double Iqwmax Iqwmax double double Iqw_reg Iqw_reg double double Idw_urm Idw_urm double double Idw_reg Idw_reg double double Sw Sw double double MAXw_lim MAXw_lim double double U U double double Um Um double double Urmf Urmf double double Kodv_ud Kodv_ud double double Kodv_uind Kodv_uind double double Isd Isd double double Isq Isq double double Fs Fs double double Fmag Fmag double double moment moment double double K_Fs K_Fs double double K_moment K_moment double double Ismaxf2 Ismaxf2 double double tmp_omezeni tmp_omezeni double double ladeni_regulace[10] [10] ladeni_regulace double double Treg Treg unsigned int unsigned int start_reg start_reg double double Ucf Ucf double double Isdf Isdf double double Ukomp Ukomp double double zbeta zbeta double double zbeta_vyp zbeta_vyp double double zbetaw zbetaw unsigned int unsigned int ALGORITMUS ALGORITMUS double double Sbeta Sbeta double double SUkomp SUkomp unsigned int unsigned int blokace_beta blokace_beta 0 void void init_regulace (double *param, double TV) init_regulace double * param double TV void 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) 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 void void reset_regulatoru (void) reset_regulatoru void double double pi_reg (double epsilon, double Kpf, double Kif, double MAX, double MIN, double *S) pi_reg double epsilon double Kpf double Kif double MAX double MIN double * S double double uhel (double x, double y) uhel double x double y void void filtr (double U, double *Uf, double kt) filtr double U double * Uf double kt /********************************* Vektoroverizeni regulacnistruktura-plovouciradovacarka Z.Peroutka Rev.15.3.2008 5.6.2007Doplnenvypocetuhlubeta,kterydoplnujeregulatorIsq **********************************/ #define_USE_MATH_DEFINES #include<math.h> #include"nastaveni_regulatoru_float.h" #include"regulace.h" voidinit_regulace(double*param,doubleTV); //voidvektor_regulace(doubleIdw,doubleIqw,doubleUrm_max,doubleWw,double*u,doubleIsx,doubleIsy,doubletheta,doublerychlost,doubleUcn_2,unsignedintrezim); voidvektor_regulace(doubleIdw,doubleIqw,doubleUrm_max,doubleWw,double*u,doubleIsx,doubleIsy,doubletheta,doublerychlost,doubleUcn_2,doubleUc,doubleUcn,unsignedintrezim); staticvoidreset_regulatoru(void); staticdoublepi_reg(doubleepsilon,doubleKpf,doubleKif,doubleMAX,doubleMIN,double*S); staticdoubleuhel(doublex,doubley); staticvoidfiltr(doubleU,double*Uf,doublekt); //regulatoryproudu staticdoubleud,uq,alfa,beta; staticdoubleSid,Siq,Surm; //omezenimomentu staticdoubleKiqmax,Iqwmax,Iqw_reg; staticdoubleIdw_urm,Idw_reg; //regulatorrychlosti staticdoubleSw,MAXw_lim; staticdoubleU; staticdoubleUm,Urmf;//velikostnapeti //odvazbovaciobvod-blokvypocetnapeti staticdoubleKodv_ud,Kodv_uind; staticdoubleIsd,Isq,Fs,Fmag,moment,K_Fs,K_moment; staticdoubleIsmaxf2,tmp_omezeni; doubleladeni_regulace[10]; doubleTreg; staticunsignedintstart_reg; staticdoubleUcf,Isdf,Ukomp; staticdoublezbeta,zbeta_vyp,zbetaw;//zateznyuhelvyhodnocenyzpolohypozadovanehonapeti staticunsignedintALGORITMUS; //regulatorIsqpreszateznyuhel staticdoubleSbeta; //regulatorIsd-kompenzacevypoctenehonapeti staticdoubleSUkomp; //pomocnepromenne staticunsignedintblokace_beta=0; doubleuhel(doublex,doubley) { doubleth; if(x==0) if(y==0)th=0.; elseif(y>0)th=M_PI/2.; elseth=-M_PI/2.; else th=atan(y/x); if(x<0)th+=M_PI; returnth; } doublepi_reg(doubleepsilon,doubleKpf,doubleKif,doubleMAX,doubleMIN,double*S) { doubleout; out=Kpf*epsilon+*S; if(out>MAX)out=MAX; elseif(out<MIN)out=MIN; else *S+=Kif*epsilon; returnout; } voidfiltr(doubleU,double*Uf,doublekt) { doubleUfpom; Ufpom=*Uf; *Uf=Ufpom+kt*(U-*Uf); } voidinit_regulace(double*param,doubleTV) { doubleKpd;//pomocnavelicina doubleRs,Ls,kp,p; //parametryproodvazbeni Rs=*(param+0); Ls=*(param+1); Fmag=*(param+2); kp=*(param+5); p=*(param+4); //vyhoditazsemvDSP//////////////////// Sid=0;//nulovaniintegracnislozky Siq=0; //regulatorodbuzovani //Kpd=Kpurm*2.0/Iref;//2.0,protozeUrmjeladenovpomernychhodnotachUm*2/UcnatadyjetovztazenonaUcn //Kiurmf=prevod((Kpd*TV/Tiurm),Q_Kiurm); //Kt_urm=prevod(TV/Tfurm,Q_Kturm);//casovakonstantafiltruUrm Surm=0;//nulovaniintegracnislozky Urmf=0;//filtrovanenapetiUrm //omezenimomentukvulimomentuzvratu-omezenipomocifrmax Ismaxf2=Ismax*Ismax; //blokVYPOCETNAPETI Kodv_ud=Ls; Kodv_uind=Fmag; //regulatorrychlosti Sw=0; //vypocetmodelu K_Fs=Ls; K_moment=kp*p*Fmag; Treg=TV; //pocatecniALGORITMUS ALGORITMUS=0;//implicitnesepouzijeklasickevektoroverizeni //regulatorIsqprostrednictvimbeta zbeta=0.; Sbeta=0.; //regulatorIsd-kompenzacevypoctenehonapeti SUkomp=0.; //startregulace start_reg=1;//indikace1.pruchoduregulacnismyckoukvulinastavenispravnychhodnotfiltruUcaIsd } voidvektor_regulace(doubleIdw,doubleIqw,doubleUrm_max,doubleWw,double*u,doubleIsx,doubleIsy,doubletheta,doublerychlost,doubleUcn_2,doubleUc,doubleUcn,unsignedintrezim) { //vypocetIsd,Isq Isd=Isx*cos(theta)+Isy*sin(theta); Isq=Isy*cos(theta)-Isx*sin(theta); //filtracenapetissobvodu if(start_reg==1) { Ucf=Uc; Isdf=Isd; start_reg=0; } else { filtr(Uc,&Ucf,Treg/Tfuc); filtr(Isd,&Isdf,Treg/Tfid); } //vybervariantyregulace if(fabs(rychlost)>prechod_1_2)ALGORITMUS=1;//prechodzalgoritmu1na2 if(fabs(rychlost)<prechod_2_1)ALGORITMUS=0;//prechodzalgortimu2na1 ALGORITMUS=0;//provozovanapouzedefinovanavariantaalgoritmurizeni Iqw_reg=pi_reg(Ww-rychlost,Kpw,Kpw*Treg/Tiw,MAXw,-MAXw,&Sw); if(rezim==0) Iqw_reg=Iqw;//vyrazenireg.rychlosti if(rezim==2)//zkouskamax.momentu Iqw_reg=Iqwmax;//Iqw=sqrt(Imax^2-Idw^2) if(ALGORITMUS==0) { //Regulatorodbuzovani Idw_urm=pi_reg(Urm_max-Urmf,Kpurm,Kpurm*Treg/Tiurm,0.,MINurm,&Surm); //regulaceprouduId,Iq //Idw_urm=Idw;//vyrazenaregulaceUrm //Idw_urm=0; ud=pi_reg(Idw_urm-Isd,Kpi,Kpi*Treg/Tii,MAXi,-MAXi,&Sid); //omezenimax.momentu(resp.max.Iqw)sohledemnamax.proud tmp_omezeni=Ismaxf2-Idw_urm*Idw_urm; if(tmp_omezeni<0)Iqwmax=0; elseIqwmax=sqrt(tmp_omezeni); if(MAXw>Iqwmax)MAXw_lim=Iqwmax;elseMAXw_lim=MAXw; //regulacerychlosti //Iqw_reg=pi_reg(Ww-rychlost,Kpw,Kpw*Treg/Tiw,MAXw_lim,-MAXw_lim,&Sw); /*if(rezim==0) Iqw_reg=Iqw;//vyrazenireg.rychlosti if(rezim==2)//zkouskamax.momentu Iqw_reg=Iqwmax;//Iqw=sqrt(Imax^2-Idw^2) */ if(Iqw_reg>Iqwmax)Iqw_reg=Iqwmax; if(Iqw_reg<-Iqwmax)Iqw_reg=-Iqwmax; uq=pi_reg(Iqw_reg-Isq,Kpi,Kpi*Treg/Tii,MAXi,-MAXi,&Siq); ud-=Kodv_ud*rychlost*Iqw_reg; uq+=Kodv_ud*rychlost*Idw_urm+Kodv_uind*rychlost; //omezeni-saturacenapeti-stejneomezenijakoureg.proudu if(ud>MAXi)ud=MAXi; if(ud<-MAXi)ud=-MAXi; if(uq>MAXi)uq=MAXi; if(uq<-MAXi)uq=-MAXi; //velikostapolohavektorunapeti Um=sqrt(ud*ud+uq*uq); alfa=uhel(ud,uq); //vypocetzateznehouhluzpozadovanehonapeti if(rychlost>=0) zbetaw=alfa-M_PI/2.; else {zbetaw=alfa+M_PI/2.; if(ud<0)zbetaw-=2.*M_PI; } Sbeta=zbetaw;//inicializaceregulatoruIsqprostednictvimbeta-sumacepriprepnutinastavenanaaktualnihodnotuzateznehouhlu! } else//(ALGORITMUS==1)...regulacezateznehouhlu { //omezenimax.momentu(resp.max.Iqw)sohledemnamax.proud tmp_omezeni=Ismaxf2-Isdf*Isdf; if(tmp_omezeni<0)Iqwmax=0; elseIqwmax=sqrt(tmp_omezeni); if(MAXw>Iqwmax)MAXw_lim=Iqwmax;elseMAXw_lim=MAXw; //regulacerychlosti /*Iqw_reg=pi_reg(Ww-rychlost,Kpw,Kpw*Treg/Tiw,MAXw_lim,-MAXw_lim,&Sw); if(rezim==0) Iqw_reg=Iqw;//vyrazenireg.rychlosti if(rezim==2)//zkouskamax.momentu Iqw_reg=Iqwmax;//Iqw=sqrt(Imax^2-Idw^2) */ if(Iqw_reg>Iqwmax)Iqw_reg=Iqwmax; if(Iqw_reg<-Iqwmax)Iqw_reg=-Iqwmax; //Iqw_reg=Iqw;//bezomezovaniIsqw //regulaceIsqprostrednictvimzateznehouhlu zbeta=pi_reg(Iqw_reg-Isq,Kpib,Kpib*Treg/Tiib,MAXbeta,-MAXbeta,&Sbeta); //testupravysignalubeta //if((zbeta>73./180.*M_PI)||(blokace_beta==1)){zbeta=80./180.*M_PI;blokace_beta=1;} //vypocetnapeti-jednoduchavariantabezuvazovaniodbuzeni Um=sqrt(Kodv_uind*Kodv_uind*rychlost*rychlost+Kodv_ud*Kodv_ud*rychlost*rychlost*Iqw_reg*Iqw_reg);//rychlostmusibytvkvadratu,abyseeliminovalvlivznamenkarychlosti-Ummusibyt>=0! //Um=rychlost*sqrt(Kodv_uind*Kodv_uind+Kodv_ud*Kodv_ud*Iqw*Iqw);//neniuvazovanoomezeniIsqw //vylepsenyvypocetnapeti-odvazbovaciobvod /*ud=Kodv_ud*rychlost*Iqw_reg; uq=Kodv_ud*rychlost*Isdf+Kodv_uind*rychlost; Um=sqrt(ud*ud+uq*uq); */ //korecenapetipomociregulatoruIsd Ukomp=pi_reg(0.-Isd,0,1.*Treg/Tiidb,MAXud,MINud,&SUkomp);//dozaporuneomezovat!!! Um+=Ukomp; //zahrnutivlivuskutecnehonapetiUc Um=Um*Ucn/Ucf;//vpevneradovecarcepak:Um=(long)Um*Qm/Ucf; //omezeninaUrm=1 if(Um>Ucn_2)Um=Ucn_2; reset_regulatoru();//zajistujepripravenostreg.strukturyVAR1 //doplnenivypoctuzateznehouhlu...zbeta_vyp if(fabs(rychlost*Kodv_ud*Iqw_reg)>=Um) if(Iqw_reg>0) if(rychlost>0)zbeta_vyp=M_PI/2.; elsezbeta_vyp=-M_PI/2.; else if(rychlost>0)zbeta_vyp=-M_PI/2.; elsezbeta_vyp=M_PI/2.; else zbeta_vyp=asin(rychlost*Kodv_ud*Iqw_reg/Um);//kontrola,zdaKodv_ud=Ls! //zbeta=0;//vyrazenireg.Isq zbeta_vyp=0;//vyrazenibloku"vypocetzateznehouhlu" //urcenipozadovanehozateznehouhluajehoomezenina<-90st;90st.> if(rychlost<0) zbetaw=-zbeta_vyp+zbeta; else zbetaw=zbeta_vyp+zbeta; if(zbetaw>MAXbeta) zbetaw=MAXbeta; if(zbetaw<-MAXbeta) zbetaw=-MAXbeta; //urcenipolohyvektorunapetivsouradnemsystemu(d,q) if(rychlost<0) alfa=zbetaw-M_PI/2.; else alfa=zbetaw+M_PI/2.; } //KODSPOLECNYPROOBEREGULACNISTRUKTURY //vypocetpolohyvektorunapetivestojicimsouradnemsystemu beta=alfa+theta; //vypocetUrmfprodalsiperioduvzorkovani filtr(Um/Ucn_2,&Urmf,Treg/Tfurm); //vypocetvelikostitokuamomentu Fs=Fmag+K_Fs*Isd; moment=K_moment*Isq; //testfiltru-umistenifiltrunemavubecvlivnakvalituregulacealgoritmyvar.2 /*if(start_reg==0) { filtr(Uc,&Ucf,Treg/Tfuc); filtr(Isd,&Isdf,Treg/Tfid); }*/ //OdblokovatpripouzitiPWM_FULL_NEW() //*u=ud*cos(theta)-uq*sin(theta); //*(u+1)=ud*sin(theta)+uq*cos(theta); //OdblokovatpripouzitiPWM_FULL_NEW_3h() *u=Um; *(u+1)=beta; ladeni_regulace[0]=Idw_urm; ladeni_regulace[1]=Urmf; ladeni_regulace[2]=Iqw_reg; ladeni_regulace[3]=Um; ladeni_regulace[4]=Fs; ladeni_regulace[5]=moment; ladeni_regulace[6]=zbetaw; ladeni_regulace[7]=Um/Ucn_2; ladeni_regulace[8]=Ukomp; ladeni_regulace[9]=ALGORITMUS; } voidreset_regulatoru(void) { Surm=0; Sid=0; Siq=0; }