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