extern int isalfa; /* Q15 alfa axis phase current */
extern int isbeta; /* Q15 beta axis phase current */
extern int isalfae; /* Q15 alfa axis estimated current */
extern int isbetae; /* Q15 beta axis estimated current */
extern int vsalfa; /* Q6 alaf axis phase voltage */
extern int vsbeta; /* Q6 beta axis phase voltage */
extern int esalfa; /* Q6 alfa axis back EMF */
extern int esbeta; /* Q6 beta axis back EMF */
extern int zalfa; /* Q6 alfa axis sliding control */
extern int zbeta; /* Q6 beta axis sliding control */
extern int quadrant; /* determine the rotor angle quadrant */
extern int thetatemp;
extern int thetae; /* estimated rotor position angle */
extern int tana[180];
extern int gujun;
void Smopos(void)
{
Frac16 tempf1;
Frac16 tempf2;
Frac16 tempf3;
long int temp32;
long int temp33;
int n1;
int n2;
int fsmopos=26693; /* motor dependent plant matrix */
int gsmopos=495; /* motor dependent control gain */
int kslf=1980; /* sliding control filter gain */
int kslide=1010; /* sliding mode control gain */
/* Sliding Mode Current Observer... */
temp32=(L_mult(fsmopos,isalfae))>>16; /* Q15 */
temp33=(L_mult(gsmopos,(vsalfa-esalfa-zalfa)))>>7; /* Q15 */
isalfae=(int)(temp32+temp33); /* Q15 */
temp32=(L_mult(fsmopos,isbetae))>>16; /* Q15 */
temp33=(L_mult(gsmopos,(vsbeta-esbeta-zbeta)))>>7; /* Q15 */
isbetae=(int)(temp32+temp33); /* Q15 */
/* Sliding Control Calculator... */
if(isalfae>isalfa)
zalfa=kslide; /* Q6 */
else if(isalfae<isalfa)
zalfa=-kslide; /* Q6 */
else
zalfa=0; /* Q6 */
if(isbetae>isbeta)
zbeta=kslide; /* Q6 */
else if(isbetae<isbeta)
zbeta=-kslide; /* Q6 */
else
zbeta=0; /* Q6 */
/* Back EMF Calculator... */
temp32=(L_mult(kslf,(zalfa-esalfa)))>>16; /* Q6 */
esalfa=esalfa+(int)(temp32); /* Q6 */
temp33=(L_mult(kslf,(zbeta-esbeta)))>>16; /* Q6 */
esbeta=esbeta+(int)(temp33); /* Q6 */
/* Rotor Angle Calculator... */
/* To determine the quadrant */
tempf2=-esalfa;
tempf1=esbeta;
// gujun=tempf1;
// gujun=tempf2;
if(tempf2>0) /* 0 to 180 degree */
{
if(tempf1>0) /* 0 to 90 degree */
{
if(tempf1>tempf2) /* 0 to 45 degree */
quadrant=1; /* 45 to 90 degree */
else
quadrant=2;
}
else /* 90 to 180 degree */
{
tempf1=-tempf1;
if(tempf1>tempf2)
quadrant=4; /* 135 to 180 degree */
else
quadrant=3; /* 90 to 135 degree */
}
}
else /* 180 to 360 degree */
{
tempf2=-tempf2;
if(tempf1>0) /* 270 to 360 degree */
{
if(tempf1>tempf2)
quadrant=8; /* 315 to 360 degree */
else
quadrant=7; /* 270 to 315 degree */
}
else
{
tempf1=-tempf1; /* 180 to 270 degree */
if(tempf1>tempf2)
quadrant=5; /* 180 to 225 degree */
else
quadrant=6; /* 225 to 270 degree */
}
}
/* To calculate the tan(theta) */
if(tempf1<tempf2)
tempf3=div_s(tempf1,tempf2); /* cotangent value */
else
tempf3=div_s(tempf2,tempf1); /* tangent value */
if(tempf3<tana[23])
n1=0;
else
n1=1;
thetatemp=0;
for(thetatemp=0;thetatemp<23;thetatemp++)
{
if(tempf3>(tana[22*n1+thetatemp]))
n2=thetatemp;
else
break;
}
thetae=23*n1+n2; /* calculalte the rotor position angle (0 to 45) */
switch(quadrant)
{
case(1):
{
thetae=thetae;
}break;
case(2):
{
thetae=90-thetae;
}break;
case(3):
{
thetae=90+thetae;
}break;
case(4):
{
thetae=180-thetae;
}break;
case(5):
{
thetae=180+thetae;
}break;
case(6):
{
thetae=270-thetae;
}break;
case(7):
{
thetae=270+thetae;
}break;
case(8):
{
thetae=360-thetae;
}break;
}
} |
楼主
