AGV / agv_shengyiPPxiaoche · Files

#include "CommunicationForCenter.h"
char ParamBuff[40];
ResendStruct ReStructor;
TrafficLandMarks traffic_land_marks;
char Uart8SendBuff[UART8BUFFSTRING];
char GeCmddate[20];
//应答和接收标识符
char SendOrReplyTypeDataCode[][20] = {
	"s",
	"r",
 "Default"
};
// 工位IP号
char SlaveStationIdCode[][20] = {
	"B000",
	"B002",
	"B003",
	"C001",
	"Default"
};
char EquipmentTypeDataCode[][20] = {
	"A001",
	"A002",
	"A003",
	"A004",
	"A005",
	"A006",
	"Default"
};

char ContentTypeDataCode[ct_Size][22] = {
	"rptac",
	"getac",
	"getvol",
	"geterr",
	"getspd",
	"getmt",
	"rpterr",
	"rptscan",
	"rptmt",
	"rptrtp",
	"rptvol",
	"rptvmot",
	"rptin",
	"rptlac",
	"rptstart",
	"rpttravel",
	"rptmt_th",//告知agv现在小车要给11LINE上料
	"rptspd",
	"rptmode",
	"rptgo",
	"rptpos",
	"rptback",
	"rtplayer",
	"rptangle",
	"releagv",
	"callagv",
	"settask",//设置系统给agv任务
	"setrout",//下发路径指令
	"setstp",
	"setspd",
	"setrtp",//预停车
	"settra",//循迹方式
	"setmov",//启动
	"setvmot",//设置滚筒或者皮带动作
	"setlock",
	"setavoid",//红外区域设置
	"setmusic",//音乐开关
	"setlaser",//激光打开或关闭
	"setout",
	"setlrout",
	"setlayer",
	"setangle",//设置旋转角度
	"delrtp",//取消预停车
	"settcvoice",//设置交通避让语音提示
	"rptnode",
	"rptline",//上报路线
	"settips",//空闲放行
	"print",
//	"setreach",//磁条信号
	"Default"
};
/*
  接收成功后的数据原形
*/
char Original_Command_String[ZIGBEEBUFF_SIZE_NEW];
tsCommand CenterCommand;
ctDecodestate CenterDecode(char Byte, tsCmdList* iList)
{

		static bool sFirst = 0;
	static ctDecodestate Status;
	static u16  DataCount = 0;
	static u16  FinishDataCount = 0;
	static u16  StartDataCount = 0;
	unsigned int  Check_Sum = 0x00;//存储接收到的校验码
	unsigned int  Add_Sum = 0x00;//存储计算校验码
	if (!sFirst)
	{
		memset(CenterCommand.CmdList.ReceiveData, 0, sizeof(CenterCommand.CmdList.ReceiveData));
		Status = teERR;
		sFirst = 1;
		DataCount = 0;
	}
	if (Byte == '#')
	{
		Status = teDecoding;
		StartDataCount++;
		if (StartDataCount == 1)
		{
			DataCount = 0;
		}
	}
	else
	{
		StartDataCount = 0;
	}
	switch (Status)
	{
	case teERR:
		break;
	case teDecodLost:
	case teDecodSuccess:
		memset(CenterCommand.CmdList.ReceiveData, 0, sizeof(CenterCommand.CmdList.ReceiveData));
		Status = teDecoding;
		DataCount = 0;
	case teDecoding:
		if (Byte != ' ')
		{
			if (DataCount < ZIGBEEBUFF_SIZE_NEW)
				CenterCommand.CmdList.ReceiveData[DataCount++] = Byte;//从#号开始存储数据到Z号结束
		}
		if (Byte == 'Z')
		{
			FinishDataCount++;
			if (FinishDataCount == 2)
			{
				Status = teDecodCRC;
			}
			else
			{
				break;
			}
		}
		else
		{
			FinishDataCount = 0;
			if (DataCount >= ZIGBEEBUFF_SIZE_NEW)
			{
				Status = teDecodLost;
			}
			else
			{
				break;
			}
		}

	case teDecodCRC:

		sscanf(&CenterCommand.CmdList.ReceiveData[DataCount - 8], "%04x", &Check_Sum);//接收中控校验码
		Add_Sum = AddCheckSum(CenterCommand.CmdList.ReceiveData, DataCount - 8);//计算接收数据的校验码
		if (Add_Sum == Check_Sum || (Check_Sum == 0x1234))
		{
			memset(Original_Command_String, 0, sizeof(Original_Command_String));
			strcpy(Original_Command_String, CenterCommand.CmdList.ReceiveData);//将接收的数据存到Original_Command_String数组中

			memset(CenterCommand.CmdList.ReceiveData, 0, sizeof(CenterCommand.CmdList.ReceiveData));
			CenterCommand.CmdList.RecesiveDate = strlen(Original_Command_String);//存储本次接收字符数据量
			DataCount = 0;
			Status = teDecodSuccess;
			break;
		}
		else
		{
			memset(CenterCommand.CmdList.ReceiveData, 0, sizeof(CenterCommand.CmdList.ReceiveData));
			DataCount = 0;
			Status = teDecodLost;
		}
		if (Byte == '#')
		{
			memset(CenterCommand.CmdList.ReceiveData, 0, sizeof(CenterCommand.CmdList.ReceiveData));
			DataCount = 0;
		}
		break;
	
	default:
		while (1);
	}
	return Status;
}
//求数组iCmd数组所有参数和的最小两位数
//uint16_t AddCheckSum(char* cBuffer, short iBufLen)
//{
//	uint16_t i = 0, j = 0;
//	uint16_t wCrc = 0xffff;
//	uint16_t wPolynom = 0xA003;
//	for (i = 0; i < iBufLen; i++)
//	{
//		wCrc ^= cBuffer[i];
//		for (j = 0; j < 8; j++)
//		{
//			if (wCrc & 0x0001)
//			{
//				wCrc = (wCrc >> 1) ^ wPolynom;
//			}
//			else
//			{
//				wCrc = wCrc >> 1;
//			}
//		}
//	}
//	return wCrc;
//}

uint16_t AddCheckSum(char* cBuffer, short iBufLen)
{
	uint16_t i = 0;
	uint16_t wCrc = 0;
	for (i = 0; i < iBufLen; i++)
	{
		wCrc += cBuffer[i];	
	}
	return wCrc;
}
//求数组iCmd数组所有参数和的最小两位数
uint16_t AddCheckSumCRC(char* cBuffer, short iBufLen)
{
	uint16_t i = 0, j = 0;
	uint16_t wCrc = 0xffff;
	uint16_t wPolynom = 0xA003;
	for (i = 4; i < iBufLen; i++)
	{
		wCrc ^= cBuffer[i];
		for (j = 0; j < 8; j++)
		{
			if (wCrc & 0x0001)
			{
				wCrc = (wCrc >> 1) ^ wPolynom;
			}
			else
			{
				wCrc = wCrc >> 1;
			}
		}
	}
	return wCrc;
}
/*************************
分割字符串，得到所有关键字
**************************/
int GetKeyWords(tsCmdList * iList)
{
	char *pch;
	iList->Cmd_ListSize = 0;
	pch = strtok(Original_Command_String, ",,");//将第一个没有用的，号排除
	while (pch != NULL)
	{
		if (iList->Cmd_ListSize == 4)
		{
			memset(iList->Cmd_Lt, 0, sizeof(iList->Cmd_Lt));
			sprintf(iList->Cmd_Lt, "%s", pch);//单独存储指令接收到的参数	
			sprintf(iList->Cmd_List[iList->Cmd_ListSize], "%s", "0");//将之前的第3号存储参数的位置存储为0字符			
			memset(iList->Cmd_RoadParam, 0, sizeof(iList->Cmd_RoadParam));
			memcpy(iList->Cmd_RoadParam, iList->Cmd_Lt, ZIGBEEBUFF_SIZE_NEW);
		}
		else
		{
			sprintf(iList->Cmd_List[iList->Cmd_ListSize], "%s", pch);
		}
		iList->Cmd_ListSize++;
		pch = strtok(NULL, ",,");
	}
	return 0;//接收成功返回0
}
/*************************
分割字符串，得到所有关键字
**************************/
int GetKeyWordschar(tsCmdListParam * iList, char *cmd, const char *str)
{
	char *pch;
	char node_commonds[50] = { 0 };
	int length = strlen(cmd);
	memcpy(node_commonds, cmd, length);
	iList->Cmd_ListSize = 0;
	pch = strtok(node_commonds, str);//将第一个没有用的，号排除
	while (pch != NULL)
	{
		sprintf(iList->Cmd_List[iList->Cmd_ListSize], "%s", pch);
		iList->Cmd_ListSize++;
		pch = strtok(NULL, str);
	}
	return 0;//接收成功返回0
}

//动作方向处理
short ActionProcess(char paraOne,char paraTwo)
{
	if(paraOne == 'd')
	{
		agv.Public.i_AutoCentringDown = 1;
	}
	else
	{
		agv.Public.i_AutoCentringDown = 0;
	}
		//上对中
	if(paraOne == 'u')
	{
		agv.Public.i_AutoCentringUp = 1;
	}
	else
	{
		agv.Public.i_AutoCentringUp = 0;
	}
	if(paraOne == 'e' || paraOne == 'd' || paraOne == 'u')
	{
		return 0;
	}
	else if(paraOne == 'f' && paraTwo == 'a')
	{
		return 3;
	}
	else if(paraOne == 'f' && paraTwo == 'c')
	{
		return 4;
	}
	else if(paraOne == 'f' && paraTwo == 'm')
	{
		return 1;
	}
	else if(paraOne == 'b' && paraTwo == 'a')
	{
		return 5;
	}
	else if(paraOne == 'b' && paraTwo == 'c')
	{
		return 6;
	}
	else if(paraOne == 'b' && paraTwo == 'm')
	{
		return 2;
	}
	else if(paraOne == 'f' && paraTwo == 'l')
	{
		return 7;
	}
	else if(paraOne == 'b' && paraTwo == 'l')
	{
		return 9;
	}
	else if(paraOne == 'f' && paraTwo == 'r')
	{
		return 8;
	}
	else if(paraOne == 'b' && paraTwo == 'r')
	{
		return 10;
	}
	else if(paraOne == 'F' && paraTwo == 'm')
	{
		return 11;
	}
	else if(paraOne == 'B' && paraTwo == 'm')
	{
		return 12;
	}
	else 
		return 0;
}
/********************************
解码参数位
********************************/
int Getparameters(tsCmdList * iList)
{
	uint16_t i = 0, ii = 0, j;
	char *pch;
	iList->Cmd_ParametersSize = 0;

	for (i = 0; i < ParamSize; i++)
	{
		memset(iList->Cmd_parameters[i], 0, NameSize);//清除参数
	}

	pch = strtok(iList->Cmd_Lt, "::");//分解参数
	while (pch != NULL)
	{
		sprintf(iList->Cmd_parameters[ii], "%s", pch);
		pch = strtok(NULL, "::");
		ii++;//记录参数个数
	}
	if (strcasecmp(CenterCommand.CmdList.Cmd_List[3], "setlrout") == 0 )//&& !AGVStatus.TurnSelect )//当发送是路径指令时
	{
		for (i = 0; i < ii; i++)
		{
			memset(&CenterCommand.CmdListParam[i], 0, sizeof(CenterCommand.CmdListParam[i]));

		}
	}

	CenterCommand.Paramcount = ii;//存储参数个数
	
	if (strcasecmp(CenterCommand.CmdList.Cmd_List[3], "setlrout") == 0)//当发送是路径指令时
	{
		if(agv.Public.i_UpdatePathSig == 1)
		{				
			clearPathInfomation();
			traffic_land_marks.size = ii;
			for (i = 0; i < ii; i++)
			{					
				CenterCommand.RoadParamcount = ii;//存储参数个数

				GetKeyWordschar(&CenterCommand.CmdListParam[i], iList->Cmd_parameters[i], "_");
				for (j = 0; j < 2; j++)
				{
					CenterCommand.CmdListParam[i].Cmd_parameters[j] = strtoul(CenterCommand.CmdListParam[i].Cmd_List[j], NULL, 10);
					
					traffic_land_marks.land_marks[i].pose[j] = CenterCommand.CmdListParam[i].Cmd_parameters[j];
				}
				traffic_land_marks.land_marks[i].LandMarkID = atoi(CenterCommand.CmdListParam[i].Cmd_List[CenterCommand.CmdListParam[i].Cmd_ListSize-1]);
				traffic_land_marks.land_marks[i].LaserArea = atoi(CenterCommand.CmdListParam[i].Cmd_List[CenterCommand.CmdListParam[i].Cmd_ListSize-2]);
				traffic_land_marks.land_marks[i].DispatchSpeed = atoi(CenterCommand.CmdListParam[i].Cmd_List[CenterCommand.CmdListParam[i].Cmd_ListSize-3]);
				 memcpy(&CenterCommand.CmdListParam[i].Cmd_Dir[0], &CenterCommand.CmdListParam[i].Cmd_List[CenterCommand.CmdListParam[i].Cmd_ListSize - 5], 2); //size
				 memcpy(&CenterCommand.CmdListParam[i].Cmd_RotateDir[0], &CenterCommand.CmdListParam[i].Cmd_List[CenterCommand.CmdListParam[i].Cmd_ListSize - 4], 2); //size
				if(CenterCommand.CmdListParam[i].Cmd_RotateDir[0] == 'p')
					traffic_land_marks.land_marks[i].LightStopSig = 1;
				else if(CenterCommand.CmdListParam[i].Cmd_RotateDir[0] == 'q')
					traffic_land_marks.land_marks[i].LightStopSig = 2;
				else
					traffic_land_marks.land_marks[i].LightStopSig = 0;	
				traffic_land_marks.land_marks[i].Direction = ActionProcess(CenterCommand.CmdListParam[i].Cmd_Dir[0],CenterCommand.CmdListParam[i].Cmd_RotateDir[0]);
			}
			agv.Public.i_PlatformStopFlag = 0;
			agv.Public.i_QRCodeCenterStopFlag = 0;
			agv.Public.i_UpdatePathSig = 0;
			Reset_Alarm(0x0248);//清除脱线,停稳，系统停车
		}
	}
	else
	{
		for (i = 0; i < ii; i++)
		{
			if(strcasecmp(CenterCommand.CmdList.Cmd_List[3], "settips") == 0)
			{
				CenterCommand.Param[i] = strtoul(iList->Cmd_parameters[i], NULL, 16);
			}
			else
			{
				CenterCommand.Param[i] = strtoul(iList->Cmd_parameters[i], NULL, 10);//把接收的字符按10进制存储数据
			}
		}
	}
	return 0;//解码成功
}
/**关键字解码判断**/
char CommandAnalysis(void)
{
	//任何一处解码失败，程序就不会继续往下执行
	uint8_t i;
	int Status = 0;
	CenterCommand.SendOrReply = sort_Default;
	CenterCommand.Content = ct_Default;
	Status = GetKeyWords(&CenterCommand.CmdList);//初步用，号判断有几段用，号隔开的数据且返回判断结果
	for (i = 0; i < sort_Size; i++)
	{
		Status = strcasecmp(CenterCommand.CmdList.Cmd_List[2], SendOrReplyTypeDataCode[i]);//判断方向
		if (Status == 0)
		{
			CenterCommand.SendOrReply = (teSendOrReplyType)i;//存储方向
			break;
		}
		if (i == sort_Default)
		{
			return 2;//失败返回2
		}
	}
	for (i = 0; i < et_Size; i++)
	{
		Status = strcasecmp(CenterCommand.CmdList.Cmd_List[1], EquipmentTypeDataCode[i]);//判断动作(content)模式
		if (Status == 0)
		{
			CenterCommand.EquipmentType = (teEquipmentType)i;//存储动作模式
			break;
		}
		if (i == et_Default)
		{
			return 3;//失败返回3
		}
	}
	for (i = 0; i < ct_Size; i++)
	{
		Status = strcasecmp(CenterCommand.CmdList.Cmd_List[3], ContentTypeDataCode[i]);//判断动作(content)模式
		if (Status == 0)
		{
			CenterCommand.Content = (teContentType)i;//存储动作模式
			break;
		}
		if (i == ct_Default)
		{
			return 7;//失败返回7
		}
	}
	return 0;
}
/**设置或者回复指令**/
void SendOrReplyTypeHandle(void)
{
	switch (CenterCommand.SendOrReply)
	{
	case sort_Receive:
		if (Getparameters(&CenterCommand.CmdList) != 0)return;
		memset(GeCmddate, 0, sizeof(GeCmddate));
		sprintf(GeCmddate, "%s", CenterCommand.CmdList.Cmd_List[3]);//获得中控回复指令
		break;
	
	case sort_Send:
		if (Getparameters(&CenterCommand.CmdList) != 0)
			return;
		if (agv.Command.Au_Hand)//自动模式
			Bady_DictateTypeHandle();//获得中控发送指令
		break;
		
	default:break;
	}
}

void Bady_DictateTypeHandle(void)
{
	u8 num =0;
	static int dst = 0;

	switch (CenterCommand.Content)
	{
	case ct_getac://获得RFID位置
		break;
	
	case ct_getvol://获得电压值
		Create_BodyTreaty(false, et_smtagv, sort_Receive, ct_getvol, CenterCommand.CmdList.Cmd_RoadParam);
//		Create_BodyTreaty(false, et_smtagv, sort_Send, ct_rptvol, ParameterTransform("%2.2f", AGVStatus.battery_voltage*0.0001f));
		break;
	
	case ct_geterr://获得报警状态
		Create_BodyTreaty(false, et_smtagv, sort_Receive, ct_geterr, CenterCommand.CmdList.Cmd_RoadParam);
		Create_BodyTreaty(true, et_smtagv, sort_Send, ct_rpterr, ParameterTransform("%04x", agv.Public.Error_Flag));
		break;
	
	case ct_getspd://获得小车速度
		Create_BodyTreaty(false, et_smtagv, sort_Receive, ct_getspd, CenterCommand.CmdList.Cmd_RoadParam);

//		Create_BodyTreaty(false, et_smtagv, sort_Send, ct_rptspd, ParameterTransform("%2.4f", AGVStatus.Speed));
		break;
	
	case ct_rptvmot://获取料架上报并回复
//		Create_BodyTreaty(false, et_smtagv, sort_Receive, ct_rptvmot, CenterCommand.CmdList.Cmd_RoadParam);

//		Create_BodyTreaty(false, et_smtagv, sort_Send, ct_rptspd, ParameterTransform("%2.4f", AGVStatus.Speed));
		break;
	
	case ct_rptmt:
		switch (CenterCommand.Param[0])//检测1号工位
		{
			case 0:
				break;
			case 1:
				break;
		}	
		Create_BodyTreaty(false, et_B124, sort_Receive, ct_rptmt, CenterCommand.CmdList.Cmd_RoadParam);
		break;
		
	case ct_setlrout://设置行走轴
		Create_BodyTreaty(false, et_smtagv, sort_Receive, ct_setlrout, CenterCommand.CmdList.Cmd_RoadParam);		
	break;
	
	case ct_setvmot: //设置小车滚筒转动状态
		Create_BodyTreaty(false, et_smtagv, sort_Receive, ct_setvmot, CenterCommand.CmdList.Cmd_RoadParam);
		commandActionAnalysis(CenterCommand.Param[0],CenterCommand.Param[1]);//悬臂轴自动对接
	break;
	
	case ct_setmov: 
		Create_BodyTreaty(false, et_smtagv, sort_Receive, ct_setmov, CenterCommand.CmdList.Cmd_RoadParam);
		commandActionAnalysisRunHand(CenterCommand.Param[0],CenterCommand.Param[1]);
	}
}

char UdpSendBuff[2000] = {0};
//生成通讯协议
void Create_BodyTreaty(bool AskAgainEnable, teEquipmentType EquipmentType, teSendOrReplyType SendOrReplyType, teContentType ContentType, char *Parameter)
{
	char CheckSum[4];
//	//先清零重发数据的字符数组
	if(agv.Command.UdpSendFlag == 0)//当为0代表上次已发送才能更换发送内容
	{
		memset(UdpSendBuff, 0, sizeof(UdpSendBuff));
		
		if (agv.Parameter.AGVID != 0)//AGG ID号不等于0时设置AGVID
		{
			sprintf(EquipmentTypeDataCode[0], "%04X", agv.Parameter.AGVID);
		}
		sprintf(UdpSendBuff, "##,,%s,,%s,,%s,,%s,,", EquipmentTypeDataCode[EquipmentType], SendOrReplyTypeDataCode[SendOrReplyType],
			ContentTypeDataCode[ContentType], Parameter);
		
		sprintf(CheckSum, "%04x,,ZZ", AddCheckSum(UdpSendBuff, strlen(UdpSendBuff)));
		
		strcat(UdpSendBuff, CheckSum);
		
		agv.Command.UdpSendFlag = 1;//更换完指令内容，允许发送
	}
	
//	UartSend(COM6,(unsigned char*)UdpSendBuff,strlen(UdpSendBuff));

//	strcpy(UdpSendBuff,UdpSendBuff);
//	
//	memcpy(ReStructor.Matmesseg[ContentType], Uart8SendBuff, strlen(Uart8SendBuff));
//	//是否重发？ 由Resend决定
//	ReStructor.MatmesEnable[ContentType] = AskAgainEnable;
//	//清零重发计时
//	ReStructor.Matmestime[ContentType] = 0;
}

//将HEX 安装一定的格式转换成字符串
char* ParameterTransform(char *fmt, ...)
{
	int cnt = 0;
	va_list argptr;
	cnt = cnt;
	va_start(argptr, fmt); //开始使用可变参数
	cnt = vsprintf(ParamBuff, fmt, argptr);//格式化输出
	va_end(argptr);//结束使用可变参数
	return(ParamBuff);
}

/*
 * 函数名：Recive_check
 * 描述  ：取消重复发送函数
 * 输入  ：无
 * 输出  ：无
 */
void Recive_check(void)
{
	uint8_t ii;
	for (ii = 0; ii < ct_Size; ii++)
	{
		if (strcasecmp(GeCmddate, ReStructor.Matmes[ii]) == 0)
			ReStructor.MatmesEnable[ii] = 0;
	}
}
/*
 * 函数名：SysACKcheck
 * 描述  ：重复发送函数
 * 输入  ：无
 * 输出  ：无
 */
void SysACKcheck(void)
{
	uint8_t ii;
	static bool iFirst = true;
	static u32 LastTime = 0;
	if (iFirst)
	{
		iFirst = false;
		for (ii = 0; ii < ct_Size; ii++)
		{
			memset(ReStructor.Matmes[ii], 0, STRING_SIZE);
			ReStructor.MatmesEnable[ii] = 0;
			ReStructor.Matmestime[ii] = 0;
		}
	}
	else
	{
		for (ii = 0; ii < ct_Size; ii++)
		{
			//如果相应标志位为1/true ,切时间>1.5S，重发
			if (ReStructor.MatmesEnable[ii] && (agv.Public.SystemTime - LastTime >= 1500))//重发间隔时间
			{
				LastTime = agv.Public.SystemTime;
				
				memset(Uart8SendBuff, 0, sizeof(Uart8SendBuff));
				//将要重发的数据复制回来
				memcpy(Uart8SendBuff, ReStructor.Matmesseg[ii], WifiBUFF_SIZE);
				
				WriteUart(COM6,(unsigned char*)Uart8SendBuff,sizeof(Uart8SendBuff));
				//重发时间清零
				ReStructor.Matmestime[ii] = 0;
				//ReStructor.Matmes_Num[ii]++;
				//if (ReStructor.Matmes_Num[ii] >= 5)//决定发送个数
				//{
				//	ReStructor.Matmes_Num[ii] = 0;
				//	ReStructor.MatmesEnable[ii] = 0;
				//}
			}
		}
	}
}

void UartReceiveDataFromSystem()
{
	unsigned char buff[2000];
	unsigned int len;
	unsigned int i;
	len = ReadUart(COM6,buff,2000);	
	if( len > 0)   
	{
		for(i = 0; i < len; i++)
		{
			/**解碼接收的數據**/
			if (CenterCommand.NewReceive == false)
			{
				if (CenterDecode(buff[i], &CenterCommand.CmdList) == teDecodSuccess)
				{
					CenterCommand.NewReceive = true;
				}
			}
			/**處理接收的數據**/
			if (CenterCommand.NewReceive)
			{
				if (CommandAnalysis() == 0)  //解码命令字
				{
					SendOrReplyTypeHandle();		// 命令字码处理
				}
				CenterCommand.NewReceive = false;
				//如果接收回来的数据是中控应答的，不再做处理
				if (CenterCommand.SendOrReply == sort_Receive)//判断是否是接受中控协议
				{
					Recive_check();	//取消重新发送
				}
			}
		}
	}
	SysACKcheck();
}