From 78c88943351d4120ec167e9d28d5170ccfd848cc Mon Sep 17 00:00:00 2001 From: hehe Date: Tue, 25 Nov 2025 18:14:00 +0800 Subject: [PATCH] can speed mode --- motor_dev/include/motor_dev/rs485_driver.hpp | 11 +- motor_dev/src/main.cpp | 108 ++++++------------- motor_dev/src/rs485_driver.cpp | 6 +- 3 files changed, 41 insertions(+), 84 deletions(-) diff --git a/motor_dev/include/motor_dev/rs485_driver.hpp b/motor_dev/include/motor_dev/rs485_driver.hpp index 72ad696..b531434 100644 --- a/motor_dev/include/motor_dev/rs485_driver.hpp +++ b/motor_dev/include/motor_dev/rs485_driver.hpp @@ -5,7 +5,7 @@ #include #include #include - +#include namespace motor_dev { class RS485Driver @@ -16,7 +16,7 @@ namespace motor_dev // 打开串口 bool openPort(const std::string &port_name, int baud_rate = 115200); - + std::map speedMap; // 关闭串口 void closePort(); @@ -25,13 +25,14 @@ namespace motor_dev // 接收数据 bool receiveData(std::vector &data, size_t max_length, int timeout_ms = 100); + std::vector sendCan(uint8_t cmd,const std::vector data); //本末 - int bm_set_enable(uint8_t motor_id,uint8_t enable); + 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); + int bm_set_speed(float speed1, float speed2,float speed3,float speed4); float bm_get_pos(uint8_t motor_id); - float bm_get_speed(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); diff --git a/motor_dev/src/main.cpp b/motor_dev/src/main.cpp index 68a166b..fcca0e3 100644 --- a/motor_dev/src/main.cpp +++ b/motor_dev/src/main.cpp @@ -15,42 +15,23 @@ int main(int argc,char* argv[]){ 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,2); - rs485_driver_.bm_set_param(2,28,2); - rs485_driver_.bm_set_param(3,28,2); - rs485_driver_.bm_set_param(4,28,2); + + + + 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,1); - rs485_driver_.bm_set_param(2,84,1); - rs485_driver_.bm_set_param(3,84,1); - rs485_driver_.bm_set_param(4,84,1); - usleep(1000000); - rs485_driver_.bm_set_speed(8.0f,10,10,10); - int idx=0; - while(idx<20){ - rs485_driver_.bm_get_speed(); - usleep(100000); - idx+=1; - } - ///usleep(20000000); - rs485_driver_.bm_set_speed(0,0,0,0); - idx=0; - while(idx<10){ - rs485_driver_.bm_get_speed(); - usleep(100000); - idx+=1; - } - //rs485_driver_.bm_set_enable(2,1); - //rs485_driver_.bm_set_enable(3,1); - //rs485_driver_.bm_set_enable(4,1); - return 0; + 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); - rs485_driver_.bm_set_param(1,84,10); - rs485_driver_.bm_set_param(2,84,10); - //init /////rs485_driver_.bm_set_mode(1,3);//位置模式 auto rs485_sub_=node->create_subscription("/motor_cmd",10,[](const interfaces::msg::MotorCmd::SharedPtr msg){ @@ -59,17 +40,15 @@ int main(int argc,char* argv[]){ if(msg->type=="bm"){ size_t n = msg->motor_id.size(); if(n<2){ - printf("bm msg need two angle\n"); + printf("bm msg need four angle\n"); return; } - //float angle1 =msg->motor_angle[0]; - //float angle2 =msg->motor_angle[1]; - float speed1=msg->motor_speed[0]; float speed2=msg->motor_speed[1]; - printf("##############:%s,speed:%.1f,%.1f\n",msg->type.c_str(),speed1,speed2); - //rs485_driver_.bm_set_pos(angle1,angle2); - rs485_driver_.bm_set_speed(speed1,speed2,0,0); + 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]; @@ -82,58 +61,33 @@ int main(int argc,char* argv[]){ rclcpp::TimerBase::SharedPtr timer_=node->create_wall_timer(std::chrono::milliseconds(5000), [=]() { static int pub_cnt=0; interfaces::msg::MotorPos motor_pos; - //float angle1=rs485_driver_.bm_get_pos(1); - //float angle2=rs485_driver_.bm_get_pos(2); - #if 0 - float speed1=rs485_driver_.bm_get_speed(1); - float speed2=rs485_driver_.bm_get_speed(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 + rs485_driver_.bm_get_speed(); + motor_pos.motor_id.clear(); motor_pos.motor_angle.clear(); motor_pos.motor_speed.clear(); - ////printf("pub msg[%03d],%.1f,%.1f\n",pub_cnt,angle1,angle2); - //if(angle1>-1&&angle2>-1) { motor_pos.source = "rs485"; motor_pos.type = "bm"; motor_pos.position="none"; - motor_pos.motor_id.push_back(1); - motor_pos.motor_id.push_back(2); - motor_pos.motor_speed.push_back(speed1); - motor_pos.motor_speed.push_back(speed2); + 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); - //std::cout<<"pub msg["<publish(motor_pos); - } - #endif }); auto motor_param_srv=node->create_service("motor_param",[=](const std::shared_ptr req,std::shared_ptr res){ - uint8_t mid=req->motor_id; - res->ret=-1; - printf("recv sv:%d,%d,%d,%d\n",req->motor_id,req->max_speed,req->add_acc,req->dec_acc); - if(mid>-1&&mid<500){ - if(req->max_speed<500&&req->add_acc<500&&req->dec_acc<500){ - rs485_driver_.bm_set_param(mid,77,req->max_speed); - rs485_driver_.bm_set_param(mid,78,req->add_acc); - rs485_driver_.bm_set_param(mid,79,req->dec_acc); - res->ret=0; - printf("svc:%d\n",res->ret); - } + 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); } }); diff --git a/motor_dev/src/rs485_driver.cpp b/motor_dev/src/rs485_driver.cpp index 9433f12..0d3a46f 100644 --- a/motor_dev/src/rs485_driver.cpp +++ b/motor_dev/src/rs485_driver.cpp @@ -44,7 +44,7 @@ namespace motor_dev std::vector rx_data; if(ret){ rx_data.clear(); - ret=receiveData(rx_data, 40, 1); + ret=receiveData(rx_data, 100, 1); if(ret){ printf("<---:"); for(auto d:rx_data) @@ -398,7 +398,7 @@ namespace motor_dev command.push_back(low3); command.push_back(high4); command.push_back(low4); - printf("set_speed\n"); + printf("set_speed.%.1f\n",speed1); sendCan(0x32,command); return 0; } @@ -486,11 +486,13 @@ namespace motor_dev command.push_back(0); command.push_back(0); std::vector rx=sendCan(0x35,command); + speedMap.clear(); for(int x=0;x