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

ben mo ok

Browse Source
This commit is contained in:
NuoDaJia02
2025-10-20 20:07:29 +08:00
parent 75150a4a49
commit efd49b99cd
4 changed files with 175 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -26,13 +26,14 @@ namespace motor_dev
// 接收数据
bool receiveData(std::vector<uint8_t> &data, size_t max_length, int timeout_ms = 100);
//本末
int bm_set_enable(uint8_t motor_id,uint8_t enable);
int bm_set_mode(uint8_t motor_id, uint8_t mode);
int bm_set_pos(uint8_t motor_id, float angle);
int bm_set_pos(float angle1, float angle2);
float bm_get_pos(uint8_t motor_id);
int bm_query_id();
int mt_query_id();
//脉塔
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);

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

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

48
motor_dev/src/main.cpp
View File

@@ -9,40 +9,50 @@ int main(int argc,char* 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/ttyUSB0", 115200)){
if (!rs485_driver_.openPort("/dev/ttyS1", 115200)){
std::cout << "Failed to open RS485 port." << std::endl;
}
//rs485_driver_.mt_set_pos(1,3456);
//usleep(2000000);
usleep(100000);
rs485_driver_.bm_set_mode(1,4);
//usleep(10000);
rs485_driver_.bm_set_enable(1,2);
//usleep(10000);
rs485_driver_.bm_set_mode(2,4);
//usleep(10000);
rs485_driver_.bm_set_enable(2,2);
//usleep(10000);
//rs485_driver_.mt_set_pos(1,-5);
//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);
}
if(msg->type=="bm"){
float angle1 =msg->motor_angle[0];
float angle2 =msg->motor_angle[1];
std::cout<<"########:"<<msg->type<<","<<angle1<<","<<angle2<<std::endl;
rs485_driver_.bm_set_pos(angle1,angle2);
}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);
}
}else if(msg->target=="ethercat"){
//TODO
}
});
//读取电机位置
rclcpp::TimerBase::SharedPtr timer_=node->create_wall_timer(std::chrono::milliseconds(200), [=]() {
//rs485_driver_.bm_set_pos(id);
//rs485_driver_.mt_get_pos(id);
rclcpp::TimerBase::SharedPtr timer_=node->create_wall_timer(std::chrono::milliseconds(2000), [=]() {
//rs485_driver_.bm_set_pos(1);
float angle=rs485_driver_.bm_get_pos(2);
printf("bm cur angle:%.1f\n",angle);
///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);
if(angle!=9999)
printf("angle:%.1f\n",angle);
//float angle=rs485_driver_.mt_get_pos(1);
//if(angle!=9999)
// printf("angle:%.1f\n",angle);
// MotorPos motor_pos;
// input_msg->source = cfg.getSource();
// input_msg->type = cfg.getType();
@@ -57,4 +67,4 @@ int main(int argc,char* argv[]){
////rs485_driver_.closePort();
rclcpp::spin(node);
rclcpp::shutdown();
}
}

202
motor_dev/src/rs485_driver.cpp
View File

@@ -46,6 +46,18 @@ namespace motor_dev
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()
{
@@ -160,13 +172,18 @@ namespace motor_dev
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;
}
@@ -216,95 +233,165 @@ namespace motor_dev
int RS485Driver::bm_query_id(){
std::vector<uint8_t> command;
command.push_back(0);
command.push_back(0x36);
command.push_back(0x80);
command.push_back(0);
command.push_back(0x65);
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);
uint8_t crc8 = CRC8_Table(command.data(), 10);
//uint8_t crc8=crc8_maxim_direct(command.data(),10);
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);
bool ret=receiveData(data, 20, 20);
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;
if(mode>3)
mode=3;
command.push_back(0);
command.push_back(0x36);
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(0x1c);
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(0);
command.push_back(0);
uint8_t crc8 = CRC8_Table(command.data(), 10);
command.push_back(crc8);
bool ret = sendData(command);
if(!ret)
printf("bm send err!\n");
if(ret){
std::vector<uint8_t> data;
data.clear();
ret=receiveData(data, 20, 20);
if(ret){
printf("<---:");
for(auto d:data)
printf("%02x ",d);
printf("\n");
}
}
return 0;
}
int RS485Driver::bm_set_enable(uint8_t motor_id,uint8_t enable){
std::vector<uint8_t> command;
command.push_back(0);
command.push_back(0x38);
command.push_back(motor_id);
command.push_back(enable);
command.push_back(0);
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);
command.push_back(crc8);
bool ret = sendData(command);
if(ret){
std::vector<uint8_t> data;
data.clear();
ret=receiveData(data, 20, 20);
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, 20);
if(ret){
printf("<---:");
for(auto d:data)
printf("%02x ",d);
printf("\n");
}
}
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(0x74);
command.push_back(0x0d);
command.push_back(1);
command.push_back(2);
command.push_back(3);
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);
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, 20);
if(ret){
//uint8_t pos = data[7];
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;
return angle;
///printf("pos:%d,angle:%.1f\n",pos,angle);
}
//float angle = pos * 360.0 /255.0;
float angle=0;
return angle;
///float angle=0;
}
}
return -1;
@@ -345,10 +432,6 @@ namespace motor_dev
packet.push_back((uint8_t)(pos>>16));
packet.push_back((uint8_t)(pos>>24));
add_crc16(packet);
// printf("send:");
// for(auto d:packet)
// printf("%02x ",d);
// std::cout<<std::endl;
bool ret=sendData(packet);
if(!ret)
printf("send error\n");
@@ -371,20 +454,12 @@ namespace motor_dev
packet.push_back(0);
packet.push_back(0);
add_crc16(packet);
// printf("send:");
// for(auto d:packet)
// printf("%02x ",d);
// std::cout<<std::endl;
bool ret=sendData(packet);
if(ret){
#if 1
std::vector<uint8_t> data;
data.clear();
ret=receiveData(data, 20, 10);
// printf("recv:");
// for(auto d:data)
// printf("%02x ",d);
// printf("\n");
ret=receiveData(data, 20, 20);
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);
@@ -422,13 +497,10 @@ 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){
std::vector<uint8_t> data;
ret=receiveData(data, 20, 1000);
ret=receiveData(data, 20, 20);
if(ret){
uint8_t id= data[10];
return id;