📄 follow.c

📁 2005年全国大学生电子设计竞赛论文集
💻 C
字号:
#include "spce061v004.h"
#include "main.h"

extern int SysStatus;
extern unsigned int TimeCountX,TimeCountY;

int FSetPosition=0,FSetValue[6]={0};

int StartPointX,StartPointY;
int PrePointX=0,PrePointY=0;
int NextPointX,NextPointY;
int Step;

//=========================================================================================
//跟踪窗口
//设定起点
//=========================================================================================
void FollowPage(void)
{
	unsigned int HZCode[10]={0xC6F0,0xB5E3,0xC9E8,0xD6C3,
							 0x2020,0x2020,0x2020,0x2020,0x2020,0x2020};		//为液晶行赋初值	

	LCDWriteLine(1,HZCode);
	
	HZCode[0]=0xC6F0;HZCode[1]=0xB5E3;HZCode[2]=0xA3B0;HZCode[3]=0xA3B0;
	HZCode[4]=0x2020;HZCode[5]=0xA3B0;HZCode[6]=0xA3B0;HZCode[7]=0xA3B0;
	LCDWriteLine(2,HZCode);
	
	HZCode[0]=0xB7BD;HZCode[1]=0xCFF2;HZCode[2]=0xA1FA;HZCode[3]=0x2020;//A1FB,A1FC,A1FD
	HZCode[4]=0x2020;HZCode[5]=0x2020;HZCode[6]=0x2020;HZCode[7]=0x2020;
	LCDWriteLine(3,HZCode);
	
	FSetValue[0]=0;FSetValue[1]=0;
	FSetValue[2]=0;FSetValue[3]=0;FSetValue[4]=0;
	FSetValue[5]=0;
	
	WriteByte(0x92,0);	
	WriteByte(0x0d,0);
	FSetPosition=0;

	SysStatus=SYS_ST_FOLLOW;
	
}

//=========================================================================================
//
//=========================================================================================
void SetFollow(int key)
{
	unsigned int HZCode[10]={0x2020,0x2020,0x2020,0x2020,
							 0x2020,0x2020,0x2020,0x2020,0x2020,0x2020};		//为液晶行赋初值	
							 
	int x1,x2,x3,y1,y2,y3;
	switch(key)
	{
		case 0x000d:			//up/down
			FSetValue[FSetPosition]++;
				if((FSetPosition==2)&&(FSetValue[FSetPosition]>1))
					FSetValue[FSetPosition]=0;
				else if((FSetPosition==0)&&(FSetValue[FSetPosition]>8))
					FSetValue[FSetPosition]=0;
				else if((FSetPosition==5)&&(FSetValue[FSetPosition]>3))
					FSetValue[FSetPosition]=0;
				else if(FSetValue[FSetPosition]>9)
					FSetValue[FSetPosition]=0;
				
			HZCode[0]=0xC6F0;HZCode[1]=0xB5E3;HZCode[2]=0xA3B0+FSetValue[0];HZCode[3]=0xA3B0+FSetValue[1];
			HZCode[4]=0x2020;HZCode[5]=0xA3B0+FSetValue[2];HZCode[6]=0xA3B0+FSetValue[3];HZCode[7]=0xA3B0+FSetValue[4];
			LCDWriteLine(2,HZCode);
			
			HZCode[0]=0xB7BD;HZCode[1]=0xCFF2;HZCode[2]=0xA1FA+FSetValue[5];HZCode[3]=0x2020;//A1FB,A1FC,A1FD
			HZCode[4]=0x2020;HZCode[5]=0x2020;HZCode[6]=0x2020;HZCode[7]=0x2020;
			LCDWriteLine(3,HZCode);

			if(FSetPosition<2)
				WriteByte(0x92+FSetPosition,0);
			else if((FSetPosition>1)&&(FSetPosition<5))
				WriteByte(0x93+FSetPosition,0);
			else if(FSetPosition==5)
				WriteByte(0x8A,0);
			WriteByte(0x0d,0);

			break;
		case 0x000b:			//left/right
			FSetPosition++;
			if(FSetPosition>5) FSetPosition=0;
			if(FSetPosition<2)
				WriteByte(0x92+FSetPosition,0);
			else if((FSetPosition>1)&&(FSetPosition<5))
				WriteByte(0x93+FSetPosition,0);
			else if(FSetPosition==5)
				WriteByte(0x8A,0);
			WriteByte(0x0d,0);
			break;
		case 0x0007:			//up
			WriteByte(0x0c,0);
			seek();
			break;
		case 0x0006:			//exit
			*P_IOA_Data=*P_IOB_Buffer&0x0fff;
			OpenMenu(1,1);					 //返回菜单页1
			SysStatus=SYS_ST_MENU;
			WriteByte(0x0c,0);
			break;
	}

}

//=========================================================================================
//
//=========================================================================================
int seek()
{
	int id,ic;
	int LineX[9],LineY[9];
	int IDetect;
	int DirR;
	int Lx,Rx;
	int hisPointX[3]={0},hisPointY[3]={0};
	
	StartPointX=FSetValue[0]*100+FSetValue[1]*10;
	StartPointY=FSetValue[2]*1000+FSetValue[3]*100+FSetValue[4]*10;
	
	IDetect=0;
	
	if((*P_IOB_Data & 0x0100)==0)
	{	
		PrePointX=StartPointX;PrePointY=StartPointY;
l2:	
		LineX[0]=StartPointX;LineY[0]=StartPointY;
		LineX[1]=StartPointX;LineY[1]=StartPointY+10;
		LineX[2]=StartPointX+10;LineY[2]=StartPointY;
		LineX[3]=StartPointX;LineY[3]=StartPointY-10;
		LineX[4]=StartPointX-10;LineY[4]=StartPointY;
//		LineX[5]=StartPointX-1;LineY[5]=StartPointY-1;
//		LineX[6]=StartPointX-1;LineY[6]=StartPointY;
//		LineX[7]=StartPointX-1;LineY[7]=StartPointY+1;
//		LineX[8]=StartPointX;LineY[8]=StartPointY+1;
		
		for(DirR=0;DirR<4;DirR++)
		{
			Lx=LTime(LineX[DirR],LineY[DirR],LineX[DirR+1],LineY[DirR+1]);
			Rx=RTime(LineX[DirR],LineY[DirR],LineX[DirR+1],LineY[DirR+1]);
			
			motorgo(Lx,Rx);
			
			IDetect=*P_IOB_Data & 0x0100;
			if(IDetect==0)
			{
				if((LineX[DirR+1]!=hisPointX[2])||(LineY[DirR+1]!=hisPointY[2]))
					break;
			}
		}
		
		
		if(DirR>=4)
		{
			Lx=LTime(LineX[4],LineY[4],LineX[0],LineY[0]);
			Rx=RTime(LineX[4],LineY[4],LineX[0],LineY[0]);
			
			motorgo(Lx,Rx);
			
			LineX[0]=StartPointX;LineY[0]=StartPointY;
			LineX[1]=StartPointX-20;LineY[1]=StartPointY+20;
			LineX[2]=StartPointX;LineY[2]=StartPointY+20;
			LineX[3]=StartPointX+20;LineY[3]=StartPointY+20;
			LineX[4]=StartPointX+20;LineY[4]=StartPointY;
			LineX[5]=StartPointX+20;LineY[5]=StartPointY-20;
			LineX[6]=StartPointX;LineY[6]=StartPointY-20;
			LineX[7]=StartPointX-20;LineY[7]=StartPointY-20;
			LineX[8]=StartPointX-20;LineY[8]=StartPointY;
			
			for(DirR=0;DirR<8;DirR++)
			{
				Lx=LTime(LineX[DirR],LineY[DirR],LineX[DirR+1],LineY[DirR+1]);
				Rx=RTime(LineX[DirR],LineY[DirR],LineX[DirR+1],LineY[DirR+1]);
				
				motorgo(Lx,Rx);
				
				IDetect=*P_IOB_Data & 0x0100;
				if(IDetect==0)
				{
					if((LineX[DirR+1]!=hisPointX[1])||(LineY[DirR+1]!=hisPointY[1]))
						break;
				}
			}
			if(DirR>=8)
			{
				return(-1);	
			}
			else
			{
				DirR++;
			}
		}
		else
		{
			DirR=(DirR+1)*2;
		}
		
		if(DirR<9)
		{
			PrePointX=StartPointX;PrePointY=StartPointY;
			StartPointX=LineX[DirR];StartPointY=LineY[DirR];
			
l1:			
			hisPointX[0]=hisPointX[1];hisPointY[0]=hisPointY[1];
			hisPointX[1]=hisPointX[2];hisPointY[1]=hisPointY[2];
			hisPointX[2]=PrePointX;hisPointY[2]=PrePointY;
			
			Direct(DirR);
			
			if(*P_IOB_Data & 0x0100)
			{
				Lx=LTime(NextPointX,NextPointY,StartPointX,StartPointY);
				Rx=RTime(NextPointX,NextPointY,StartPointX,StartPointY);
				motorgo(Lx,Rx);
				goto l2;
			}
			else
			{
				PrePointX=StartPointX;PrePointY=StartPointY;
				StartPointX=NextPointX;StartPointY=NextPointY;
				goto l1;
			}
		}
		else
		{
		}
	}
}

//=========================================================================================
//
//=========================================================================================
void motorgo(int TimeLx,int TimeRx)
{
	unsigned D;
	
	TimeLx=(TimeLx/4)*4;TimeRx=(TimeRx/4)*4;
	while((TimeLx!=0)||(TimeRx!=0))
	{
		if(TimeLx>0)
		{
			D=*P_IOA_Buffer&0xfff0;
			D=D|0x0009;
			*P_IOA_Data=D;
			Delay(9000);		
		*P_Watchdog_Clear=C_WDTCLR;
			D=*P_IOA_Buffer&0xfff0;
			D=D|0x0003;
			*P_IOA_Data=D;
			Delay(9000);		
		*P_Watchdog_Clear=C_WDTCLR;
			D=*P_IOA_Buffer&0xfff0;
			D=D|0x0006;
			*P_IOA_Data=D;
			Delay(9000);		
		*P_Watchdog_Clear=C_WDTCLR;
			D=*P_IOA_Buffer&0xfff0;
			D=D|0x000c;
			*P_IOA_Data=D;
			Delay(9000);
		*P_Watchdog_Clear=C_WDTCLR;
			TimeLx=TimeLx-4;
		}
		if(TimeLx<0)
		{
			D=*P_IOA_Buffer&0xfff0;
			D=D|0x000c;
			*P_IOA_Data=D;
			Delay(9000);		
		*P_Watchdog_Clear=C_WDTCLR;
			D=*P_IOA_Buffer&0xfff0;
			D=D|0x0006;
			*P_IOA_Data=D;
			Delay(9000);		
		*P_Watchdog_Clear=C_WDTCLR;
			D=*P_IOA_Buffer&0xfff0;
			D=D|0x0003;
			*P_IOA_Data=D;
			Delay(9000);		
		*P_Watchdog_Clear=C_WDTCLR;
			D=*P_IOA_Buffer&0xfff0;
			D=D|0x0009;
			*P_IOA_Data=D;
			Delay(9000);
		*P_Watchdog_Clear=C_WDTCLR;
			TimeLx=TimeLx+4;
		}
		if(TimeRx>0)
		{
			D=*P_IOA_Buffer&0xff0f;
			D=D|0x0090;
			*P_IOA_Data=D;
			Delay(9000);		
		*P_Watchdog_Clear=C_WDTCLR;
			D=*P_IOA_Buffer&0xff0f;
			D=D|0x0030;
			*P_IOA_Data=D;
			Delay(9000);		
		*P_Watchdog_Clear=C_WDTCLR;
			D=*P_IOA_Buffer&0xff0f;
			D=D|0x0060;
			*P_IOA_Data=D;
			Delay(9000);		
		*P_Watchdog_Clear=C_WDTCLR;
			D=*P_IOA_Buffer&0xff0f;
			D=D|0x00c0;
			*P_IOA_Data=D;
			Delay(9000);
		*P_Watchdog_Clear=C_WDTCLR;
			TimeRx=TimeRx-4;
		}
		if(TimeRx<0)
		{
			D=*P_IOA_Buffer&0xff0f;
			D=D|0x00c0;
			*P_IOA_Data=D;
			Delay(9000);		
		*P_Watchdog_Clear=C_WDTCLR;
			D=*P_IOA_Buffer&0xff0f;
			D=D|0x0060;
			*P_IOA_Data=D;
			Delay(9000);		
		*P_Watchdog_Clear=C_WDTCLR;
			D=*P_IOA_Buffer&0xff0f;
			D=D|0x0030;
			*P_IOA_Data=D;
			Delay(9000);		
		*P_Watchdog_Clear=C_WDTCLR;
			D=*P_IOA_Buffer&0xff0f;
			D=D|0x0090;
			*P_IOA_Data=D;
			Delay(9000);
		*P_Watchdog_Clear=C_WDTCLR;
			TimeRx=TimeRx+4;
		}
	}
}

//=========================================================================================
//
//=========================================================================================
void Direct(int ddr)
{
	int Lx,Rx;

	switch( ddr)
	{
		case 1:
			NextPointX=StartPointX-10;NextPointY=StartPointY+10;
			break;
		case 2:
			NextPointX=StartPointX;NextPointY=StartPointY+10;
			break;
		case 3:
			NextPointX=StartPointX+10;NextPointY=StartPointY+10;
			break;
		case 4:
			NextPointX=StartPointX+10;NextPointY=StartPointY;
			break;
		case 5:
			NextPointX=StartPointX+10;NextPointY=StartPointY-10;
			break;
		case 6:
			NextPointX=StartPointX;NextPointY=StartPointY-10;
			break;
		case 7:
			NextPointX=StartPointX-10;NextPointY=StartPointY-10;
			break;
		case 8:
			NextPointX=StartPointX-10;NextPointY=StartPointY;
			break;
	}
	Lx=LTime(StartPointX,StartPointY,NextPointX,NextPointY);
	Rx=RTime(StartPointX,StartPointY,NextPointX,NextPointY);
	motorgo(Lx,Rx);
}
💿 文件大小 5653 K
👤 上传用户 tsdtz
📂 所属分类单片机开发
🏷️ 相关标签

#2005 #大学生电子设计竞赛 #论文
⌨️ 快捷键说明

复制代码 Ctrl + C
搜索代码 Ctrl + F
全屏模式 F11
切换主题 Ctrl + Shift + D
显示快捷键 ?
增大字号 Ctrl + =
减小字号 Ctrl + -