HiveCoreRD/hivecore_robot_drivers
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

motor_dev

Browse Source
This commit is contained in:
hehe
2025-10-16 10:05:13 +08:00
parent dedc31a46e
commit 1a370a796d
6 changed files with 496 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
motor_dev/CMakeLists.txt Normal file
View File

@@ -0,0 +1,33 @@
cmake_minimum_required(VERSION 3.8)
project(motor_dev)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(std_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME} "msg/MotorCmd.msg")
include_directories(include ${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_cpp)
add_executable(motor_dev_node src/main.cpp src/rs485_driver.cpp)
add_dependencies(motor_dev_node ${PROJECT_NAME}__rosidl_typesupport_cpp)
target_link_libraries(motor_dev_node ${PROJECT_NAME}__rosidl_typesupport_cpp )
ament_target_dependencies(motor_dev_node rclcpp std_msgs)
install(TARGETS motor_dev_node DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

61
motor_dev/include/motor_dev/rs485_driver.hpp Normal file
View File

@@ -0,0 +1,61 @@
#ifndef RS485_DRIVER_HPP
#define RS485_DRIVER_HPP
#include <string>
#include <vector>
#include <termios.h>
namespace motor_dev
{
class RS485Driver
{
public:
RS485Driver();
~RS485Driver();
// 打开串口
bool openPort(const std::string &port_name, int baud_rate = 115200);
// 关闭串口
void closePort();
// 发送数据
bool sendData(const std::vector<uint8_t> &data);
// 接收数据
bool receiveData(std::vector<uint8_t> &data, size_t max_length, int timeout_ms = 100);
//本末
int bm_set_mode(uint8_t motor_id, uint8_t mode);
int bm_set_pos(uint8_t motor_id, float angle);
float bm_get_pos(uint8_t motor_id);
//脉塔
void add_crc16(std::vector<uint8_t> &data);
int mt_set_pos(uint8_t motor_id, float angle);
float mt_get_pos(uint8_t motor_id);
private:
int com_fd_;
//bool is_open_;
struct termios original_termios_;
//size_t max_motors_;
//size_t control_index_;
// 转换波特率
speed_t convertBaudRate(int baud_rate);
// 电机协议常量
// static constexpr uint8_t START_BYTE = 0x64;
// static constexpr uint8_t STOP_BYTE = 0x55;
// static constexpr uint8_t CONTROL_CMD = 0x64;
// static constexpr uint8_t ZERO_CMD = 0x00;
// static constexpr uint8_t SPEED_CMD = 0x14;
// static constexpr uint8_t DEFAULT_TIME = 0x64;
// static constexpr uint8_t STOP_CMD = 0x02;
// static constexpr uint8_t ALL_MOTORS_CMD = 0x03;
};
}
#endif // RS485_DRIVER_HPP

5
motor_dev/msg/MotorCmd.msg Normal file
View File

@@ -0,0 +1,5 @@
string target
string type
string position
int32[] motor_id
float32[] motor_angle

21
motor_dev/package.xml Normal file
View File

@@ -0,0 +1,21 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>motor_dev</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="you@example.com">h</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<depend>std_msgs</depend>
<depend>rclcpp</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

36
motor_dev/src/main.cpp Normal file
View File

@@ -0,0 +1,36 @@
#include"rclcpp/rclcpp.hpp"
#include"motor_dev/rs485_driver.hpp"
#include"motor_dev/msg/motor_cmd.hpp"
using namespace motor_dev;
RS485Driver rs485_driver_;
int main(int argc,char* argv[]){
rclcpp::init(argc,argv);
auto node=rclcpp::Node::make_shared("motor_dev_node");
std::cout << "open RS485 port.." << std::endl;
if (!rs485_driver_.openPort("/dev/ttyS0", 115200)){
std::cout << "Failed to open RS485 port." << std::endl;
}
//init
rs485_driver_.bm_set_mode(1,3);//位置模式
auto rs485_sub_=node->create_subscription<motor_dev::msg::MotorCmd>("/motor_cmd",10,[](const motor_dev::msg::MotorCmd::SharedPtr msg){
size_t n = msg->motor_id.size();
if (msg->target == "rs485") {
for (size_t i = 0; i < n; ++i) {
int32_t id = msg->motor_id[i];
float angle =msg->motor_angle[i];
std::cout<<"recv:"<<msg->type<<","<<id<<","<<angle<<std::endl;
if(msg->type=="bm"){
rs485_driver_.bm_set_pos(id,angle);
}else if(msg->type=="mt"){
rs485_driver_.mt_set_pos(id,angle);
}
}
}else if(msg->target=="ethercat"){
//TODO
}
});
rs485_driver_.closePort();
rclcpp::spin(node);
rclcpp::shutdown();
}

340
motor_dev/src/rs485_driver.cpp Normal file
View File

@@ -0,0 +1,340 @@
#include "motor_dev/rs485_driver.hpp"
#include <fcntl.h>
#include <unistd.h>
#include <cstring>
#include <iostream>
#include <chrono>
#include <thread>
#include "motor_dev/rs485_driver.hpp"
namespace motor_dev
{
const unsigned char 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
};
/**
* @name CRC8_Table
* @brief This function is used to Get CRC8 check value.
* @param p :Array pointer. counter :Number of array elements.
* @retval crc8
*/
unsigned char CRC8_Table(unsigned char *p, int counter)
{
unsigned char crc8 = 0;
for( ; counter > 0; counter--)
{
crc8 = CRC8Table[crc8^*p];
p++;
}
return(crc8);
}
RS485Driver::RS485Driver()
{
}
RS485Driver::~RS485Driver()
{
closePort();
}
speed_t RS485Driver::convertBaudRate(int baud_rate)
{
switch (baud_rate)
{
case 9600: return B9600;
case 19200: return B19200;
case 38400: return B38400;
case 57600: return B57600;
case 115200: return B115200;
case 230400: return B230400;
case 460800: return B460800;
case 500000: return B500000;
case 576000: return B576000;
case 921600: return B921600;
default: return B0;
}
}
bool RS485Driver::openPort(const std::string &port_name, int baud_rate)
{
// 打开串口
com_fd_ = open(port_name.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
if (com_fd_ == -1)
{
std::cerr << "无法打开串口: " << port_name << std::endl;
return false;
}
// 保存原始配置
if (tcgetattr(com_fd_, &original_termios_) == -1)
{
std::cerr << "无法获取串口属性" << std::endl;
close(com_fd_);
com_fd_ = -1;
return false;
}
// 配置串口
struct termios tty;
memset(&tty, 0, sizeof(tty));
// 设置波特率
speed_t speed = convertBaudRate(baud_rate);
if (speed == B0)
{
std::cerr << "不支持的波特率: " << baud_rate << std::endl;
closePort();
return false;
}
cfsetospeed(&tty, speed);
cfsetispeed(&tty, speed);
// 8位数据位，无奇偶校验，1位停止位
tty.c_cflag &= ~PARENB; // 无奇偶校验
tty.c_cflag &= ~CSTOPB; // 1位停止位
tty.c_cflag &= ~CSIZE; // 清除数据位设置
tty.c_cflag |= CS8; // 8位数据位
// 禁用硬件流控制
tty.c_cflag &= ~CRTSCTS;
// 启用接收器，设置本地模式
tty.c_cflag |= (CLOCAL | CREAD);
// 禁用软件流控制
tty.c_iflag &= ~(IXON | IXOFF | IXANY);
// 原始输入模式
tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
// 原始输出模式
tty.c_oflag &= ~OPOST;
// 设置读取超时
tty.c_cc[VTIME] = 10; // 1秒超时
tty.c_cc[VMIN] = 0;
// 应用配置
if (tcsetattr(com_fd_, TCSANOW, &tty) != 0)
{
std::cerr << "无法设置串口属性" << std::endl;
closePort();
return false;
}
std::cout << "串口 " << port_name << " 已打开，波特率: " << baud_rate << std::endl;
return true;
}
void RS485Driver::closePort()
{
if (com_fd_>0)
{
// 恢复原始配置
tcsetattr(com_fd_, TCSANOW, &original_termios_);
close(com_fd_);
com_fd_ = -1;
std::cout << "串口已关闭" << std::endl;
}
}
bool RS485Driver::sendData(const std::vector<uint8_t> &data)
{
ssize_t bytes_written = write(com_fd_, data.data(), data.size());
if (bytes_written != static_cast<ssize_t>(data.size()))
{
std::cerr << "rs485,fd:"<<com_fd_<<",发送ERR:" <<bytes_written<<"/"<<data.size()<<std::endl;
return false;
}
return true;
}
bool RS485Driver::receiveData(std::vector<uint8_t> &data, size_t max_length, int timeout_ms)
{
data.clear();
uint8_t buffer[256];
ssize_t bytes_read;
auto start_time = std::chrono::steady_clock::now();
while (data.size() < max_length)
{
// 检查超时
auto current_time = std::chrono::steady_clock::now();
auto elapsed_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
current_time - start_time).count();
if (elapsed_ms >= timeout_ms)
{
break; // 超时
}
// 尝试读取数据
bytes_read = read(com_fd_, buffer, std::min(sizeof(buffer), max_length - data.size()));
if (bytes_read > 0)
{
data.insert(data.end(), buffer, buffer + bytes_read);
start_time = std::chrono::steady_clock::now(); // 重置超时计时器
}
else if (bytes_read < 0)
{
std::cerr << "接收数据错误" << std::endl;
return false;
}
else
{
// 没有数据，短暂休眠
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
}
return !data.empty();
}
int RS485Driver::bm_set_mode(uint8_t motor_id, uint8_t mode){
std::vector<uint8_t> command;
if(mode>3)
mode=3;
command.push_back(motor_id);
command.push_back(0xa0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(mode);
return 0;
}
int RS485Driver::bm_set_pos(uint8_t motor_id, float angle){
std::vector<uint8_t> command;
if(angle>360.0)
angle=360.0;
int32_t pos=angle*32767/360.0;
uint8_t high_byte = (pos >> 8) & 0xFF; // 高8位
uint8_t low_byte = pos & 0xFF; // 低8位
command.push_back(motor_id);
command.push_back(0x64);
command.push_back(high_byte);
command.push_back(low_byte);
command.push_back(0);
command.push_back(0);
command.push_back(0x64);
command.push_back(0);
unsigned char crc8 = CRC8_Table(command.data(), 9);
command.push_back(crc8);
bool ret = sendData(command);
return 0;
}
float RS485Driver::bm_get_pos(uint8_t motor_id){
std::vector<uint8_t> command;
command.push_back(motor_id);
command.push_back(0x74);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
unsigned char crc8 = CRC8_Table(command.data(), 9);
command.push_back(crc8);
bool ret = sendData(command);
if(ret){
std::vector<uint8_t> data;
bool ret=receiveData(data, 20, 1000);
if(ret){
uint8_t pos = data[7];
float angle = pos * 360.0 /255.0;
return angle;
}
}
return -1;
}
//mt
void RS485Driver::add_crc16(std::vector<uint8_t>& data) {
uint16_t crc = 0xFFFF;
for (uint8_t byte : data) {
crc ^= byte;
for (int i = 0; i < 8; i++) {
if (crc & 0x0001) {
crc >>= 1;
crc ^= 0xA001;
} else {
crc >>= 1;
}
}
}
data.push_back(static_cast<uint8_t>(crc & 0xFF));
data.push_back(static_cast<uint8_t>((crc >> 8) & 0xFF));
}
//
int RS485Driver::mt_set_pos(uint8_t motor_id, float angle){
std::vector<uint8_t> packet;
packet.push_back(0x3e);
packet.push_back(motor_id);
packet.push_back(0x08);
uint16_t pos = static_cast<uint16_t>(angle * 100);
uint8_t low=pos & 0xff;
uint8_t high=(pos>>8)&0xff;
packet.push_back(0xa6);
packet.push_back(0x00);//方向
packet.push_back(0x01);
packet.push_back(0x00);//速度
packet.push_back(low);//位置,pos的低8位
packet.push_back(high);//位置，pos的高8位
packet.push_back(0);
packet.push_back(0x0);
add_crc16(packet);
return 0;
}
float RS485Driver::mt_get_pos(uint8_t motor_id){
std::vector<uint8_t> packet;
packet.push_back(0x3e);
packet.push_back(motor_id);
packet.push_back(0x08);
//
packet.push_back(0x94);
packet.push_back(0);
packet.push_back(0);
packet.push_back(0);
packet.push_back(0);
packet.push_back(0);
packet.push_back(0);
packet.push_back(0);
add_crc16(packet);
bool ret=sendData(packet);
if(ret){
std::vector<uint8_t> data;
ret=receiveData(data, 20, 1000);
if(ret){
uint32_t pos = (data[7]<<8)+(data[6]&0xff);
float angle = pos /100.0;
return angle;
}
}
return -1;
}
}