ctrl.c

2005年全国大学生电子设计竞赛论文集

#include "spce061v004.h"
#include "Ctrl.h"
#include "math.h"

#define LA    14
#define BB    115
#define RA    95
#define CC    1.5
#define Step_Len   0.01715
#define Dot_Num    310   //设定线点数
#define NN         100   //脉冲延时
#define Dot_Step   0.5
#define Dir_LP    *P_IOB_Buffer=*P_IOB_Buffer|0x0200
#define Dir_LN    *P_IOB_Buffer=*P_IOB_Buffer&0xfdff
#define Dir_RP    *P_IOB_Buffer=*P_IOB_Buffer|0x0800
#define Dir_RN    *P_IOB_Buffer=*P_IOB_Buffer&0xf7ff
#define Pulse_LP  *P_IOB_Buffer=*P_IOB_Buffer|0x0100
#define Pulse_LN  *P_IOB_Buffer=*P_IOB_Buffer&0xfeff
#define Pulse_RP  *P_IOB_Buffer=*P_IOB_Buffer|0x0400
#define Pulse_RN  *P_IOB_Buffer=*P_IOB_Buffer&0xfbff
#define NO1		0x0001
#define NO2		0x0002
#define NO3		0x0004
#define NO4		0x0008
#define NO5		0x0010
#define NO6		0x0020
#define NO7		0x0040
#define NO8		0x0080
#define Step1   0.5
#define Step2	1.0

unsigned int X_Lable[2]={0,0};
unsigned int Y_Lable[3]={0,0,0};
unsigned int X_Locate[3]={0,0,0};
unsigned int Y_Locate[4]={0,0,0,0};
unsigned int Setting=0;
unsigned int Change=4; //0: X十位   1:X个位   2:Y百位   3:Y十位    4:Y个位
unsigned int Mode=0;//0:停止   1：设定线   2：圆周   3:点    4:轨迹  
unsigned int Stop=0;
unsigned int Change1=0;

extern Time_Consume;
float X, Y;       //  当前物体所在位置
float Len_Left, Len_Right;//左线长度，右线长度

//=========================================================================================
//系统初始化
//=========================================================================================
void System_Init( )
{
      
      //A口低八位输入扫描     高八位显示数据位
      *P_IOA_Dir=0xff00;
      *P_IOA_Attrib=0xff00;
      *P_IOA_Data=0x0000;
      
      //B2,外部中断，按键扫描       B8－B11电机脉冲   B12－B15LCD控制
      *P_IOB_Dir=0xff00;
      *P_IOB_Attrib=0xff00;
      *P_IOB_Data=0x0fff;
      
      *P_INT_Ctrl=C_IRQ3_EXT1;  //C_IRQ1_TMA++C_IRQ3_KEY
      __asm("INT IRQ");
}

//=========================================================================================
//
//=========================================================================================
void Show_Init( )
{
   LCD_Load("END:(00,000)    000S(00.0,000.0)");
   LCD_Update( );
}

//=========================================================================================
//
//=========================================================================================
void Display_Line( )
{
	LCD_Display(4,'s');
	LCD_Display(5,'i');
	LCD_Display(6,'n');
	LCD_Display(7,' ');
	LCD_Display(8,' ');
	LCD_Display(9,' ');
	LCD_Display(10,' ');
	LCD_Display(11,' ');
	
}

//=========================================================================================
//
//=========================================================================================
void Display0( )
{
	LCD_Display(4,'(');
	LCD_Display(5,'0');
	LCD_Display(6,'0');
	LCD_Display(7,',');
	LCD_Display(8,'0');
	LCD_Display(9,'0');
	LCD_Display(10,'0');
	LCD_Display(11,')');
	
}

//=========================================================================================
//
//=========================================================================================
void Play(unsigned int index)
{
    unsigned int clock;
    clock=*P_SystemClock;		     						
    SACM_S480_Initial(1);
    SACM_S480_Play(index,1, 3); 
    while(SACM_S480_Status()&0x01) 
    { 
        SACM_S480_ServiceLoop();
        *P_Watchdog_Clear=0x0001;
    }
    *P_INT_Ctrl=C_IRQ3_EXT1;
    *P_SystemClock=clock;
}

//=========================================================================================
//播报运动时间
//=========================================================================================
void Play_Time( )
{
						 Play(5);
					     if(Time_Consume/100)
					     {
					     	Play(Time_Consume/100+7);
					     	Play(18);
					     }
					     
					     if(Time_Consume%100/10)
					     {
					     	Play(Time_Consume%100/10+7);
					     	Play(17);
					     	if(Time_Consume%10)
					     		Play(Time_Consume%10+7);
					     }
					     else 
					     {
					     	if(Time_Consume/100==0)
					     		Play(Time_Consume+7);
					     	else
					     	{
					     		if(Time_Consume%10)
					     		{
					     			Play(7);
					     			Play(Time_Consume%10+7);
					     		}
					     	}
					       }
					     Play(6);
}					     
	
//=========================================================================================
//电机
//=========================================================================================
void Motor(float x_next, float y_next)
{
	float len_left, len_right;
	float L_change, R_change;
        int steps_L, steps_R;
	unsigned int xx,yy;
 	len_left=(x_next+LA)*(x_next+LA)+(BB-y_next)*(BB-y_next)+CC*CC;
 	len_right=(RA-x_next)*(RA-x_next)+(BB-y_next)*(BB-y_next)+CC*CC;
  	len_left=sqrt(len_left);
  	len_right=sqrt(len_right);
 	L_change=len_left-Len_Left;   //大于0，放线
 	R_change=len_right-Len_Right;
 	X=x_next;
 	Y=y_next;
 	Len_Left=len_left;
 	Len_Right=len_right;
 	
 	if(L_change>=0)       //得到电机转动步数
 		Dir_LP;
 	else 
 	{
 		Dir_LN;
 		L_change=-L_change;
 	} 		
 	steps_L=(int)(L_change/Step_Len+0.5);
 	
 	if(R_change>=0)
 		Dir_RP;
 	else 
 	{
 		Dir_RN;
 		R_change=-R_change;
 	}
 	steps_R=(int)(R_change/Step_Len+0.5);   //步数：四舍五入取整
 	
 	Pulse_LP;   
 	Pulse_RP;
 	while((steps_L>0)||(steps_R>0))
 	{	
 		Delay(NN);     //??????????????????????????//
 		if(steps_L>0)
 		{
 			Pulse_LN;
 			steps_L--;
 		}
 		if(steps_R>0)
 		{
 			Pulse_RN;
 			steps_R--;
 		}
 		Delay(NN);
 		Pulse_LP;
 		Pulse_RP;
 		
 	} 
 	xx=(int)(x_next*10);
 	LCD_Display(21,xx/100+0x0030);
 	LCD_Display(22,xx%100/10+0x0030);
 	LCD_Display(24,xx%10+0x0030);
 	yy=(int)(y_next*10);
 	LCD_Display(26,yy/1000+0x0030);
 	yy%=1000;
 	LCD_Display(27,yy/100+0x0030);
 	yy%=100;
 	LCD_Display(28,yy/10+0x0030);
 	yy%=10;
 	LCD_Display(30,yy+0x0030); 		
}
           
//=========================================================================================
//按不同路线运动
//=========================================================================================
void Move( )
{
	unsigned int i=0;
	unsigned int num;  //直线的点数
	float k;           //直线斜率
	float xxx,yyy,xx1,yy1,xx,yy;
			switch(Mode)
			{
				case 0: break;
				case 1:{  //设定线  正弦
					        xx=(float)(X_Locate[2]*10+X_Locate[1]);
					        yy=(float)(Y_Locate[3]*100+Y_Locate[2]*10+Y_Locate[1]);
					    	X=0.0;
			         		Y=0.0;
			         		Len_Left=(X+LA)*(X+LA)+(BB-Y)*(BB-Y);
 					        Len_Right=(RA-X)*(RA-X)+(BB-Y)*(BB-Y);
                 		                Len_Left=sqrt(Len_Left);
 					        Len_Right=sqrt(Len_Right);
			         	//	xx=X_Lable[1]*10+X_Lable[0];
			         	//	yy=Y_Lable[2]*100+Y_Lable[1]*10+Y_Lable[0];
			         		k=yy/xx;
						Time_Consume=0;
			        	       	*P_INT_Ctrl=*P_INT_Mask|C_IRQ5_2Hz;
			        	       	if(k<=1)//按X轴分段
			 			{
			 				num=(int)(xx/Dot_Step+0.5);			         			
			         			for(i=1;i<=num;i++)
			         	 		{
			         				if(Setting)
					    			break;
			         				Motor(i*Dot_Step,i*Dot_Step*k);
			         			}
			         			
			        	        }
			        	        else   //按Y轴分段
			        	        {
			        	        	num=(int)(yy/Dot_Step+0.5);
			        	        	for(i=1;i<=num;i++)
			        	        	{
			         				if(Setting)
					    			break;			        	        		
			        	        		Motor(i*Dot_Step/k,i*Dot_Step);
			        	        	}
			        	        }
					    X=xx;
					    Y=yy;
					    yy=yy-40;
					    //???????????????????????????/
					    Len_Left=(X+LA)*(X+LA)+(BB-Y)*(BB-Y)+CC*CC;
 					    Len_Right=(RA-X)*(RA-X)+(BB-Y)*(BB-Y)+CC*CC;
                 		            Len_Left=sqrt(Len_Left);
 					    Len_Right=sqrt(Len_Right);
					    i=0;
					     while(i<Dot_Num-1)
					    {
					      	i++;
					    	Motor(xx+Curve_X[i],Curve_Y[i]+yy);
					    	if(Setting)
					    		break;
					     }
					     Mode=0;
					     *P_INT_Ctrl=*P_INT_Mask&(~C_IRQ5_2Hz);
					     Play_Time( );
					     break;
					 }
			        case 2:  //圆
			       		{     
			       		      float xx=X_Lable[1]*10+X_Lable[0];
			       		      float yy=Y_Lable[2]*100+Y_Lable[1]*10+Y_Lable[0];
			        	      X=xx+Circle_X[0];
			        	      Y=yy+Circle_Y[0];
			        	      Len_Left=(X+LA)*(X+LA)+(BB-Y)*(BB-Y)+CC*CC;
 					      Len_Right=(RA-X)*(RA-X)+(BB-Y)*(BB-Y)+CC*CC;
                 		              Len_Left=sqrt(Len_Left);
 					      Len_Right=sqrt(Len_Right);
			        	      i=0;
			        	      Time_Consume=0;
			        	      *P_INT_Ctrl=*P_INT_Mask|C_IRQ5_2Hz;
			        	      while(i<360)
			        	      {
			        	      	i++;
			        	      	Motor(xx+Circle_X[i],yy+Circle_Y[i]);
			        	      	if(Setting)
					    		break;
			        	       }
			        	       Mode=0;
			        	       *P_INT_Ctrl=*P_INT_Mask&(~C_IRQ5_2Hz);
			        	       Play_Time( );
			        	       break;
			        	  }
			         
			         case 3:{	yyy=Y_Locate[3]*100+Y_Locate[2]*10+Y_Locate[1];
			         		xxx=X_Locate[2]*10+X_Locate[1];
			         		yy1=Y_Lable[2]*100+Y_Lable[1]*10;
			         		xx1=X_Lable[1]*10+X_Lable[0];
			         		yy1=yy1-yyy;
			         		xx1=xx1-xxx;
			         		k=yy1/xx1;
			         		X=xxx;
			         		Y=yyy;
			         		Len_Left=(X+LA)*(X+LA)+(BB-Y)*(BB-Y)+CC*CC;
 					        Len_Right=(RA-X)*(RA-X)+(BB-Y)*(BB-Y)+CC*CC;
                 		                Len_Left=sqrt(Len_Left);
 					        Len_Right=sqrt(Len_Right);
			         		
						Time_Consume=0;
			        	       	*P_INT_Ctrl=*P_INT_Mask|C_IRQ5_2Hz;
			        	       	
			        	       	if(k<0)
			        	       		k=-k;			        	      			        	       	
			        	       	if((xx1>0)&&(yy1>0))
			        	       	{
			        	       	   if(k<=1)
			        	       	   {
			        	       	 	num=(int)(xx1/Dot_Step+0.5);
			        	       	 	for(i=1;i<=num;i++)
			         	 		{
			         				if(Setting)
					    			break;
			         				Motor(X+Dot_Step,Y+k*Dot_Step);
			         			}
			        	       	 
			        	       	    }
			        	       	  
			        	       	   else 
			        	       	    {
			        	       	 	num=(int)(yy1/Dot_Step+0.5);
			        	       	 	for(i=1;i<=num;i++)
			         	 		{
			         				if(Setting)
					    			break;
			         				Motor(X+Dot_Step/k,Y+Dot_Step);
			         			}
			        	       	 
			        	       	     }
			        	       	    }
			        	       else if((xx1<0)&&(yy1<0))
			        	       	 {
			        	       	     xx1=-xx1;
			        	       	     yy1=-yy1;
			        	       	     if(k<=1)
			        	       	   {
			        	       	 	num=(int)(xx1/Dot_Step+0.5);
			        	       	 	for(i=1;i<=num;i++)
			         	 		{
			         				if(Setting)
					    			break;
			         				Motor(X-Dot_Step,Y-k*Dot_Step);	
			         			}
			        	       	 
			        	       	    }
			        	       	   else 
			        	       	    {
			        	       	 	num=(int)(yy1/Dot_Step+0.5);
			        	       	 	for(i=1;i<=num;i++)
			         	 		{
			         				if(Setting)
					    			break;
			         				Motor(X-Dot_Step/k,Y-Dot_Step);	
			         			}
			        	       	 
			        	       	     }
			        	       	  }
			        	       	  else if((xx1<0)&&(yy1>0))
			        	       	  {
			        	       	        xx1=-xx1;
			        	       	         if(k<=1)
			        	       	       {
			        	       	 	num=(int)(xx1/Dot_Step+0.5);
			        	       	 	for(i=1;i<=num;i++)
			         	 		{
			         				if(Setting)
					    			break;
			         				Motor(X-Dot_Step,Y+k*Dot_Step);
			         			}
			        	       	 	
			        	       	       }
			        	       	     else 
			        	       	      {
			        	       	 	num=(int)(yy1/Dot_Step+0.5);
			        	       	 	for(i=1;i<=num;i++)
			         	 		{
			         				if(Setting)
					    			break;
			         				Motor(X-Dot_Step/k,Y+Dot_Step);
			         			}
			        	       	 	
			        	       	     }
			        	       	  }
			        	       	  else if((xx1>0)&&(yy1<0))
			        	       	  {
			        	       	  	yy1=-yy;
			        	       	  	if(k<=1)
			        	       	   {
			        	       	 	num=(int)(xx1/Dot_Step+0.5);
			        	       	 	for(i=1;i<=num;i++)
			         	 		{
			         				if(Setting)
					    			break;
			         				Motor(X+Dot_Step,Y-k*Dot_Step);
			         			}
			        	       	 	
			        	       	    }
			        	       	   else 
			        	       	    {
			        	       	 	num=(int)(yy1/Dot_Step+0.5);
			        	                for(i=1;i<=num;i++)
			         	 		{
			         				if(Setting)
					    		        	break;
			         				Motor(X+Dot_Step/k,Y-Dot_Step);
			         			}
			        	       	 	
			        	       	     }
			        	       	    }
			        	               Mode=0;
			        	        	*P_INT_Ctrl=*P_INT_Mask&(~C_IRQ5_2Hz);
			        	        	
			        	        	Play_Time( ); 
			        	        	break;	         			
			         	}
			         case 4:{  	
			         			//寻迹			         			
			         			X=X_Locate[2]*10+X_Locate[1];
			         			Y=Y_Locate[3]*100+Y_Locate[2]*10+Y_Locate[1];
			         			Len_Left=(X+LA)*(X+LA)+(BB-Y)*(BB-Y)+CC*CC;
 					   		Len_Right=(RA-X)*(RA-X)+(BB-Y)*(BB-Y)+CC*CC;
                 		          		Len_Left=sqrt(Len_Left);
 					   		Len_Right=sqrt(Len_Right);
			         			Time_Consume=0;
			         			*P_INT_Ctrl=*P_INT_Mask|C_IRQ5_2Hz;
			         			while(Stop==0)
			         			{
			         			 Orientation( );
			         			 *P_Watchdog_Clear=0x0001;
			         			 if(Setting)
			         			 	break;
			         		         }
			         		         Mode=0;
			        	        	*P_INT_Ctrl=*P_INT_Mask&(~C_IRQ5_2Hz);	
			        	        	Play_Time( );
			         		         break;		         		
			         	     }
			        default: break;
			   }