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

add svc to set param

Browse Source
This commit is contained in:
NuoDaJia02
2025-10-28 15:54:03 +08:00
parent 382be86637
commit 6681ebff6e
3 changed files with 24 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
motor_dev/msg/MotorCmd.msg
View File

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

5
motor_dev/msg/MotorPos.msg
View File

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

30
motor_dev/src/main.cpp
View File

@@ -2,6 +2,7 @@
#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_;
@@ -20,12 +21,12 @@ int main(int argc,char* argv[]){
rs485_driver_.bm_set_enable(2,2);
usleep(500000);
rs485_driver_.bm_set_param(1,77,10);
rs485_driver_.bm_set_param(1,78,10);
rs485_driver_.bm_set_param(1,79,10);
rs485_driver_.bm_set_param(1,78,2);
rs485_driver_.bm_set_param(1,79,2);
//rs485_driver_.bm_set_param(1,28,4);
rs485_driver_.bm_set_param(2,77,10);
rs485_driver_.bm_set_param(2,78,10);
rs485_driver_.bm_set_param(2,79,10);
rs485_driver_.bm_set_param(2,78,2);
rs485_driver_.bm_set_param(2,79,2);
//rs485_driver_.mt_set_pos(1,-5);
//init
/////rs485_driver_.bm_set_mode(1,3);//位置模式
@@ -91,8 +92,25 @@ int main(int argc,char* argv[]){
}
#endif
});
////rs485_driver_.closePort();
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){
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);
}
}
});
rclcpp::spin(node);
printf("now close rs485 port\n");
rs485_driver_.closePort();
rclcpp::shutdown();
}