📄 func.c

📁 这是在ADIdsp上实现的A型算法和链表程序以及无线通讯的源程序
💻 C
📖 第 1 页 / 共 3 页
字号:
上一页 1 23
//函数名称： ComputeRoadChange()
//功能：计算小车控制指令
//参数：int Degree,角度	int Distance距离
//			无
//返回值：  成功返回真，否则返回假
//本函数中的所有值都是相对迷宫的
/*****************************************************************************/
void ComputeRoadChange(void )
{
	float r,AngleA,AngleB,AngleX;
	float midX,midY;
	float NowDistance=0,NowDegree=0;
	int i,j=0;
	int D_X,D_Y;
	int NowRoadX=-1,NowRoadY=-1; 
	int X=0,Y=0;
	int MinX=0,MinY=0;

//填充路径方向
	for(i=0;i<300;i++)
	{
	    if((Road[i][0]!=-1)&&(Road[i][1]!=-1))
	    {
	        //右
	        if((Road[i][0]-Road[i+1][0])==-1)
	        Road[i][2]=1;
	        //上
	     	if((Road[i][1]-Road[i+1][1])==-1)
	        Road[i][2]=2;
	        //左
	        if((Road[i][0]-Road[i+1][0])==1)
	        Road[i][2]=3;
	        //下
	        if((Road[i][1]-Road[i+1][1])==1)
	        Road[i][2]=4;    
	    }
	    else
	    {
	        Road[i][2]=-1;	        
	    }
	}
//找出路径上的所有拐点！！！
	j=0;
	for(i=2;i<300;i++)
	{
	    if((Road[i][0]!=-1)&&(Road[i][1]!=-1))
	    {
	        //前后同向，不是拐点
	        if(Road[i][2]==Road[i-1][2])
	        {
	        	;
	        }//前后不同向，是拐点
	        else
	        {
	            RoadChange[j][0]=Road[i][0];
	            RoadChange[j][1]=Road[i][1];
	            RoadChange[j][2]=Road[i][2];
	            j++;
	            RoadChangeNum++;
	        }
	    }
	        
	    else
	    {
	        Road[i][2]=-1;	        
	    }
	}
//加上目标点，目标也是拐点！
	RoadChange[RoadChangeNum][0]=GoalPosition[0];
	RoadChange[RoadChangeNum][1]=GoalPosition[1];
	RoadChange[RoadChangeNum][2]=0;
	RoadChangeNum++;
	NowGoalRoadChange=0;	            
}
/*****************************************************************************/
//函数名称： ComputeCMD()
//功能：计算小车控制指令
//参数：传入参数全部为实际像素值（240*320）(除了路径之外)
//返回值：  成功返回真，否则返回假
//本会眼熟中的所有值都是实际像素值（240*320）(除了路径之外)
//返回至所有距离以mm计算
/*****************************************************************************/
void ComputeCMD(volatile  int CMD_G_X,volatile  int CMD_G_Y,
				volatile  int CMD_Car_X,volatile  int CMD_Car_Y	,
				volatile  int CMD_Carhead_X,volatile  int CMD_Carhead_Y)
{
	float r,AngleA,AngleB,AngleX;
	float midX,midY;
	float CMDmmCar_X,CMDmmCar_Y;
	float CMDmmGoal_X,CMDmmGoal_Y;
	float CMDmmCarhead_X,CMDmmCarhead_Y;
	int i,j=0;
	int D_X,D_Y;
	int NowRoadX=-1,NowRoadY=-1; 
	int X=0,Y=0;
	int MinX=0,MinY=0;
	
//计算现在的小车位置单位为毫米
	CMDmmCar_X=CMD_Car_X*MMPerPixel;
	CMDmmCar_Y=CMD_Car_Y*MMPerPixel;
//计算现在的小车车头位置单位为毫米
	CMDmmCarhead_X=CMD_Carhead_X*MMPerPixel;
	CMDmmCarhead_Y=CMD_Carhead_Y*MMPerPixel;
//计算现在的目标位置单位为毫米
	CMDmmGoal_X=CMD_G_X*MMPerPixel;
	CMDmmGoal_Y=CMD_G_Y*MMPerPixel;
	
//计算距离
	midX=(CMDmmGoal_X-CMDmmCar_X)*(CMDmmGoal_X-CMDmmCar_X);
	midY=(CMDmmGoal_Y-CMDmmCar_Y)*(CMDmmGoal_Y-CMDmmCar_Y);
	NowDistance=sqrt(midX+midY);
	//计算控制角度
	AngleA=atan((CMDmmGoal_Y-CMDmmCar_Y)/(CMDmmGoal_X-CMDmmCar_X));
	AngleB=atan((CMDmmCarhead_Y-CMDmmCar_Y)/(CMDmmCarhead_X-CMDmmCar_X));
	NowDegree=AngleB-AngleA;
/*		if(-0.1<NowDegree<0.1)
	{
		NowDegree=0;
	}*/
	//控制指令XY
	NowCMD_X = fabs( NowDistance * sin(NowDegree) );
	NowCMD_Y = fabs( NowDistance * cos(NowDegree) );
}

/*****************************************************************************/
//函数名称： SendCarCMD()
//功能：发送小车运行指令
//参数：int Degree,角度	int Distance距离
//			无
//返回值：  成功返回真，否则返回假
//所有距离以mm计算
/*****************************************************************************/
int SendCarCMD(volatile float Degree,	volatile float Distance, volatile  int X,volatile  int Y)
{
    char Dir,SendX,SendY;
    //若控制指令太大泽发送小的    
    if (Degree>0)
    {	Dir=0xf1;}
    else
    { 	Dir=0xf0;}
    while(X>100)
    {
        X=X>>1;
        Y=Y>>1;        
    }
    while(Y>100)
    {
        X=X>>1;
        Y=Y>>1;        
    }
    SendX=X;
    SendY=Y;   
    
    		{
				*pUART_THR = 0xAA;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
				*pUART_THR = Dir;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
				*pUART_THR = SendY;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
				*pUART_THR = SendX;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
				*pUART_THR = 0xEE;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
			}
/*			
			mDelay(10000);
			   		{
				*pUART_THR = 0xaa;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
				*pUART_THR =0x01;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
				*pUART_THR = 0x06;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
				*pUART_THR = 0x06;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
				*pUART_THR = 0xee;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
			}
			*/
    return true;
}
/*****************************************************************************/
//函数名称： CarRun(void)
//功能：小车运行，沿找到的路径走到终点//参数：此函数中的参数全部都是实际值
//			无
//返回值：  成功返回真，否则返回假
//所有距离以mm计算
/*****************************************************************************/
int CarRun(void)
{


	
		return true;
}

//test uart!!
	/*
	switch (RunState)
	{
	    case 0;
		    {
				*pUART_THR = 0xaa;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
				*pUART_THR = 0x01;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
				*pUART_THR = 0x06;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
				*pUART_THR = 0x06;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
				*pUART_THR = 0xee;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
		    }
		    case 1;
		    {
				*pUART_THR = 0xaa;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
				*pUART_THR = 0x01;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
				*pUART_THR = 0x06;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
				*pUART_THR = 0x06;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
				*pUART_THR = 0xee;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
		    }
		    case 2;
		    {
				*pUART_THR = 0xaa;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
				*pUART_THR = 0x01;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
				*pUART_THR = 0x06;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
				*pUART_THR = 0x06;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
				*pUART_THR = 0xee;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
		    }
		    case 3;
		    {
				*pUART_THR = 0xaa;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
				*pUART_THR = 0x01;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
				*pUART_THR = 0x06;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
				*pUART_THR = 0x06;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
				*pUART_THR = 0xee;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
		    }
		    case 4;
		    {
				*pUART_THR = 0xaa;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
				*pUART_THR = 0x01;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
				*pUART_THR = 0x06;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
				*pUART_THR = 0x06;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
				*pUART_THR = 0xee;;
				asm("ssync;");
				while ((*pUART_LSR & 0x0020) == 0);
		    }
	}
		//pUART_THR = 0xaa;
		//Delay(DelayLength);
	}
	*/
上一页 1 23
💿 文件大小 8 K
👤 上传用户 c_word
📂 所属分类 DSP编程
🏷️ 相关标签

#ADIdsp #算法 #程序 #无线通讯
⌨️ 快捷键说明

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