main.c

基于TMS320LF2407的一个倒立控制程序

/**** sample alpha by electrograph
      Using Pendulum Function board designed 07.03.26 
      [e-mail]   07.05.23    ****/

#include "Regs240x.h"
#include "Initsystem.h"
#include "Sample.h"
#include "Filter.h"
#include "Control.h"
#include "PWM.h"
#include "Send.h"
#include "ServoControl.h"
#include "JaoZHControl.h"      
#include "sinsource.h"

enum Mode{Calibration,PCControl,InvPdl,Servo,StopControl};
enum Mode ControlMode;

int SampleTime=0,ControlTime=0,SendTime=0; 
int PCdata;
    
int alpha,theta;
int init_alpha,init_theta; 

//inverted pendulum coeff
float FeedbackCoeff[4]={-0.3887f, -92.0636f, -7.7248f, -10.5986f};   
/**ServoPara:input,K,H,A,StartOmega,EndOmega,deltaW,T1,T2 **/
float ServoPara[9]={1.0, 1.0, 1.0, 2.0, 0.1, 5.0, 0.1,0.083,0.0041};    


float DeadZone=5.9f;
float MaxCtrlVol=12.0f; 
int   UpdatecoffFlag=0,UpdateVolFlag=0,UpdateServoFlag=0;

int UpdatecoffBuf[8] = {0};
int UpdateVolBuf[4]={0};
int UpdateServoBuf[18]={0};


int UpdatecoffNum = 0;
int UpdateVolNum = 0;
int UpdateServoNum = 0;

int ServoExpIndex = 0;

unsigned char msg_buf[5]; 

/*******servo ***********/
#define Ts 0.01f                //采样间隔
#define ATurn_Rad  0.00322f     //AD采样值转变成弧度值

int count=0;

float Phase=0.0f;    //相位
float f=0.0f;        //频率
/************************/

void main()
{   
	int dir,duty,sduty;
	int i,Filter_buf[40]={0};
	const int CaliNum = 40;
	const int SampNum = 12;
	float CtrlValue,sfctrl; 
	int   sendnum=0; /* 通信序号 */
	int TempData;
	
	/****** servo *******/
	float StandValue;     //输入的跟踪信号
	float rin;
	float K,H,A,hz_start,hz_end,hz_delta;
	float T1,T2;
	float Xl = 0,Xn = 0,Yl = 0;
	
	/*******************/
	
	Initsystem(); 
  	
	ControlMode=Calibration;
	
	*IFR = 0x0FF;            
	*IMR = 0x03;                     
	
	init_alpha=490;      
	init_theta=539;       
	
    asm(" CLRC INTM ");

	while(1)
	{ 
	  if(Calibration==ControlMode)
	  {
	      *CMPR1 = 1000;             /* do not output */
	      for(i=CaliNum-1;i>=0;i--)
	          Filter_buf[i]=SampleAlpha();
	        
	      init_alpha=Filter(Filter_buf,CaliNum);
	    
	      for(i=CaliNum-1;i>=0;i--)
	          Filter_buf[i]=Sampletheta();
	        
	      init_theta=Filter(Filter_buf,CaliNum);
	              
	      msg_buf[0] = ((init_alpha>>4)&0x30)|((init_theta>>8)&0x03)|0x0C0;
	      msg_buf[1] = init_alpha&0xff;
	      msg_buf[2] = init_theta&0xff;
	      msg_buf[4] = 0;
	  }
	  
	  if(StopControl==ControlMode)
	  {
	  	  *CMPR1 = 1000;             /* do not output */
	  } 
	   	  
	  if(SampleTime)
	  {
	     for(i=SampNum-1;i>=0;i--)
	        Filter_buf[i]=SampleAlpha();
	        
	     alpha=Filter(Filter_buf,SampNum);
	    
	     for(i=SampNum-1;i>=0;i--)
	        Filter_buf[i]=Sampletheta();
	        
	     theta=Filter(Filter_buf,SampNum);
		
	     msg_buf[0] = ((alpha>>4)&0x30)|((theta>>8)&0x03);
	     msg_buf[1] = alpha&0xff;
	     msg_buf[2] = theta&0xff;    

	     SampleTime=0;
	  }
	  
	  if(1 <= ControlTime)
	  {
	  	switch(ControlMode)
		{
		 case PCControl:
			 dir=PCdata&0x80;
			 duty=PCdata&0x7F;
			 sduty = duty;
		 break;

	     case InvPdl:
			  CtrlValue = Control();  
		 break;

		 case Servo:
		 /**/
		 	//servo control mode
		 	if(4 == ServoExpIndex)
		 	{
		 		StandValue = sinsource(A,hz_end,hz_delta,Ts); //正弦输入
		 	}
		 	else
		 	{
		 		StandValue = rin/ATurn_Rad;          //阶跃输入
		 		
		 	}
		 	//计算随动控制的控制量
		 	Xn=ServoControl(K,H,StandValue,alpha,init_alpha); 
		 	CtrlValue = Xn; 
		 	
		 	if(5 == ServoExpIndex)
		  	{
			     Yl=JaoZHControl(T1,T2,Ts,Yl,Xl,Xn);  //T1=0.049,T2=0.083
			     Xl = Xn;  
			     CtrlValue = Yl; 
		  	} 
		 	/**/
		 break;	  
	    }/*end_switch*/
	    
	    //sfctrl = CtrlValue; 
	    if(PCControl != ControlMode)
	    {
		   	if( CtrlValue>0 )
		   	{
				CtrlValue += DeadZone;
			}
			else
			{
				CtrlValue -= DeadZone; 
			}
	
			if( CtrlValue>MaxCtrlVol)
			{
				CtrlValue = MaxCtrlVol;
				//sfctrl = MaxCtrlVol;
			}
			if( CtrlValue<-MaxCtrlVol)
			{
				CtrlValue = -MaxCtrlVol;
				//sfctrl = -MaxCtrlVol;
			}
				
			if( CtrlValue<0 ){    
    	        dir = 0x80;    	        
    	        duty = -(int)(100.0*(CtrlValue/MaxCtrlVol));
    	        //sduty =  -(int)(100.0*(sfctrl/MaxCtrlVol));
            }   
            else{  
    	        dir = 0;  
    	        duty = (int)(100.0*(CtrlValue/MaxCtrlVol));
	        }
		}
 
	    PWM(dir,duty);
	    msg_buf[4] = dir | duty;
	    ControlTime=0;  
	}/*end_if(ControlTime)*/
	
     if(1 <= SendTime){
     	 sendnum++;
     	 if(sendnum>100)
     	 {
     	 	sendnum = 1;
     	 }
     	 msg_buf[3] = sendnum;
     	 
		 Send();
    	 SendTime=0;
	 }/*end_if(2<=SendTime)*/
	 
	 if(2 == UpdatecoffFlag && 8==UpdatecoffNum)  //更新底层的反馈系数
	 {
	 	for(i=0;i<4;i++)   
	    {
	        TempData = (UpdatecoffBuf[2*i]&0x7F); 
	        TempData = (TempData<<8) + UpdatecoffBuf[2*i+1];  
	           
	        if(UpdatecoffBuf[2*i]>>7)   //判断符号位
			{
			  	FeedbackCoeff[i] = (-1.0f)*(TempData/100.0f);
			}
			else
			{
			    FeedbackCoeff[i] = (1.0f)*(TempData/100.0f);
			} 
	    }
	    
	    UpdatecoffFlag = 0;    //over to update 
	 }
	 
	 if(2 == UpdateVolFlag && 4==UpdateVolNum)  //更新电压设置，电压不会有负值
	 {
	     TempData = (UpdateVolBuf[0]&0x7F); 
	     MaxCtrlVol=((TempData<<8) + UpdateVolBuf[1])/100.0f;
	       
	     TempData = (UpdateVolBuf[2]&0x7F); 
	     DeadZone=((TempData<<8) + UpdateVolBuf[3])/100.0f;	 
	       
	     UpdateVolFlag = 0;    //over to update 
	 }
	 
	 if((2 == UpdateServoFlag)&&(18 == UpdateServoNum))
	 {
	 	for(i=0;i<9;i++)   
	    {
	        TempData = (UpdateServoBuf[2*i]&0x7F); 
	        TempData = (TempData<<8) + UpdateServoBuf[2*i+1];  
	           
	        if(UpdateServoBuf[2*i]>>7)   //判断符号位
			{
			  	ServoPara[i] = (-1.0f)*(TempData/100.0f);
			}
			else
			{
			    ServoPara[i] = (1.0f)*(TempData/100.0f);
			} 
	    }
	    //ServoPara:input(rin),K,H,A,StartOmega,EndOmega,deltaW,T1,T2 
	    /****/
	     rin = *ServoPara;
		 K = *(ServoPara+1);
		 H = *(ServoPara+2);
		 A = *(ServoPara+3);
		 hz_start = *(ServoPara+4);
		 hz_end = *(ServoPara+5);
		 hz_delta = *(ServoPara+6);;
	     T1= (*(ServoPara+7))/10.0f;
	     T2= (*(ServoPara+8))/100.0f;
	     
	     f=hz_start;
	    /****/
	 	UpdateServoFlag = 0;    //over to update 
	 }
	 		
   }/*end_while  */
}

void interrupt c_int1()  /* SCI interrupt*/
{
	int msg;
 	asm(" SETC INTM ");		
 
	while( !(* SCIRXST & 0x40) ); /*Receive message */ 
	  msg = * SCIRXBUF;
    
    //更新数据区间，收到的msg<=248,为更新的数据，而非命令字
   if((1 == UpdatecoffFlag)&&(msg<=248))   //receive K[4]
	{
		UpdatecoffBuf[UpdatecoffNum] = msg;
		UpdatecoffNum++;
	}
	else if((1 == UpdateVolFlag)&&(msg<=248))   //receive update voltage
	{
		UpdateVolBuf[UpdateVolNum] = msg;
		UpdateVolNum++;
	}
	else if((1== UpdateServoFlag)&&(msg<=248))    //receive update servo para
	{
		UpdateServoBuf[UpdateServoNum] = msg;
		UpdateServoNum++;
	}
	else
	{		
	    switch(msg){      
		    case  229:         
		         ControlMode=Calibration; 
				 break;
			case  230:
				 ControlMode=StopControl;
				 break;
			case  234:         
				 ControlMode=PCControl; 
				 break;
			case  239:         
				 ControlMode=InvPdl; 
				 break;
			case  240:        
				 ControlMode=Servo;  
				 break;
			case  241:
				 ServoExpIndex = 2;
				 break;
			case  242:
				 ServoExpIndex = 3;
				 break;
			case  243:
				 ServoExpIndex = 4;
				 break;
			case  244:
				 ServoExpIndex = 5;
				 break;
				 
			case  249:
				 UpdateServoFlag = 1; //Start to receive update servo coeff
				 UpdateServoNum = 0;
				 break;
			case  250:
				 UpdateServoFlag = 2; //End to receive update servo para
				 break;
			case  251:
				 UpdatecoffFlag = 1; //Start to receive update K[4]
				 UpdatecoffNum = 0;
				 break;
			case  252:
				 UpdatecoffFlag = 2; //end to receive update K[4]
				 break;
			case  253:
				 UpdateVolFlag = 1;  //Start to receive Update DeadZone,MaxCtrlVol;
				 UpdateVolNum = 0;
				 break;
			case  254:
				 UpdateVolFlag = 2; //end to receive Update DeadZone,MaxCtrlVol;
				 
		   default:
				if(((msg&0x7F)<=100)&&(PCControl==ControlMode)) //receive ctrl value
				{
					PCdata=msg;
				}		
		   
		} /*end_switch  */
	}

  asm(" CLRC INTM ");  
}

void interrupt c_int2()  /*Timer interrupt,10ms(100Hz)*/
{
  asm(" SETC INTM"); 
   
   //forbid automaic sample/control/send during calibration
  if((Calibration!=ControlMode)&&(StopControl!=ControlMode)) 
  {
  
    SampleTime=1;     /* Time to sample （10ms）*/
    ControlTime+=1;    /* Time to Control (20ms)*/
  }
  
  SendTime+=1;         /* Time to Send (10ms)*/
  
   * EVBIFRA=0x00FF;  /*clear Timer3 period intrrupt flag */
   asm(" CLRC INTM ");
}