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

motor pub pos

Browse Source
This commit is contained in:
NuoDaJia02
2025-10-21 15:22:01 +08:00
parent eda6722b1d
commit 295d77701b
3 changed files with 48 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
motor_dev/CMakeLists.txt
View File

@@ -10,8 +10,8 @@ find_package(ament_cmake REQUIRED)
find_package(std_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME} "msg/MotorCmd.msg")
# rosidl_generate_interfaces(${PROJECT_NAME} "msg/MotorCmd.msg" "msg/MotorPos.msg")
#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)

65
motor_dev/src/main.cpp
View File

@@ -1,6 +1,7 @@
#include"rclcpp/rclcpp.hpp"
#include"motor_dev/rs485_driver.hpp"
#include"motor_dev/msg/motor_cmd.hpp"
#include"motor_dev/msg/motor_pos.hpp"
using namespace motor_dev;
RS485Driver rs485_driver_;
@@ -12,27 +13,26 @@ int main(int argc,char* argv[]){
if (!rs485_driver_.openPort("/dev/ttyS1", 115200)){
std::cout << "Failed to open RS485 port." << std::endl;
}
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") {
if(msg->type=="bm"){
size_t n = msg->motor_id.size();
if(n<2){
printf("bm msg need two angle\n");
return;
}
float angle1 =msg->motor_angle[0];
float angle2 =msg->motor_angle[1];
std::cout<<"########:"<<msg->type<<","<<angle1<<","<<angle2<<std::endl;
printf("##############:%s,%.1f,%.1f\n",msg->type.c_str(),angle1,angle2);
rs485_driver_.bm_set_pos(angle1,angle2);
}else if(msg->type=="mt"){
uint8_t id=msg->motor_id[0];
@@ -43,25 +43,44 @@ int main(int argc,char* argv[]){
//TODO
}
});
auto motor_pub=node->create_publisher<motor_dev::msg::MotorPos>("/motor_pos",10);
//读取电机位置
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);
static int pub_cnt=0;
motor_dev::msg::MotorPos motor_pos;
float angle1=rs485_driver_.bm_get_pos(1);
float angle2=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);
// MotorPos motor_pos;
// input_msg->source = cfg.getSource();
// input_msg->type = cfg.getType();
// input_msg->position=cfg.getPosition();
// input_msg->code=key;
// input_msg->value=val;
// input_pub->publish(*input_msg);
// pub_cnt+=1;
// std::cout<<"pub msg["<<pub_cnt<<"]:"<<key<<std::endl;
motor_pos.motor_id.clear();
motor_pos.motor_angle.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_angle.push_back(angle1);
motor_pos.motor_angle.push_back(angle2);
motor_pub->publish(motor_pos);
}
pub_cnt+=1;
#if 1
motor_pos.motor_id.clear();
motor_pos.motor_angle.clear();
float angle=rs485_driver_.mt_get_pos(1);
if(angle!=9999){
motor_pos.type = "mt";
motor_pos.motor_id.push_back(1);
motor_pos.motor_angle.push_back(angle);
motor_pub->publish(motor_pos);
std::cout<<"pub msg["<<pub_cnt<<"]:"<<angle<<std::endl;
}
#endif
});
////rs485_driver_.closePort();

5
motor_dev/src/rs485_driver.cpp
View File

@@ -376,7 +376,7 @@ namespace motor_dev
if(ret){
std::vector<uint8_t> data;
data.clear();
ret=receiveData(data, 20, 20);
ret=receiveData(data, 20, 50);
if(ret){
printf("<---:");
for(auto d:data)
@@ -388,6 +388,9 @@ namespace motor_dev
float angle=360.0*pos/32768.0;
return angle;
///printf("pos:%d,angle:%.1f\n",pos,angle);
}
else{
}
//float angle = pos * 360.0 /255.0;
///float angle=0;