mbot_linux_serial.cpp文件- //包含对应头文件
- #include "mbot_linux_serial.h"
- using namespace std;//设定工作空间的名称
- using namespace boost::asio;//设定工作空间的名称
- //串口相关对象
- //创建一个 io_service实例
- boost::asio::io_service iosev;
- //构造一个串口，将"/dev/ttySUB0"转移给实例iosev
- boost::asio::serial_port sp(iosev, "/dev/ttyUSB0");
- boost::system::error_code err;
- /********************************************************
- 串口发送接收相关常量、变量、共用体对象
- ********************************************************/
- const unsigned char ender[2] = {0x0d, 0x0a};//数据尾
- const unsigned char header[2] = {0x55, 0xaa};//数据头
- //发送左右轮速控制速度共用体
- union sendData
- {
- short d;
- unsigned char data[2];
- }leftVelSet,rightVelSet;
- //接收数据（左轮速、右轮速、角度）共用体（-32767 - +32768）
- union receiveData
- {
- short d;
- unsigned char data[2];
- }leftVelNow,rightVelNow,angleNow;
- /********************************************************
- 函数功能：串口参数初始化
- 入口参数：无
- 出口参数：
- ********************************************************/
- void serialInit()
- {
- sp.set_option(serial_port::baud_rate(115200));//设置波特率
- sp.set_option(serial_port::flow_control(serial_port::flow_control::none));//流量控制
- sp.set_option(serial_port::parity(serial_port::parity::none));//奇偶校验
- sp.set_option(serial_port::stop_bits(serial_port::stop_bits::one));//停止位
- sp.set_option(serial_port::character_size(8)); //字符大小
- }
- /********************************************************
- 函数功能：将对机器人的左右轮子控制速度，打包发送给下位机
- 入口参数：机器人线速度、角速度
- 出口参数：
- ********************************************************/
- void writeSpeed(double Left_v, double Right_v,unsigned char ctrlFlag)
- {
- unsigned char buf[11] = {0};//
- int i, length = 0;
- leftVelSet.d = Left_v;//mm/s
- rightVelSet.d = Right_v;
- // 设置消息头
- for(i = 0; i < 2; i++)
- buf[i] = header[i]; //buf[0] buf[1]
-
- // 设置机器人左右轮速度
- length = 5;
- buf[2] = length; //buf[2]
- for(i = 0; i < 2; i++)
- {
- buf[i + 3] = leftVelSet.data[i]; //buf[3] buf[4]
- buf[i + 5] = rightVelSet.data[i]; //buf[5] buf[6]
- }
- // 预留控制指令
- buf[3 + length - 1] = ctrlFlag; //buf[7]
- // 设置校验值、消息尾
- buf[3 + length] = getCrc8(buf, 3 + length);//buf[8]
- buf[3 + length + 1] = ender[0]; //buf[9]
- buf[3 + length + 2] = ender[1]; //buf[10]
- // 通过串口下发数据
- boost::asio::write(sp, boost::asio::buffer(buf));
- }
- /********************************************************
- 函数功能：从下位机读取数据
- 入口参数：机器人左轮轮速、右轮轮速、角度，预留控制位
- 出口参数：bool
- ********************************************************/
- bool readSpeed(double &Left_v,double &Right_v,double &Angle,unsigned char &ctrlFlag)
- {
- char i, length = 0;
- unsigned char checkSum;
- unsigned char buf[150]={0};
- //=========================================================
- //此段代码可以读数据的结尾，进而来进行读取数据的头部
- try
- {
- boost::asio::streambuf response;
- boost::asio::read_until(sp, response, "\r\n",err);
- copy(istream_iterator<unsigned char>(istream(&response)>>noskipws),
- istream_iterator<unsigned char>(),
- buf);
- }
- catch(boost::system::system_error &err)
- {
- ROS_INFO("read_until error");
- }
- //=========================================================
- // 检查信息头
- if (buf[0]!= header[0] || buf[1] != header[1]) //buf[0] buf[1]
- {
- ROS_ERROR("Received message header error!");
- return false;
- }
- // 数据长度
- length = buf[2]; //buf[2]
- // 检查信息校验值
- checkSum = getCrc8(buf, 3 + length); //buf[10] 计算得出
- if (checkSum != buf[3 + length]) //buf[10] 串口接收
- {
- ROS_ERROR("Received data check sum error!");
- return false;
- }
- // 读取速度值
- for(i = 0; i < 2; i++)
- {
- leftVelNow.data[i] = buf[i + 3]; //buf[3] buf[4]
- rightVelNow.data[i] = buf[i + 5]; //buf[5] buf[6]
- angleNow.data[i] = buf[i + 7]; //buf[7] buf[8]
- }
- // 读取控制标志位
- ctrlFlag = buf[9];
-
- Left_v =leftVelNow.d;
- Right_v =rightVelNow.d;
- Angle =angleNow.d;
- return true;
- }
- /********************************************************
- 函数功能：获得8位循环冗余校验值
- 入口参数：数组地址、长度
- 出口参数：校验值
- ********************************************************/
- unsigned char getCrc8(unsigned char *ptr, unsigned short len)
- {
- unsigned char crc;
- unsigned char i;
- crc = 0;
- while(len--)
- {
- crc ^= *ptr++;
- for(i = 0; i < 8; i++)
- {
- if(crc&0x01)
- crc=(crc>>1)^0x8C;
- else
- crc >>= 1;
- }
- }
- return crc;
- }
| 
楼主
