[50] | 1 | /* model synchronniho motoru s permanentnimi magnety |
---|
| 2 | |
---|
| 3 | PMSM v plovouci radove carce - fyzikalni model! |
---|
| 4 | |
---|
| 5 | 31.5.2007 |
---|
| 6 | |
---|
| 7 | Upraven vypocet momentu M = Fmag*Isq!! (nikoliv M = Fs*Isq!!) |
---|
| 8 | 31.5.2007 doplnen vypocet zatezneho uhlu (beta) |
---|
| 9 | */ |
---|
| 10 | |
---|
| 11 | #define _USE_MATH_DEFINES |
---|
| 12 | #include <math.h> |
---|
| 13 | #include "pmsm_mod.h" |
---|
| 14 | |
---|
| 15 | double x[10]; // (isx,isy,wme,theta_e,M,Fsd,Isd,Isq,Mz,beta) ... beta = zatezny uhel |
---|
| 16 | |
---|
| 17 | void init_pmsm(double *param, double *REL); |
---|
| 18 | void pmsm_double(double usx, double usy, double h, unsigned int vypocet); |
---|
| 19 | |
---|
| 20 | // pomocne funkce |
---|
| 21 | static double uhel(double x, double y); |
---|
| 22 | |
---|
| 23 | static double Isx,Isy,wme,Mz,M,Fs,Isd,Isq; |
---|
| 24 | static double Rs,Ls,Fmag,Bfric,p,kp,J; |
---|
| 25 | // Rs=odpor stator; Ls=statorova indukcnost (synchronni reaktance) |
---|
| 26 | // p=pocet polparu, kp=konst. Parkovy transf., |
---|
| 27 | // J=moment setrvacnosti (J<0 ... nekonecny) |
---|
| 28 | |
---|
| 29 | static double Uref, Iref, Wref, Mref, Fref, Thetaref; // referencni hodnoty |
---|
| 30 | |
---|
| 31 | static double dIsx,dIsx1,dIsx2,dIsx3,dIsy,dIsy1,dIsy2,dIsy3; |
---|
| 32 | static double dTheta,dTheta1,dTheta2,dTheta3; |
---|
| 33 | static double dw,dw1,dw2,dw3; |
---|
| 34 | |
---|
| 35 | static double A,B,C,D,E,F,G; |
---|
| 36 | |
---|
| 37 | static double Fsq; |
---|
| 38 | |
---|
| 39 | |
---|
| 40 | /////////////////// POMOCNE FUNKCE ////////////////////////////////// |
---|
| 41 | double uhel(double x, double y) |
---|
| 42 | { |
---|
| 43 | double th; |
---|
| 44 | |
---|
| 45 | if (x==0) |
---|
| 46 | if (y==0) th=0.; |
---|
| 47 | else if (y>0) th=M_PI/2.; |
---|
| 48 | else th=-M_PI/2.; |
---|
| 49 | else |
---|
| 50 | th=atan(y/x); |
---|
| 51 | |
---|
| 52 | if (x<0) th+=M_PI; |
---|
| 53 | |
---|
| 54 | return th; |
---|
| 55 | } |
---|
| 56 | |
---|
| 57 | |
---|
| 58 | void init_pmsm(double *param, double *REL) |
---|
| 59 | { |
---|
| 60 | unsigned int i; |
---|
| 61 | |
---|
| 62 | Rs=*(param); |
---|
| 63 | Ls=*(param+1); |
---|
| 64 | Fmag=*(param+2); |
---|
| 65 | Bfric=*(param+3); |
---|
| 66 | |
---|
| 67 | p=*(param+4); |
---|
| 68 | kp=*(param+5); |
---|
| 69 | J=*(param+6); |
---|
| 70 | |
---|
| 71 | Uref=*REL; |
---|
| 72 | Iref=*(REL+1); |
---|
| 73 | Wref=*(REL+2); |
---|
| 74 | Thetaref=*(REL+3); |
---|
| 75 | Mref=*(REL+4); |
---|
| 76 | Fref=*(REL+5); |
---|
| 77 | |
---|
| 78 | // definice konstant modelu |
---|
| 79 | A=Rs/Ls; |
---|
| 80 | B=Fmag/Ls*Wref/Iref; |
---|
| 81 | C=Uref/Iref/Ls; |
---|
| 82 | D=Bfric/J; |
---|
| 83 | E=kp*p*p*Fmag/J*Iref/Wref; |
---|
| 84 | F=p/J*Mref/Wref; |
---|
| 85 | G=Wref/Thetaref; |
---|
| 86 | |
---|
| 87 | // definice pocatecnich hodnot |
---|
| 88 | dIsx=0;dIsx1=0;dIsx2=0;dIsx3=0;dIsy=0;dIsy1=0;dIsy2=0;dIsy3=0; |
---|
| 89 | dTheta=0;dTheta1=0;dTheta2=0;dTheta3=0; |
---|
| 90 | dw=0;dw1=0;dw2=0;dw3=0; |
---|
| 91 | } |
---|
| 92 | |
---|
| 93 | void pmsm_double(double usx, double usy, double h, unsigned int vypocet) |
---|
| 94 | // vypocet<5...Euler, vypocet>4 ... Adams 4.radu |
---|
| 95 | { |
---|
| 96 | // (isx,isy,wme,theta_e,M,Fs,Mz) |
---|
| 97 | // vypocty diferenci |
---|
| 98 | dIsx=-A*x[0]+B*x[2]*sin(x[3]*Thetaref)+C*usx; |
---|
| 99 | dIsy=-A*x[1]-B*x[2]*cos(x[3]*Thetaref)+C*usy; |
---|
| 100 | dTheta=G*x[2]; |
---|
| 101 | |
---|
| 102 | if (J>0) |
---|
| 103 | dw=E*(x[1]*cos(x[3]*Thetaref)-x[0]*sin(x[3]*Thetaref))-D*x[2]-F*x[8]; |
---|
| 104 | else |
---|
| 105 | dw=0; |
---|
| 106 | |
---|
| 107 | // integrace |
---|
| 108 | if (vypocet<5) // Euler |
---|
| 109 | { x[0]+=dIsx*h; |
---|
| 110 | x[1]+=dIsy*h; |
---|
| 111 | x[2]+=dw*h; |
---|
| 112 | x[3]+=dTheta*h; |
---|
| 113 | } |
---|
| 114 | else // Adams 4.radu |
---|
| 115 | { x[0]+=h/24*(55*dIsx-59*dIsx1+37*dIsx2-9*dIsx3); |
---|
| 116 | x[1]+=h/24*(55*dIsy-59*dIsy1+37*dIsy2-9*dIsy3); |
---|
| 117 | x[2]+=h/24*(55*dw-59*dw1+37*dw2-9*dw3); |
---|
| 118 | x[3]+=h/24*(55*dTheta-59*dTheta1+37*dTheta2-9*dTheta3); |
---|
| 119 | } |
---|
| 120 | |
---|
| 121 | // uprava theta tak, aby bylo stale v rozmezi (-pi,pi) |
---|
| 122 | if (x[3]>M_PI/Thetaref) x[3]-=(2*M_PI/Thetaref); |
---|
| 123 | if (x[3]<-M_PI/Thetaref) x[3]+=(2*M_PI/Thetaref); |
---|
| 124 | |
---|
| 125 | // zamena diferenci |
---|
| 126 | dIsx3=dIsx2;dIsx2=dIsx1;dIsx1=dIsx; |
---|
| 127 | dIsy3=dIsy2;dIsy2=dIsy1;dIsy1=dIsy; |
---|
| 128 | dTheta3=dTheta2;dTheta2=dTheta1;dTheta1=dTheta; |
---|
| 129 | dw3=dw2;dw2=dw1;dw1=dw; |
---|
| 130 | |
---|
| 131 | // vypocet Isd, Isq |
---|
| 132 | x[6]=x[0]*cos(x[3]*Thetaref)+x[1]*sin(x[3]*Thetaref); // Isd |
---|
| 133 | x[7]=x[1]*cos(x[3]*Thetaref)-x[0]*sin(x[3]*Thetaref); // Isq |
---|
| 134 | |
---|
| 135 | // vypocet toku statoru Fsd - neni normovany!! |
---|
| 136 | x[5]=Ls*x[6]*Iref+Fmag; |
---|
| 137 | |
---|
| 138 | // Vypocet momentu |
---|
| 139 | x[4]=kp*p*Fmag*Iref/Mref*(x[1]*cos(x[3]*Thetaref)-x[0]*sin(x[3]*Thetaref)); |
---|
| 140 | |
---|
| 141 | // vypocet zatezneho uhlu |
---|
| 142 | Fsq=Ls*x[7]*Iref; |
---|
| 143 | x[10]=uhel(x[5],Fsq); |
---|
| 144 | } |
---|