kobuki驱动通信规范

kobuki node和kobuki 底盘framwork通信通过串口/dev/kobuki(默认情况下)，分析下kobuki驱动通信的规范，就可以清楚的明白kobuki地盘framwork做了什么工作。再结合kobuki node订阅和发布的topics，就可以清楚知道kobuki node做了什么工作。

通信分成两种：命令（node->framwork）和反馈（node<-framwork）;

通信方式serial.write("/dev/kobuki",xxxxxxx);和serial.read("/dev/kobuki",xxxxxxx);

先简单介绍下通信数据结构：命令和反馈的结构是一样的

Name Header 0 Header 1 Length Payload Checksum
Size 1 Byte 1 Byte 1 Byte N Bytes 1 Byte
Description 0xAA
(Fixed)		 0x55
(Fixed)		 Size of payload in bytes Described below XOR'ed value of every bytes of bytesream except headers

Payload
Sub-Payload 0 Sub-Payload 1 Sub-Payload 2 ... Sub-Payload N-1

Sub-Payload:
Name Header Length Data
Size 1 Byte 1 Byte N Byte(s)
Description Predefined Identifier Size of data in byte(s) Described below

Checksum：校验位

unsigned int packet_size(buffer.size());
unsigned char cs(0);
for (unsignedint i = 2; i < packet_size; i++)
{
cs ^= buffer[i];
}
return cs ? false : true;


总览：

Headers Length Payload Checksum
Header 0 Header 1 Sub-Payload 0 Sub-Payload 1 Sub-Payload 2 ... Sub-Payload N-1

1.命令格式：
命令ID

ID Name Description
1 Base Control 控制轮胎电机
2 Reserved  
3 Sound 播放用户自定义声音
4 Sound Sequence 播放预定义的声音
5 Reserved  
6 Reserved  
7 Reserved  
8 Reserved  
9 Request Extra 请求额外的信息
10 Reserved  
11 Reserved  
12 General Purpose Output 控制一般用途的输出
13 Set Controller Gain 设置轮胎速度控制器的PID增益
14 Get Controller Gain 请求当前轮胎速度控制器的PID增益

1.1 Base Control

向固件传递的是两个值：速度和半径，格式如下：

  Name Size Value Value in Hex Description
Header Identifier 1 1 0x01 Fixed
Length Size of data field 1 4 0x04 Fixed
Data Speed 2     in mm/s
Radius 2     in mm

其中速度和半径的计算公式：

Motion Speed(mm/s) Radius(mm)
Pure Translation v 0
Pure Rotation wi) * bii) / 2 1
Translation + Rotation （radius  + bii) / 2）× wi), if radius > 1
（radius  -  bii) / 2）* wi) , if radius < -1		 v/w

v：传入kobuki driver的线速度

w：传入kobuki driver的的旋转角速度  单位：每秒多少弧度，rad/s

b：轴距

举例说明：w为弧度每秒，根据数学公式，弧长 = 弧度×半径，（不明白的去百度上搜，我以前把w当成角度/s，结果怎么看公式都看不明白，后来当成弧度/s,这才看明白）

弧长/s = v(速度) = w（弧度/s）×半径 ->randius = v/w -> v = randius * w

以下图为例，robot往左前方运动，radius为左轮到原点的距离，而传入的speed为轴中心的速度。轴中心距左轮的距离为b/2，所以轴中心距园点的半径实际为（radius+b/2），

故，轴中心的速度为（radius+b/2）×w

kobuki驱动通信规范
Velocity Representation


1.2 Sound

播放自定义声音，在固件中实现，在kobuki driver中没有实现

  Name Size Value Value in Hex Description
Header Identifier 1 3 0x03 Fixed
Length Size of data field 1 3 0x03 Fixed
Data Note 2     kobuki驱动通信规范, where kobuki驱动通信规范 is frequency of sound in Hz, and kobuki驱动通信规范 is 0.00000275
Duration 1     Duration of playing note in milli-seconds


1.3 Sound Sequence

播放预置声音

  Name Size Value Value in Hex Description
Header Identifier 1 4 0x04 Fixed
Length Size of data field 1 1 0x01 Fixed
Data Sequence number 1     0 for ON sound
1 for OFF sound
2 for RECHARGE sound
3 for BUTTON sound
4 for ERROR sound
5 for CLEANINGSTART sound
6 for CLEANINGEND sound


1.4 Request Extra

请求硬件版本号，固件版本号，唯一ID

  Name Size Value Value in Hex Description
Header Identifier 1 9 0x09 Fixed
Length Size of data field 1 2 0x02 Fixed
Data Request flags 2     Set the flags to request extra data
0x01 for Hardware Version
0x02 for Firmware Version
0x08 for Unique Device ID


1.5 General Purpose Output

控制数字输出，外部电源和LED1/2

  Name Size Value Value in Hex Description
Header Identifier 1 12 0x0C Fixed
Length Size of data field 1 2 0x02 Fixed
Data Digital output flags 2     Set the flags to set high on output pins of expansion port
0x0001 for digital output ch. 0
0x0002 for digital output ch. 1
0x0004 for digital output ch. 2
0x0008 for digital output ch. 3

Set the flags to turn on external powers
0x0010 for external power 3.3V ch.
0x0020 for external power 5V ch.
0x0040 for external power 12V/5A ch.
0x0080 for external power 12V/1.5A ch.

Set the flags to turn on LEDs
0x0100 for red colour of LED1
0x0200 for green colour of LED1
0x0400 for red colour of LED2
0x0800 for green colour of LED2


1.6 Set Controller Gain

  Name Size Value Value in Hex Description
Header Identifier 1 1 0x01 Fixed
Length Size of data field 1 13 0x0D Fixed
Data Type 1     0 for factory-default PID gain
1 for user-configured PID gain
P gain 4     Kp * 1000 (default: 100*1000)
I gain 4     Ki * 1000 (default: 0.1*1000)
D gain 4     Kd * 1000 (default: 2*1000)


1.7 Get Controller Gain

  Name Size Value Value in Hex Description
Header Identifier 1 1 0x01 Fixed
Length Size of data field 1 14 0x0E Fixed
Data unused 1      


2.反馈

当kobuki上电后，以50Hz的频率周期行反馈数据上来。0.02s反馈一次，基本上while中每次serial.read读完处理数据后不需要sleep

ID Name Description Availability
1 Basic Sensor Data 基础核心传感器数据 By default
2 Reserved    
3 Docking IR 来自对接站的信号 By default
4 Inertial Sensor 陀螺仪数据，包括角度和角速度 By default
5 Cliff 面向地面的PSD数据 By default
6 Current 轮胎电机的电流 By default
7 Reserved    
8 Reserved    
9 Reserved    
10 Hardware Version kobuki 硬件版本号 On request
11 Firmware Version kobuki firmware版本号 On request
12 Reserved    
13 Raw data of 3-axis gyro 数字3轴陀螺仪原始ADC数据 By default
14 Reserved    
15 Reserved    
16 General Purpose Input 25-pin额外端口的输入数据 By default
17 Reserved    
18 Reserved    
19 Unique Device IDentifier(UDID) 识别机器人的唯一编号 On request
20 Reserved    
21 Controller Info 轮胎速度控制器的PID增益值 On request

2.1 Basic Sensor Data

  Name Size Value Value in Hex Description
Header Feedback Identifier 1 1 0x01 Fixed
Length Size of data field 1 15 0x0F Fixed
Data Timestamp 2     毫秒级时间戳
0 到 65535
Bumper 1     保险杠被按下标志位
0x01 for 右侧
0x02 for 中间
0x04 for 左侧
Wheel drop 1     轮胎丢失
0x01 for 右侧
0x02 for 左侧
Cliff 1     悬崖传感器被检测到
0x01 for 右侧
0x02 for 中间
0x04 for 左侧
Left encoder 2     左右轮胎的编码器在前进方面的逐步增加的ticks
0 到 65535
Right encoder 2    
Left PWM 1     左右轮胎电机的施加的最大电压，数值为百分比%。
值为有符号的，正为正方向，负为反方向,取值为-100 to 100
Right PWM 1    
Button 1     按键被按下的标志位
0x01 for Button 0
0x02 for Button 1
0x04 for Button 2
Charger 1     0 for DISCHARGING state
2 for DOCKING_CHARGED state
6 for DOCKING_CHARGING state
18 for ADAPTER_CHARGED state
22 for ADAPTER_CHARGING state
Battery 1     Voltage of battery in 0.1 V
Typically 16.7 V when fully charged
Overcurrent flags 1     过载电流标志位
0x01 for left wheel
0x02 for right wheel

2.2 Docking IR

kobuki有3个红外线发射器，

  Name Size Value Value in Hex Description
Header Identifier 1 3 0x03 Fixed
Length Size of data field 1 3 0x03 Fixed
Data Right signal 1     Flag will be setted when signal is detected
0x01 for NEAR_LEFT state
0x02 for NEAR_CENTER state
0x04 for NEAR_RIGHT state
0x08 for FAR_CENTER state
ox10 for FAR_LEFT state
0x20 for FAR_RIGHT state
Central signal 1    
Left signal 1    


2.3 Inertial Sensor

只有z轴数据

  Name Size Value Value in Hex Description
Header Identifier 1 4 0x04 Fixed
Length Size of data field 1 7 0x07 Fixed
Data Angle 2     Factory calibrated
Angle rate 2     Factory calibrated
Unused 1      
Unused 1      
Unused 1      


2.4 Cliff

PSD传感器提供面对地面的ACD数据，标志地面和传感器之间的距离。

  Name Size Value Value in Hex Description
Header Identifier 1 5 0x05 Fixed
Length Size of data field 1 6 0x06 Fixed
Data Right cliff sensor 2     ADC output of each PSD
Data range: 0 ~ 4095 (0 ~ 3.3V)
Distance range: 2 ~ 15 cm
Distance is not linear w.r.t. ADC output.
See the datasheet for more detail.
Central cliff sensor 2    
Left cliff sensor 2    


2.5 Current

  Name Size Value Value in Hex Description
Header Identifier 1 6 0x06 Fixed
Length Size of data field 1 2 0x02 Fixed
Data Left motor 2     in 10mA
Right motor 2     in 10mA

2.6 Hardware Version

硬件版本分成三部分：major.minor.patch

  Name Size Value Value in Hex Description
Header Identifier 1 10 0x0A Fixed
Length Size of data field 1 4 0x04 Fixed
Data Patch 1      
Minor 1      
Major 1      
Unused 1 0 0x00 Fixed


2.7 Firmwaware Version

同上

  Name Size Value Value in Hex Description
Header Identifier 1 11 0x0B Fixed
Length Size of data field 1 4 0x04 Fixed
Data Patch 1      
Minor 1      
Major 1      
Unused 1 0 0x00 Fixed

2.8 Raw data of 3-axis gyro

数字3D陀螺仪原始数据。由于采集频率和更新频率不同，可能会有2～3组数据同时到达。

数字对deg/s的比例为0.00875。

  Name Size Value Value in Hex Description
Header Identifier 1 13 0x0D Fixed
Length Size of data field 1 2+6N    
Data Frame id 1     Frame id of 'Raw gyro data 0'
Every sensor readings can identified by frame id
, It circulate from 0 to 255
Followed data length 1 3N    
Raw gyro data 0 x-axis 2     ADC output of each-axis in 0.00875 deg/s
y-axis 2    
z-axis 2    
:
:
Raw gyro data N-1 x-axis 2    
y-axis 2    
z-axis 2    

遥感三维陀螺仪轴不匹配的机器人。这是关于Z轴逆时针旋转90度。因此，需要以下转换。

constdouble digit_to_dps = 0.00875;

angular_velocity.x = -digit_to_dps * (short)raw_gyro_data.y;
angular_velocity.y = digit_to_dps * (short)raw_gyro_data.x;
angular_velocity.z = digit_to_dps * (short)raw_gyro_data.z;

2.9 General Purpose Input

  Name Size Value Value in Hex Description
Header Identifier 1 16 0x10 Fixed
Length Size of data field 1 16 0x10 Fixed
Data Digital input 2     Flag will be setted, when high voltage is applied
0x01 for digital input ch. 0
0x02 for digital input ch. 1
0x04 for digital input ch. 2
0x08 for input output ch. 3
Analog input ch.0 2     12-bit ADC output of each channel
Data range: 0 ~ 4095(2^12-1)
Voltage range: 0 ~ 3.3 V
Analog input ch.1 2    
Analog input ch.2 2    
Analog input ch.3 2    
Unused 2      
Unused 2      
Unused 2      


2.10 Unique Device IDentifier(UDID)

组成为：<UDID0>-<UDID1>-<UDID2>

  Name Size Value Value in Hex Description
Header Identifier 1 19 0x13 Fixed
Length Size of data field 1 12 0x0C Fixed
Data UDID0 4      
UDID1 4      
UDID2 4      

2.11 Controller Info

  Name Size Value Value in Hex Description
Header Identifier 1 1 0x01 Fixed
Length Size of data field 1 21 0x15 Fixed
Data Type 1     Current controller setup
0 for factory-default PID gain
1 for user-configured PID gain
P gain 4     Kp * 1000 (default: 100*1000)
I gain 4     Ki * 1000 (default: 0.1*1000)
D gain 4     Kd * 1000 (default: 2*1000)




相关文章：

猜你喜欢
相关资源
相似解决方案
热门标签
Java Python linux javascript Mysql C# Docker 算法 前端 SpringBoot Redis Vue spring 设计模式 .net core .net kubernetes c++ 数据库 数据结构 大数据 js 机器学习 微服务 Android Go 程序员 面试 JVM ASP.net core 云原生 人工智能 后端 PHP git CSS golang k8s Nginx Django mybatis 深度学习 多线程 React 架构 devops 爬虫 云计算 Spring Boot LeetCode