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

tmp

Browse Source
This commit is contained in:
NuoDaJia02
2025-10-16 17:38:20 +08:00
parent 1a370a796d
commit 99d6f87a4b
3 changed files with 117 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
motor_dev/include/motor_dev/rs485_driver.hpp
View File

@@ -28,7 +28,8 @@ namespace motor_dev
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);
int bm_query_id();
int mt_query_id();
//脉塔
void add_crc16(std::vector<uint8_t> &data);

19
motor_dev/src/main.cpp
View File

@@ -7,11 +7,14 @@ 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)){
if (!rs485_driver_.openPort("/dev/ttyS1", 115200)){
std::cout << "Failed to open RS485 port." << std::endl;
}
rs485_driver_.mt_set_pos(1,5);
usleep(2000000);
rs485_driver_.mt_set_pos(1,-5);
//init
rs485_driver_.bm_set_mode(1,3);//位置模式
/////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") {
@@ -29,8 +32,16 @@ int main(int argc,char* argv[]){
//TODO
}
});
rs485_driver_.closePort();
//读取电机位置
rclcpp::TimerBase::SharedPtr timer_=node->create_wall_timer(std::chrono::milliseconds(500), [=]() {
//rs485_driver_.bm_set_pos(id);
//rs485_driver_.mt_get_pos(id);
///rs485_driver_.bm_query_id();
//rs485_driver_.mt_query_id();//3e cd 08 79 00 01 00 00 00 00 00 f8 95
float angle=rs485_driver_.mt_get_pos(1);
printf("angle:%.1f\n",angle);
});
////rs485_driver_.closePort();
rclcpp::spin(node);
rclcpp::shutdown();
}

116
motor_dev/src/rs485_driver.cpp
View File

@@ -77,7 +77,8 @@ namespace motor_dev
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 | O_NDELAY);
com_fd_ = open(port_name.c_str(), O_RDWR | O_NOCTTY);
if (com_fd_ == -1)
{
std::cerr << "无法打开串口: " << port_name << std::endl;
@@ -175,7 +176,8 @@ namespace motor_dev
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)
{
// 检查超时
@@ -198,7 +200,7 @@ namespace motor_dev
}
else if (bytes_read < 0)
{
std::cerr << "接收数据错误" << std::endl;
std::cerr <<idx<< "接收数据错误" << std::endl;
return false;
}
else
@@ -210,6 +212,38 @@ namespace motor_dev
return !data.empty();
}
int RS485Driver::bm_query_id(){
std::vector<uint8_t> command;
command.push_back(0);
command.push_back(0x36);
command.push_back(1);
command.push_back(0x1c);
command.push_back(4);
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);
for(auto d:command)
printf("%02x ",d);
std::cout<<std::endl;
bool ret = sendData(command);
if(ret){
std::vector<uint8_t> data;
bool ret=receiveData(data, 20, 1000);
if(ret){
printf("recv:%02x,%02x,%02x,%02x\n",data[0],data[1],data[2],data[3]);
}
}
return 0;
}
int RS485Driver::bm_set_mode(uint8_t motor_id, uint8_t mode){
std::vector<uint8_t> command;
if(mode>3)
@@ -247,6 +281,7 @@ namespace motor_dev
bool ret = sendData(command);
return 0;
}
static uint8_t com_buf[128];
float RS485Driver::bm_get_pos(uint8_t motor_id){
std::vector<uint8_t> command;
command.push_back(motor_id);
@@ -262,10 +297,11 @@ namespace motor_dev
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;
//uint8_t pos = data[7];
//float angle = pos * 360.0 /255.0;
float angle=0;
return angle;
}
}
@@ -295,19 +331,20 @@ namespace motor_dev
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);//方向
int32_t pos = static_cast<int32_t>(angle * 100);
packet.push_back(0xa4);
packet.push_back(0);//方向
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);
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 ok\n");
return 0;
}
float RS485Driver::mt_get_pos(uint8_t motor_id){
@@ -325,6 +362,53 @@ namespace motor_dev
packet.push_back(0);
packet.push_back(0);
add_crc16(packet);
for(auto d:packet)
printf("%02x ",d);
std::cout<<std::endl;
bool ret=sendData(packet);
if(ret){
#if 1
std::vector<uint8_t> data;
ret=receiveData(data, 20, 100);
printf("xrecv:");
for(int i=0;i<data.size();i++)
printf("%02x ",data[i]);
printf("\n");
if(ret){
uint32_t pos = (data[7]<<8)+(data[6]&0xff);
float angle = pos /100.0;
return angle;
}
#else
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 -1;
}
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);
for(auto d:packet)
printf("%02x ",d);
std::cout<<std::endl;
bool ret=sendData(packet);
if(ret){
std::vector<uint8_t> data;