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

add motor_dev can interface

Browse Source
This commit is contained in:
hehe
2026-01-05 18:02:00 +08:00
parent 9c723941a9
commit bc3e7beb76
5 changed files with 836 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
motor_dev_can/CMakeLists.txt Normal file
View File

@@ -0,0 +1,35 @@
cmake_minimum_required(VERSION 3.8)
project(motor_dev_can)
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)
find_package(interfaces REQUIRED)
#rosidl_generate_interfaces(${PROJECT_NAME} "msg/MotorCmd.msg")
#rosidl_generate_interfaces(${PROJECT_NAME} "msg/MotorCmd.msg" "msg/MotorPos.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 interfaces )
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()

69
motor_dev_can/include/motor_dev/rs485_driver.hpp Normal file
View File

@@ -0,0 +1,69 @@
#ifndef RS485_DRIVER_HPP
#define RS485_DRIVER_HPP
#include <string>
#include <vector>
#include <termios.h>
#include <mutex>
#include<map>
namespace motor_dev
{
class RS485Driver
{
public:
RS485Driver();
~RS485Driver();
// 打开串口
bool openPort(const std::string &port_name, int baud_rate = 115200);
std::map<uint8_t, float> speedMap;
// 关闭串口
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);
std::vector<uint8_t> sendCan(uint8_t cmd,const std::vector<uint8_t> data);
//本末
int bm_set_enable(uint8_t enable);
int bm_set_mode(uint8_t motor_id, uint8_t mode);
int bm_set_pos(float angle1, float angle2);
int bm_set_speed(float speed1, float speed2,float speed3,float speed4);
float bm_get_pos(uint8_t motor_id);
int bm_get_speed();
int bm_query_id();
int bm_set_param(uint8_t motor_id, uint8_t param,uint32_t val);
//脉塔
int mt_query_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_;
std::mutex uart_mutex;
//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

21
motor_dev_can/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_can</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>

100
motor_dev_can/src/main.cpp Normal file
View File

@@ -0,0 +1,100 @@
#include"rclcpp/rclcpp.hpp"
#include"motor_dev/rs485_driver.hpp"
#include"interfaces/msg/motor_cmd.hpp"
#include"interfaces/msg/motor_pos.hpp"
#include"interfaces/srv/motor_param.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");
////auto pos_pub = node->create_publisher<motor_dev::msg::MotorPos>("/motor_pos", 10);
std::cout << "open RS485 port.." << std::endl;
if (!rs485_driver_.openPort("/dev/ttyACM0", 921600)){
std::cout << "Failed to open RS485 port." << std::endl;
}
usleep(100000);
rs485_driver_.speedMap.clear();
rs485_driver_.bm_set_enable(1);
rs485_driver_.bm_set_param(1,28,3);
rs485_driver_.bm_set_param(2,28,3);
rs485_driver_.bm_set_param(3,28,3);
rs485_driver_.bm_set_param(4,28,3);
rs485_driver_.bm_set_enable(2);
rs485_driver_.bm_set_param(1,84,100);
rs485_driver_.bm_set_param(2,84,100);
rs485_driver_.bm_set_param(3,84,100);
rs485_driver_.bm_set_param(4,84,100);
//init
/////rs485_driver_.bm_set_mode(1,3);//位置模式
auto rs485_sub_=node->create_subscription<interfaces::msg::MotorCmd>("/motor_cmd",10,[](const interfaces::msg::MotorCmd::SharedPtr msg){
///
if (msg->target == "rs485") {
if(msg->type=="bm"){
size_t n = msg->motor_id.size();
if(n<2){
printf("bm msg need four angle\n");
return;
}
float speed1=msg->motor_speed[0];
float speed2=msg->motor_speed[1];
float speed3=msg->motor_speed[3];
float speed4=msg->motor_speed[4];
printf("###:%s,speed:%.1f,%.1f,%.1f,%.1f\n",msg->type.c_str(),speed1,speed2,speed3,speed4);
rs485_driver_.bm_set_speed(speed1,speed2,speed3,speed4);
}else if(msg->type=="mt"){
uint8_t id=msg->motor_id[0];
float angle =msg->motor_angle[0];
rs485_driver_.mt_set_pos(id,angle);
}
}
});
auto motor_pub=node->create_publisher<interfaces::msg::MotorPos>("/motor_pos",10);
//读取电机位置
rclcpp::TimerBase::SharedPtr timer_=node->create_wall_timer(std::chrono::milliseconds(5000), [=]() {
static int pub_cnt=0;
interfaces::msg::MotorPos motor_pos;
rs485_driver_.bm_get_speed();
motor_pos.motor_id.clear();
motor_pos.motor_angle.clear();
motor_pos.motor_speed.clear();
{
motor_pos.source = "rs485";
motor_pos.type = "bm";
motor_pos.position="none";
for(const auto& kv:rs485_driver_.speedMap){
printf("%d:%.1f,",kv.first,kv.second);
motor_pos.motor_id.push_back(kv.first);
motor_pos.motor_speed.push_back(kv.second);
}
printf("\n");
motor_pub->publish(motor_pos);
}
pub_cnt+=1;
});
auto motor_param_srv=node->create_service<interfaces::srv::MotorParam>("motor_param",[=](const std::shared_ptr<interfaces::srv::MotorParam::Request> req,std::shared_ptr<interfaces::srv::MotorParam::Response> res){
res->ret=0;
printf("recv sv:%d,%d\n",req->motor_id,req->accel);
if(req->accel<500){
rs485_driver_.bm_set_param(1,84,300);
res->ret=1;
printf("svc:%d\n",res->ret);
}
});
rclcpp::spin(node);
printf("now close rs485 port\n");
rs485_driver_.closePort();
rclcpp::shutdown();
}

611
motor_dev_can/src/rs485_driver.cpp Normal file
View File

@@ -0,0 +1,611 @@
#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
{
std::vector<uint8_t> RS485Driver::sendCan(uint8_t cmd,const std::vector<uint8_t> data) {
std::vector<uint8_t> packet;
packet.push_back(0x55); // 同步字节1
packet.push_back(0xAA); // 同步字节2
packet.push_back(0x1E); // 30字节
packet.push_back(0x01);
packet.push_back(0x01);
packet.push_back(0x00);
packet.push_back(0x00);
packet.push_back(0x00);
packet.push_back(0x0a);
packet.push_back(0x00);
packet.push_back(0x00);
packet.push_back(0x00);
packet.push_back(0x00);
packet.push_back(cmd);
packet.push_back(0x00);
packet.push_back(0x00);
packet.push_back(0x00);
packet.push_back(0x00);
packet.push_back(0x08);
packet.push_back(0x00);
packet.push_back(0x00);
for (uint8_t i = 0; i < 8; i++) {
packet.push_back(data[i]);
}
packet.push_back(0x00);
bool ret = sendData(packet);
std::vector<uint8_t> rx_data;
if(ret){
rx_data.clear();
ret=receiveData(rx_data, 100, 1);
if(ret){
printf("<---:");
for(auto d:rx_data)
printf("%02x ",d);
printf("\n");
return rx_data;
}
}
return rx_data;
}
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);
}
uint8_t crc8_maxim_direct(const uint8_t *data, size_t len) {
uint8_t crc = 0x00; // 初始值
const uint8_t poly = 0x31;
for (size_t i = 0; i < len; i++) {
crc ^= data[i];
for (uint8_t j = 0; j < 8; j++) {
crc = (crc << 1) ^ ((crc & 0x80) ? poly : 0);
}
}
return crc;
}
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);
com_fd_ = open(port_name.c_str(), O_RDWR | O_NOCTTY);
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)
{
static int idx=0;
idx+=1;
printf("[%03d]-->",idx);
for(auto d:data)
printf("%02x ",d);
printf("\n");
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();
static int idx=0;
idx+=1;
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 <<idx<< "接收数据错误" << std::endl;
return false;
}
else
{
// 没有数据，短暂休眠
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
}
return !data.empty();
}
int RS485Driver::bm_query_id(){
std::vector<uint8_t> command;
command.push_back(0);
command.push_back(0x80);
command.push_back(0);
command.push_back(0x65);
command.push_back(1);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
uint8_t crc8 = CRC8_Table(command.data(), 10);
//uint8_t crc8=crc8_maxim_direct(command.data(),10);
command.push_back(crc8);
bool ret = sendData(command);
if(ret){
std::vector<uint8_t> data;
bool ret=receiveData(data, 20, 1);
if(ret){
printf("recv:%02x,%02x,%02x,%02x\n",data[0],data[1],data[2],data[3]);
}else{
printf("recv error!\n");
}
}
return 0;
//00 32 01 00 01 00 01 00 01 00 7f
//00 33 01 00 01 00 01 00 01 00 3c
}
int RS485Driver::bm_set_mode(uint8_t motor_id, uint8_t mode){
std::vector<uint8_t> command;
command.push_back(0);
command.push_back(0x36);
command.push_back(motor_id);
command.push_back(0x1c);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(2);
command.push_back(0);
command.push_back(0);
//uint8_t crc8 = CRC8_Table(command.data(), 10);
//command.push_back(crc8);
bool ret = sendData(command);
if(ret){
std::vector<uint8_t> data;
data.clear();
ret=receiveData(data, 20, 1);
if(ret){
printf("<---:");
for(auto d:data)
printf("%02x ",d);
printf("\n");
}
}
return 0;
}
int RS485Driver::bm_set_pos(float angle1,float angle2){
std::vector<uint8_t> command;
//if(angle>360.0)
// angle=360.0;
int32_t pos=angle1/3.6;
uint8_t high1 = (pos >> 8) & 0xFF; // 高8位
uint8_t low1 = pos & 0xFF; // 低8位
pos=angle2/3.6;
uint8_t high2 = (pos >> 8) & 0xFF; // 高8位
uint8_t low2 = pos & 0xFF; // 低8位
//if(motor_id<5){
command.push_back(0x00);
command.push_back(0x32);
command.push_back(high1);
command.push_back(low1);
command.push_back(high2);
command.push_back(low2);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
//}
unsigned char crc8 = CRC8_Table(command.data(), 10);
command.push_back(crc8);
bool ret = sendData(command);
if(ret){
std::vector<uint8_t> data;
data.clear();
ret=receiveData(data, 20, 1);
if(ret){
printf("<---:");
for(auto d:data)
printf("%02x ",d);
printf("\n");
}
}
return 0;
}
int RS485Driver::bm_set_speed(float speed1,float speed2,float speed3,float speed4){
std::vector<uint8_t> command;
int32_t pos=speed1*10;
uint8_t high1 = (pos >> 8) & 0xFF; // 高8位
uint8_t low1 = pos & 0xFF; // 低8位
pos=speed2*10;
uint8_t high2 = (pos >> 8) & 0xFF; // 高8位
uint8_t low2 = pos & 0xFF; // 低8位
pos=speed3*10;
uint8_t high3 = (pos >> 8) & 0xFF; // 高8位
uint8_t low3 = pos & 0xFF; // 低8位
pos=speed4*10;
uint8_t high4 = (pos >> 8) & 0xFF; // 高8位
uint8_t low4 = pos & 0xFF; // 低8位
command.push_back(high1);
command.push_back(low1);
command.push_back(high2);
command.push_back(low2);
command.push_back(high3);
command.push_back(low3);
command.push_back(high4);
command.push_back(low4);
printf("set_speed.%.1f\n",speed1);
sendCan(0x32,command);
return 0;
}
int RS485Driver::bm_set_enable(uint8_t enable){
std::vector<uint8_t> command;
command.push_back(enable);
command.push_back(enable);
command.push_back(enable);
command.push_back(enable);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
printf("set_enable\n");
sendCan(0x38,command);
return 0;
}
int RS485Driver::bm_set_param(uint8_t motor_id, uint8_t param,uint32_t val){
std::vector<uint8_t> command;
command.push_back(motor_id);
command.push_back(param);
command.push_back((uint8_t)(val));
command.push_back((uint8_t)(val>>8));
command.push_back((uint8_t)(val>>16));
command.push_back((uint8_t)(val>>24));
command.push_back(0);
command.push_back(0);
printf("set_param:%d,%d,%d\n",motor_id,param,val);
sendCan(0x36,command);
return 0;
}
float RS485Driver::bm_get_pos(uint8_t motor_id){
std::vector<uint8_t> command;
command.push_back(0);
command.push_back(0x35);
command.push_back(motor_id);
command.push_back(0x0d);
command.push_back(4);
command.push_back(0x0b);
command.push_back(3);
command.push_back(0);
command.push_back(0);
command.push_back(0);
unsigned char crc8 = CRC8_Table(command.data(), 10);
command.push_back(crc8);
bool ret = sendData(command);
if(ret){
std::vector<uint8_t> data;
data.clear();
ret=receiveData(data, 20, 1);
if(ret){
printf("<---:");
for(auto d:data)
printf("%02x ",d);
printf("\n");
uint8_t nid=0x70+motor_id;
if(data[0]==0&&data[1]==nid){
//uint16_t pos = (data[2]<<8)+(data[3]&0xff);
//float angle=360.0*pos/32768.0;
int16_t pos=(data[6]<<8)+(data[7]&0xff);
float angle=3.60f*pos;
//printf("id:%d,pos:%d\n",motor_id,pos);
return angle;
///printf("pos:%d,angle:%.1f\n",pos,angle);
}
else{
}
//float angle = pos * 360.0 /255.0;
///float angle=0;
}
}
return -1;
}
int RS485Driver::bm_get_speed(){
std::vector<uint8_t> command;
command.push_back(1);
command.push_back(5);
command.push_back(11);
command.push_back(13);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
std::vector<uint8_t> rx=sendCan(0x35,command);
speedMap.clear();
for(int x=0;x<rx.size();x++)
if(rx[x]==0xaa&&rx[x+1]==0x11&&rx[x+2]==0x08){
uint8_t motor_id=rx[x+3]-0x70;
int16_t speed=(rx[x+7]<<8)+(rx[x+8]&0xff);
printf("id:%d,speed:%d\n",motor_id,speed);
speedMap[motor_id]=speed/10.0f;
}
return 0;
}
//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::lock_guard<std::mutex> lock(uart_mutex);
std::vector<uint8_t> packet;
packet.push_back(0x3e);
packet.push_back(motor_id);
packet.push_back(0x08);
int32_t pos = static_cast<int32_t>(angle * 100);
packet.push_back(0xa4);
packet.push_back(0);//方向
packet.push_back(0x10);
packet.push_back(0x00);//速度
packet.push_back((uint8_t)(pos));//位置,pos的低8位
packet.push_back((uint8_t)(pos>>8));//位置，pos的高8位
packet.push_back((uint8_t)(pos>>16));
packet.push_back((uint8_t)(pos>>24));
add_crc16(packet);
bool ret=sendData(packet);
if(!ret)
printf("send error\n");
return 0;
}
float RS485Driver::mt_get_pos(uint8_t motor_id){
std::lock_guard<std::mutex> lock(uart_mutex);
std::vector<uint8_t> packet;
packet.push_back(0x3e);
packet.push_back(motor_id);
packet.push_back(0x08);
//
packet.push_back(0x92);
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){
#if 1
std::vector<uint8_t> data;
data.clear();
ret=receiveData(data, 20, 1);
if(ret){
if(data[0]==0x3e&&data[1]==motor_id&&data[3]==0x92){
uint32_t pos = (data[10]<<24)+(data[9]<<16)+(data[8]<<8)+(data[7]&0xff);
///printf("pos:%d\n",pos);
float angle = pos /100.0;
return angle;
}
return 9999;
}
#else
static uint8_t com_buf[128];
memset(com_buf,0,128);
ret = read(com_fd_, com_buf, 128);
///bool ret=receiveData(data, 20, 1000);
printf("recv:");
for(int i=0;i<ret;i++)
printf("%02x ",com_buf[i]);
printf("\n");
#endif
}
return 9999;
}
int RS485Driver::mt_query_id(){
std::vector<uint8_t> packet;
packet.push_back(0x3e);
packet.push_back(0xcd);
packet.push_back(0x08);
//
packet.push_back(0x79);
packet.push_back(0);
packet.push_back(1);
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, 1);
if(ret){
uint8_t id= data[10];
return id;
}
}
return -1;
}
}