smopos.c

用freescale公司的DSP56F8013芯片实现的PMSM的SVPWM 驱动

#include "Cpu.h"
#include "Events.h"
#include "Smopos.h"
#include "Clarke.h"
#include "Start.h"

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;
  }
  
  
}