低成本3D空间导航/测绘机器人(4)——数据包通讯,由模块走向系统

151
0
2021年2月14日 09时20分

写在前面

 

上一小节我们介绍了机器人的舵机驱动与串口通讯的原理。到目前为止,我们已经完成了:

 

  • 机器人编码器和轮胎的驱动
  • 机器人头部舵机的驱动
  • 机器人与上位机通讯串口的基本配置

 

到这里,机器人的在硬件配置和底层驱动上已经可以实现基本功能了,我们今天来完成机器人与上位机基于“数据包”的通讯,这样我们就可以使用ROS/HTTP等方式来对机器人进行控制了~

 

数据包——万物通讯皆可数据包

 

在一起学习数据包之前,我们首先要了解一个概念:

 

  • 数据包与上一节所说的硬件协议(例如IIC,SPI,USART等不是一个层级的概念)
  • 数据包是硬件协议之上的层级。

 

我们用下面的图来解释一下数据包与硬件协议之间的关系:

 

图片1

 

实际上我们不论是使用串口,还是网络TCP,都会将BYTE封装为这种类型的数据包。封装为数据包可以使大量,多种类型数据的传输既整洁又高效。

 

机器人数据包的构建

 

在使用数据包之前,我们首先要按照实际情况对机器人的数据包其进行定义。

 

先来看我们的机器人有哪些特点:

 

  • 要传输左右轮胎的速度,两个舵机角度,以及电池电量,LED控制等功能。
  • 每个数据的长度都不大,一般不会超过100个字节
  • 数据密度要求相对比较高,比如机器人再导航时,数据包的传输频率要在20Hz以上

 

4)数据可能出现错码的现象,需要一个校验机制

 

基于上面这些特点,我们提出一个自己的数据包(下图):

 

我们的数据包包含一个4字节(固定)的包头段,以及一个长度有变化的数据。为了避免传输过程中出现的错误,我们在数据末端加入一个数据末校验位,来检查在一个数据包的传输中是否出现了什么错误。

 

图片2

 

在数据传输中为了实现信息的有序化,我们使用指令—>应答的半双工通讯方式。首先上位机发送一个请求,然后单片机向上位机进行反馈,流程如下:

 

图片3

 

针对数据包的包类型(CmdID),我们定义如下宏定义:

 

#define SET_VELOCITY 0x01
#define SET_HEAD_ROTATE0x02
#define SET_ARM 0x03
#define SET_UTILS 0x04
#define SET_LED 0x05
#define ASK_UTIL_STATE 0x11
#define ASK_SONAR_VALUE 0x12
#define ASK_IMU 0x13
#define ASK_BATT 0x14

USART数据包的解析

 

现在我们构建了数据包,但是正如上一节所说,机器人的单片机每一个中断只能够接收到一个字节。很显然,一个字节是肯定构建不出数据包的。

 

所以我们使用了一个缓存列表,每接收一个BYTE数据,我们就将它传入缓存列表中。当我们接收了4次数据,我们就分析数据头,得到包长度N;当我们又接收了N-4个数据时,我们对数据包进行统一的解析,生成应答数据,然后清空缓存列表,等待下一个数据包的到来。

 

换个显而易见的例子,假设我是一个流水线上给鸡蛋包装的工人。一盒里需要装12个鸡蛋。但是流水线上每次之给我送一个。所以每次送来一个鸡蛋,我就把它放到盒子里。当盒子里装满了鸡蛋时,我才开始统一打包,然后拿出一个新盒子准备打包下一盒。

 

所以,我们首先来构建一个“装鸡蛋的盒子”——缓存变量

 

Tx与Rx数据包缓存队列的构建

 

我们先来构建三类变量,第一个变量是叫Data,它是个数组,对应装鸡蛋的盒子,第二个变量是pack_Len,对应我们已经收到鸡蛋的个数。第三个变量是pack_Cmd,对应鸡蛋类型,比如土鸡蛋我们用袋子装,城里鸡蛋我们就用礼盒装……

 

u8 rx_Data[256],tx_Data[256]; //All data in tx/rx packs
u8 rx_pack_Len,tx_pack_Len; //Pack length(HEAD included)
u8 rx_pack_Cmd,tx_pack_Cmd; //Pack Commands

 

由于串口中断每次只能接收到一个数据,所以我们先将鸡蛋存储到队列里,同时检查一下我们到底接受到了几个鸡蛋:

 

图片4

 

下面是将单个鸡蛋转化为一包鸡蛋的程序,当然,实际程序是会稍微复杂一些的。在这个程序的末尾,我们有一行叫做USART_Process()的代码,它可以对一个数据包内的数据做相关的处理。

 

void UART4_IRQHandler()
{
	uint8_t temp = 0;
	if(USART_GetITStatus(UART4, USART_IT_RXNE)!=RESET)
	{
		//if RX interrupt(received data)
		/*******************************************
				Processing the pack head
		*******************************************/
		if(rx_pack_State==PROCESSING_HEAD)
		{
			//If the STATE is processing-head
			rx_Data[rx_pack_Addr++]=USART_ReceiveData(UART4);
			if(rx_Data[0]!=0xff) Process_IO_Error();
			//If received a FULL head
			if(rx_pack_Addr==4)
				{
				//If gotten a wrong package
				if(rx_Data[PACK_HEAD_POSITION]!=0xff || rx_Data[PACK_HEAD_POSITION+1]!=0xff){Process_IO_Error();return;}
				rx_pack_Len=rx_Data[PACK_LEN_POSITION];rx_pack_Cmd=rx_Data[PACK_CMD_POSITION];
				rx_pack_State=PROCESSING_BODY;
			}
		}
		/*******************************************
				Processing the pack body
		*******************************************/
		else if(rx_pack_State==PROCESSING_BODY)
		{
			rx_Data[rx_pack_Addr++]=USART_ReceiveData(UART4);
			//If received a FULL body
			if(rx_pack_Addr==rx_pack_Len)
			{
				Check_CRC();
				rx_pack_Addr=0;
				rx_pack_State=PROCESSING_HEAD;
				Usart_Process();
			}
		}
	}
}

 

数据包的拆包与解析

 

我们现在完成了数据包的基础打包,但是如何从数据包中解析出具体的数据呢?

 

(为了便于理解,我们暂时忽略数据包中校验相关的内容)

 

举一个例子,在我们的机器人中,控制机器人双轮速度以及灯光控制的包如下:

 

可以看到,两个包的数据(灰色)都是具有一定规律的,而这种规律又由CmdID(蓝色)所决定。

 

图片5

 

所以,我们可以在接收一个完整的数据包后,基于每一个数据包的CmdID来解析相关的数据。

 

下面的程序就是利用CmdID来进行不同类型数据解析的代码:

 

void Usart_Process()
{
		delay_us(50);
		switch(rx_pack_Cmd)
		{
			case SET_VELOCITY:
				/*速度设置编码格式:  [0]     [1]     [2]     [3]     [4]     [5]
													 l_Dir	  l_HBits	l_LBits r_Dir		r_HBits	r_LBits
					Value=(x_HBits<<8|L_Bits)*0.001 when x_Dir==0
					Value=(x_HBits<<8|L_Bits)*-0.001 when x_Dir!=0
					---
					Ret: [0]		[1]		 [2]		[3]			[4]			[5]			[6]			[7]
					[Left Encoder Lowbyte->Highbyte] [Right Encoder Lowbyte->Highbyte]
				*/
				//解析两个轮子的速度6字节
				tar_spd_L=((float)rx_Data[PACK_DATA_POSITION+1]*256+rx_Data[PACK_DATA_POSITION+2])*(rx_Data[PACK_DATA_POSITION+0]==0?0.001:-0.001);
				tar_spd_R=((float)rx_Data[PACK_DATA_POSITION+4]*256+rx_Data[PACK_DATA_POSITION+5])*(rx_Data[PACK_DATA_POSITION+3]==0?0.001:-0.001);
				//Set_Dir(tar_spd_L,tar_spd_R);
				/*构建反馈类型:[1]     [2]     [3]     [4]     			[5]     [6]			[7]			[8]
											 SIGNED INT LEFT ENCODER VAL					SIGNED INT RIGHT ENCODER VAL
				*/
				Init_Tx_Data(PACK_DATA_POSITION+sizeof(encoder_L)+sizeof(encoder_R),SET_VELOCITY);
				memcpy(tx_Data+PACK_DATA_POSITION,&encoder_L,sizeof(encoder_L));
				memcpy(tx_Data+PACK_DATA_POSITION+sizeof(encoder_L),&encoder_R,sizeof(encoder_R));
				Usart_Feedback();
				break;
			case SET_LED:
				/*设置灯光1字节: (0: OFF 1: ON)
					Ret: 									[1]
																(0: OFF 1: ON)
				*/
				if(rx_Data[PACK_DATA_POSITION]==0) MAIN_OUTPUT_OFF;
				else MAIN_OUTPUT_ON;
				Init_Tx_Data(PACK_DATA_POSITION+sizeof(u8),SET_LED);
				memcpy(tx_Data+PACK_DATA_POSITION,&rx_Data[PACK_DATA_POSITION],sizeof(u8));
				Usart_Feedback();
				break;
			case SET_HEAD_ROTATE:
				/*头部舵机控制:   [1]			[2]				[3]			[4]	
												PITCH_H		PITCH_L		YAW_H		YAW_L
				Ret:						 [1]			[2]				[3]			[4]	
												PITCH_H		PITCH_L		YAW_H		YAW_L
				*/
				Set_Servo_PWM(8,(uint16_t)(rx_Data[PACK_DATA_POSITION]*256+rx_Data[PACK_DATA_POSITION+1]));
				Set_Servo_PWM(7,(uint16_t)(rx_Data[PACK_DATA_POSITION+2]*256+rx_Data[PACK_DATA_POSITION+3]));
				Init_Tx_Data(PACK_DATA_POSITION+sizeof(u8)*4,SET_HEAD_ROTATE);
				memcpy(tx_Data+PACK_DATA_POSITION,rx_Data+PACK_DATA_POSITION,sizeof(u8)*4);
				Usart_Feedback();
				break;
			case ASK_BATT:
				/*
					查询电池电量: None
					---
					Ret: [1]   [2]    [3]
			         Volt  Prcet	Health	
				*/
				Init_Tx_Data(PACK_DATA_POSITION+sizeof(u8)*3,ASK_BATT);
				tx_Data[PACK_DATA_POSITION+0]=(u8)(VV*10);
				tx_Data[PACK_DATA_POSITION+1]=(u8)(bat_Percentage*10);
				tx_Data[PACK_DATA_POSITION+2]=(u8)(0);
				Usart_Feedback();
				break;

			default:
				break;
		}
}

数据包的使用

 

我们在Usart_Process()函数中解析了上位机发送的指令,我们可以在解析的函数中直接调用驱动外设的函数(比如驱动舵机旋转),这种方法具有较高的实时性,但是会占用通讯资源。另一种方式就是将指令存入全局变量,然后让其他函数读取全局变量(比如轮胎速度的控制)。

 

我们机器人的通讯功能还比较简单,核心功能仅有舵机以及轮胎控制;在更为复杂的机器人系统中,我们还可以增加协议的种类,以实现更为复杂的机器人上位机–下位机信息交互~

 

硬件相关设计小结

 

到这里我们已经简单梳理了小机器人硬件上的基本功能原理,包括:

 

  1.     轮胎及编码器的电路设计及控制
  2.    2路舵机云台的电路设计以及软件控制
  3.    USART通讯的基础内容
  4.    基于USART与数据包的上位机–下位机通讯

 

实际上,我们还有一些其他功能,比如电量检测,灯光控制等等,这些周边功能由于不是核心,我们会在完成基本功能后,再单独用一篇博客来讲。

 

本章总结

 

本章我们一起探讨了数据包的相关内容。至此,我们已经基本介绍完成了硬件相关的实现原理和设计方案,下一节,我们将会开启上位机的编程之路~

 

    大家可以访问:http://wiki.ros.org,先按照官网教程配置好基本的上位机编程环境(ROS),我们将从简单的数学知识入手,逐步介绍如何使用一个二维激光雷达配合云台,实现完整3D场景的炫酷重建。让我们拭目以待!

 

联系作者

 

微信截图_20210202215846

发表评论

后才能评论