logo头像

博学广问,自律静思

【4】通讯指令设计-Pcduino平台下的智能小车

一、指令数据包协议设计

在远程控制智能车系统中,远程客户端通过指令通信对智能车进行控制。在本文设计中仅涉及速度方向控制指令,为了考虑系统的可扩展性:因为后续需求可能出现更多类型的指令,本文为指令数据设计了指令代号,指令由指令代号和指令数据区组成;考虑到客户端和服务端更好地进行指令处理,指令字长为32位,与int数据类型字长相等,在服务端和和客户端进行socet套接字收发数据时可等同于int数据进行处理。在与本设计类似的课题研究中,往往使用char字符来控制方向,我觉得是不足取的,因为一个char类型如果进行分位存取数据可以节省网络流量和尽可能保证实时性的。

指令代号(4bit)

指令数据区(28bit)

当指令代号为1,表示发送车的速度方向控制指令,指令数据包为示意图:

0

3

4

11

12

13

20

21

31

指令代号 (4bit)

舵机角度 (8bit)

方向 (1bit)

速度 (8bit)

预留字段 (11bit)

1.**舵机角度:8bit,控制舵机的偏转角度(A),取值范围0-180 A=90度时,舵机处于中间位置; A<90时,舵机偏右; A>90时,舵机偏左。 2.方向:1bit,控制车的前进与后退(D),取值0**或1 D=0,表示小车后退; D=1,表示小车前进。 3.**速度:8bit,控制车的的行进速度(S) 取值范围为Arduino PWM引脚可写入的数据范围,为0-255。 4.**预留字段 用于后续可能出现的需求的扩展

二、指令数据包的封包与解包

根据上一小节对指令的协议设计,我们可以通过对int数据的移位运算和逻辑运算进行封包和解包。 指令的封包就是在32位数据结构的相应位置存放对应的指令数据。 下面以指令内容为指令代号(code)c=1,角度(angle)a=40,方向(direction)d=1,速度v=150的封包与解包过程为例(为了阅读更加直观,下面的计算值使用32位二进制表示):

  1. 将指定代号c=1左移28位:

c<<28 = 0001 0000 0000 0000 0000 0000 0000 0000

  1. 将a的二进制(101000)表示左移20位:

a<<20 = 0000 0010 1010 0000 0000 0000 0000 0000

  1. 将d的二进制(1)表示左移19位:

d<<19 = 0000 0000 0000 1000 0000 0000 0000 0000

  1. 将v的二进制(10010110)表示左移11位:

d<<19 = 0000 0000 0000 0100 1011 0000 0000 0000

  1. 将上述计算的临时值进行或运算即可得到最终的指令:

cmd = 0001 0010 1010 11001011 0000 0000 0000 综上所述,我们得到指令封包计算公式: cmd = (c<<28) | (a<<20) | (d<<19) | (v<<11) 等同于 ((c<<17)|(a<<9)|(d<<8)| v ) << 11 指令的解包就是从收到的指令数据(int)的对应位置取出数据。 下面以收到的指令为cmd = 313307136(二进制:0001 0010 1010 11001011 0000 0000 0000)为例进行数据的解包:

  1. 将cmd右移11位,剔除保留字段,这时速度数据区(8位)的最后一位移到末位,将结果赋值给cmd, 通过与运算“取出”移位后的末尾8位,即为速度:

cmd >> 11 = 0001 0010 1010 11001011 0 v = cmd | (1111 1111) = 1001 0110

  1. cmd右移8位,剔除速度字段,这时方向数据区(1位)的最后一位移到末位,将结果赋值给cmd, 通过与运算“取出”移位后的末尾最后1位,即为方向:

cmd >> 8 = 0001 0010 1010 1 d = cmd | (1) = 1

  1. cmd右移8位,剔除速度字段,这时方向数据区(1位)的最后一位移到末位,将结果赋值给cmd, 通过与运算“取出”移位后的末尾最后8位,即为角度:

cmd >> 1 = 0001 0010 1010 d = cmd | (1111 1111) = 0010 1010 综上所述,我们得到指令解包计算公式: cmd = cmd >> 11; v = cmd & 0xFF; d = (cmd >> 8) & 1; a = (cmd >> 9) & 0xFF;

三、指令数据包封包、解包的程序实现

定义汽车速度方向“向量”结构体:

struct CarVector{
int angle; //角度
bool direction;//方向
int speed;//速度
};

根据上述的封包计算公式,可以使用如下C++代码实现封包函数,传入汽车向量结构体返回int数据:

int createCmd(struct CarVector carVector) {
int code = 1;//命令代号
return ((code << 17) | (carVector.angle << 9) | (carVector.direction << 8) | carVector.speed) << 11;
}

实现解包函数把int数据传给carVector结构体指针

void parseCmd(int cmd, struct CarVector *carVector) {
cmd = cmd >> 11;
carVector->speed = cmd & 0xFF;
carVector->direction = (cmd >> 8) & 1;
carVector->angle = (cmd >> 9) & 0xFF;
}

四、通信指令封包解包测试

编写如下程序进行实验测试,本程序通过先构造一个速度为40,后退,舵机偏角为150的CarVector,将其封包后在进行解包,解析出的速度、方向、偏角值与CarVector相同,表明测试通过。

int main() {
struct CarVector sendCarVector={40,false,150},receiveCarVector;
int cmd =createCmd(sendCarVector);
printf(“发送命令%d(10)\n”,cmd);
parseCmd(cmd,&receiveCarVector);
printf(“角度:%d\n方向:%d\n速度:%d”,receiveCarVector.angle,receiveCarVector.direction,receiveCarVector.speed);
return 0;
}