169 lines
10 KiB
Markdown
169 lines
10 KiB
Markdown
# AutolaborM2串口通讯协议
|
||
|
||
AutolaborM2采用串口通讯,通讯波特率为***115200***。通讯消息分为无数据域和有数据域两种类型。其中无数据域类型主要用于查询指令。
|
||
|
||
串口连接成功后,能收到机器人上报的里程计消息和车辆速度、转角消息,可依此作为机器人连接的判断。当前车辆上报得到左轮轮速和右轮轮速是直接从编码器给出的***角速度***,单位为rad。如果需要知道车轮线速度,则需要查询车辆轮半径,根据轮半径计算出车轮线速度。比如,角速度10rad/s,车轮半径0.15m,则线速度v = 10 * 0.15 = 1.5m/s
|
||
|
||
机器人有三种急停方式:硬件开关急停、手柄远程急停、软件消息急停。三种急停均可使得车辆立即停止。其中***硬件急停优先级最高***,直接切断电机供电。在发送急停消息后,可根据查询机器人状态信息来判断是否成功进入急停。如果发现机器人进入急停状态,可根据查询消息,判定急停触发的来源。
|
||
|
||
车辆的运动控制采用两轮自行车模型,控制量为***前轮转角和后轮转速***。其中转速为***相对速度***,需要用户根据查询到的车辆最大速度,计算出相对速度大小。比如,查询到车辆最大速度为2m/s,想要让车辆按照1m/s的速度运行,则需要发送 v = 1/2 = 0.5。
|
||
|
||
## 1、两种数据消息
|
||
|
||
* 无数据域消息
|
||
* 长度:6字节(1字节帧头0xFE + 4字节消息类型 + CRC8校验)
|
||
|
||
* 有数据域消息
|
||
* 长度:14字节(1字节帧头0xFE + 4字节消息类型 + 8字节数据 + CRC8校验)
|
||
|
||
## 2、查询指令
|
||
|
||
* 查询指令:
|
||
* 4字节消息类型:0x0D,0x00,消息ID,0x00
|
||
* 反馈指令:
|
||
* 4字节消息类型:0x2D,0x00,消息ID,0x00
|
||
* 8个字节的数据字节解析参考下面的表格,不同消息ID解析方式不同。
|
||
* 针对不同指令,只有第三位消息ID不同。
|
||
|
||
|
||
| 消息ID | 内容 | 格式| 读/写|备注|
|
||
| --- | --- | --- | --- | --- |
|
||
| 0x80 | 状态 |无数据域<br>[0]运行状态[1]-[7]保留|写<br>读|0x10:正常运行 0xff:急停状态|
|
||
| 0x02 | 重置里程计 |无数据域|写|里程计的方向清零,即车辆朝向清零|
|
||
| 0x11 | 剩余容量百分比 |无数据域<br>[0] uint8 [1]-[7]保留|写<br>读|<br>[0,100]% (采样频率=1Hz)|
|
||
| 0x12 | 剩余可用时间 |无数据域<br>[0][1][2][3] uint32 [4]-[7]保留|写<br>读|<br>[0,360000]s (采样频率=1Hz)|
|
||
| 0x13 | 剩余容量 |无数据域<br>[0][1][2][3] uint32 [4]-[7]保留|写<br>读|<br>[0,5000000]mAh (采样频率=1Hz)|
|
||
| 0x14 | 电池电压 |无数据域<br>[0][1] uint16 [2]-[7]保留|写<br>读|<br>[0,500000]mV (采样频率=1Hz)|
|
||
| 0x15 | 电池电流 |无数据域<br>[0][1][2][3] uint32 [4]-[7]保留|写<br>读|<br>[-750000,750000]mA 正:电池充电,负:电池放电(采样频率=1Hz)|
|
||
| 0x16 | 手柄数据 | 无数据域<br>[0]-[7]详见 [手柄消息结构定义](#gamepad-struct) |写<br>读| |
|
||
| 0x17 | 急停开关 | 无数据域<br>[0]bool[1]-[7]保留|写<br>读| <br> 0:正常 1:急停 |
|
||
| 0x18 | 查询软急停 | 无数据域<br>[0]bool[1]-[7]保留|写<br>读| <br> 0:正常 1:急停 |
|
||
| 0x19 | 查询手柄急停 | 无数据域<br>[0]bool[1]-[7]保留|写<br>读| <br> 0:正常 1:急停 |
|
||
| 0x1a | 查询最大线速度 | 无数据域<br>[0]-[3]float [4]-[7]保留 |写<br>读| 根据车轮大小计算出的最大线速度(m/s) |
|
||
| 0x1b | 查询最大转角 | 无数据域<br>[0]-[3]float [4]-[7]保留 |写<br>读| 根据机械结构得到的最大转角(rad) |
|
||
| 0x1c | 查询车辆宽度 | 无数据域<br>[0]-[3]float [4]-[7]保留 |写<br>读| 根据机械结构得到的车宽(m) |
|
||
| 0x1d | 查询车辆长度 | 无数据域<br>[0]-[3]float [4]-[7]保留 |写<br>读| 根据机械结构得到的车长(m) |
|
||
| 0x1e | 查询轮半径 | 无数据域<br>[0]-[3]float [4]-[7]保留 |写<br>读| 根据机械结构得到的车轮半径(m) |
|
||
|
||
***范例***
|
||
|
||
| 指令 | 单位| 写字节流 | 反馈字节流 |含义 |
|
||
| ------------- | ------------- | ------------- | ------------- | ------------- |
|
||
|状态查询||0xFE,0x0D,0x00,0x80,0x00,0xB2|0xFE,0x2D,0x00,0x80,0x00,0x10,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x09| 状态正常 |
|
||
|重置编码器||0xFE,0x0D,0x00,0x02,0x00,0x0C||
|
||
|剩余容量百分比||0xFE,0x0D,0x00,0x11,0x00,0xB5|0xFE,0x2D,0x00,0x11,0x00,0x64,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x79|容量100%|
|
||
|剩余可用时间|秒|0xFE,0x0D,0x00,0x12,0x00,0xE0|0xFE,0x2D,0x00,0x12,0x00,0x50,0xC3,0x00,0x00,0x00,0x00,0x00,0x00,0xCC|剩余50000s|
|
||
|剩余容量|mAh|0xFE,0x0D,0x00,0x13,0x00,0x24|0xFE,0x2D,0x00,0x13,0x00,0x50,0xC3,0x00,0x00,0x00,0x00,0x00,0x00,0x02|剩余50000mAh|
|
||
|电池电压|10mV|0xFE,0x0D,0x00,0x14,0x00,0x4A|0xFE,0x2D,0x00,0x14,0x00,0x7D,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x99|1.25V|
|
||
|电池电流|1mA|0xFE,0x0D,0x00,0x15,0x00,0x8E|0xFE,0x2D,0x00,0x15,0x00,0x4D,0x08,0x00,0x00,0x00,0x00,0x00,0x00,0x5A|2.125A|
|
||
|急停开关||0xFE,0x0D,0x00,0x17,0x00,0x1F|0xFE,0x2D,0x00,0x17,0x00,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x58|急停开关按下|
|
||
|查询软急停||0xFE,0x0D,0x00,0x18,0x00,0x07|0xFE,0x2D,0x00,0x18,0x00,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x26|软急停触发|
|
||
|查询手柄急停||0xFE,0x0D,0x00,0x19,0x00,0xC3|0xFE,0x2D,0x00,0x19,0x00,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xE8|手柄急停按下|
|
||
|查询最大线速度|m/s|0xFE,0x0D,0x00,0x1A,0x00,0x96|0xFE,0x2D,0x00,0x1A,0x00,0x00,0x00,0xC0,0x3F,0x00,0x00,0x00,0x00,0x94|最大线速度1.5m/s|
|
||
|查询最大转角|rad|0xFE,0x0D,0x00,0x1B,0x00,0x52|0xFE,0x2D,0x00,0x1B,0x00,0x92,0x0A,0x06,0x3F,0x00,0x00,0x00,0x00,0xBC|最大转角:0.523Rad(30度)|
|
||
|查询车辆宽度|m|0xFE,0x0D,0x00,0x1C,0x00,0x3C|0xFE,0x2D,0x00,0x1C,0x00,0x00,0x00,0x00,0x3F,0x00,0x00,0x00,0x00,0x9D| 0.5m|
|
||
|查询车辆长度|m|0xFE,0x0D,0x00,0x1D,0x00,0xF8|0xFE,0x2D,0x00,0x1D,0x00,0x66,0x66,0x26,0x3F,0x00,0x00,0x00,0x00,0x83| 0.65m|
|
||
|查询轮半径|m|0xFE,0x0D,0x00,0x1E,0x00,0xAD|0xFE,0x2D,0x00,0x1E,0x00,0x9A,0x99,0x19,0x3E,0x00,0x00,0x00,0x00,0xD2| 0.15m|
|
||
|
||
## 3、控制指令(有数据域消息类型)
|
||
|
||
* 运动控制(v,theta)
|
||
* 运动控制具有超时机制,需要在200ms内发送一帧数据,否则会认为数据超时,车辆停止运动。
|
||
* v: 车辆最大速度的百分比,取值范围[-1,-1],对应从倒退最大速度到前进最大速度。
|
||
* theta: 前轮转角,单位rad,弧度制。车辆向左转为正。
|
||
* 4字节消息:0x2D,0x00,0x01,0x00
|
||
* 8字节数据:前4字节表示v,后4字节表示theta
|
||
* 数据类型:float
|
||
* 范例:0xFE,0x2D,0x00,0x01,0x00,0xCD,0xCC,0xCC,0x3D,0xCD,0xCC,0x4C,0x3E,0x82
|
||
* 指令含义:v:0.1相对速度,theta:0.2rad
|
||
|
||
* 紧急停止指令(Emergency)
|
||
* 急停触发:0xFE,0x2F,0xFF,0xFF,0x00,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xDA
|
||
* 急停解除:0xFE,0x2F,0xFF,0xFF,0x00,0x10,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x53
|
||
* 该消息可以直接关停底盘的电机输出。
|
||
|
||
|
||
## 4、反馈消息
|
||
|
||
反馈消息将持续发送,40ms间隔。可以使用反馈消息作为车辆连接的依据。
|
||
|
||
* 里程计xy坐标
|
||
* 车辆上电后的位置,为里程计原点,车头方向为x,车左侧为y。
|
||
* 4字节消息类型:0x2D,0x00,0x21,0x00
|
||
* 8字节数据内容:前4个字节为x坐标,单位m。后字节为y坐标,单位m。
|
||
* 数据类型为float
|
||
* 范例:0xFE,0x2D,0x00,0x21,0x00,0xCD,0xCC,0xCC,0x3D,0xCD,0xCC,0x4C,0x3E,0x1A
|
||
* 说明:x:0.1m y:0.2m
|
||
|
||
* 里程计朝向
|
||
* 车辆上电后的朝向,为0方向,逆时针为正。
|
||
* 4字节消息类型:0x2D,0x00,0x22,0x00
|
||
* 8字节数据内容:前4个字节为朝向角,单位为rad。
|
||
* 数据类型为float
|
||
* 范例:0xFE,0x2D,0x00,0x22,0x00,0x9A,0x99,0x99,0x3E,0x00,0x00,0x00,0x00,0xD9
|
||
* 说明:0.3rad
|
||
|
||
* 左电机速度
|
||
* 4字节消息类型:0x2D,0x11,0x11,0x00
|
||
* 8字节数据内容:前4个字节为速度,单位rad/s
|
||
* 数据类型:float
|
||
* 范例:0xFE,0x2D,0x11,0x11,0x00,0xCD,0xCC,0xCC,0x3D,0x00,0x00,0x00,0x00,0xC5
|
||
* 含义:0.1rad/s,如果要转换为m/s,需要乘以车轮半径
|
||
|
||
* 右电机速度:
|
||
* 4字节消息类型:0x2D,0x10,0x11,0x00
|
||
* 8字节数据内容:前4个字节为速度,单位rad/s
|
||
* 数据类型:float
|
||
* 范例:0xFE,0x2D,0x10,0x11,0x00,0xCD,0xCC,0x4C,0x3E,0x00,0x00,0x00,0x00,0xB4
|
||
* 含义:0.2rad/s,如果要转换为m/s,需要乘以车轮半径
|
||
|
||
* 转向角度:
|
||
* 4字节消息类型:0x2D,0x20,0x11,0x00
|
||
* 8字节数据内容:前4个字节为角度,单位rad
|
||
* 数据类型:float
|
||
范例:0xFE,0x2D,0x20,0x11,0x00,0xCD,0xCC,0xCC,0x3D,0x00,0x00,0x00,0x00,0x26
|
||
* 含义:0.1rad,正方向为向左转
|
||
|
||
|
||
## 5、CRC8校验
|
||
|
||
* 使用CRC-8/MAXIM方式计算
|
||
|
||
* 示例:在线计算crc8
|
||
|
||
![img](img/crc8-maxim.png)
|
||
|
||
<span id = "crc8-table"></span>
|
||
|
||
* 查表法计算crc8
|
||
|
||
```
|
||
const uint8_t CRC8Table[]={
|
||
0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, 65,
|
||
157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96, 130, 220,
|
||
35, 125, 159, 193, 66, 28, 254, 160, 225, 191, 93, 3, 128, 222, 60, 98,
|
||
190, 224, 2, 92, 223, 129, 99, 61, 124, 34, 192, 158, 29, 67, 161, 255,
|
||
70, 24, 250, 164, 39, 121, 155, 197, 132, 218, 56, 102, 229, 187, 89, 7,
|
||
219, 133, 103, 57, 186, 228, 6, 88, 25, 71, 165, 251, 120, 38, 196, 154,
|
||
101, 59, 217, 135, 4, 90, 184, 230, 167, 249, 27, 69, 198, 152, 122, 36,
|
||
248, 166, 68, 26, 153, 199, 37, 123, 58, 100, 134, 216, 91, 5, 231, 185,
|
||
140, 210, 48, 110, 237, 179, 81, 15, 78, 16, 242, 172, 47, 113, 147, 205,
|
||
17, 79, 173, 243, 112, 46, 204, 146, 211, 141, 111, 49, 178, 236, 14, 80,
|
||
175, 241, 19, 77, 206, 144, 114, 44, 109, 51, 209, 143, 12, 82, 176, 238,
|
||
50, 108, 142, 208, 83, 13, 239, 177, 240, 174, 76, 18, 145, 207, 45, 115,
|
||
202, 148, 118, 40, 171, 245, 23, 73, 8, 86, 180, 234, 105, 55, 213, 139,
|
||
87, 9, 235, 181, 54, 104, 138, 212, 149, 203, 41, 119, 244, 170, 72, 22,
|
||
233, 183, 85, 11, 136, 214, 52, 106, 43, 117, 151, 201, 74, 20, 246, 168,
|
||
116, 42, 200, 150, 21, 75, 169, 247, 182, 232, 10, 84, 215, 137, 107, 53 };
|
||
|
||
uint8_t can_crc8_calculate(uint8_t *p, uint8_t counter)
|
||
{
|
||
uint8_t crc8 = 0;
|
||
for( ; counter > 0; counter--){
|
||
crc8 = CRC8Table[crc8^*p];
|
||
p++;
|
||
}
|
||
return(crc8);
|
||
}
|
||
```
|