add mt motor

2025-10-16 19:21:20 +08:00
parent 99d6f87a4b
commit 75150a4a49
4 changed files with 60 additions and 30 deletions
--- a/motor_dev/CMakeLists.txt
+++ b/motor_dev/CMakeLists.txt
@@ -10,7 +10,7 @@ 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")
 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)
--- a/motor_dev/include/motor_dev/rs485_driver.hpp
+++ b/motor_dev/include/motor_dev/rs485_driver.hpp
@@ -4,6 +4,7 @@
 #include <string>
 #include <vector>
 #include <termios.h>
+#include <mutex>

 namespace motor_dev
 {
@@ -40,6 +41,7 @@ namespace motor_dev
        int com_fd_;
        //bool is_open_;
        struct termios original_termios_;
+        std::mutex uart_mutex;
        //size_t max_motors_;

        //size_t control_index_;
--- a/motor_dev/src/main.cpp
+++ b/motor_dev/src/main.cpp
@@ -3,16 +3,18 @@
 #include"motor_dev/msg/motor_cmd.hpp"
 using namespace motor_dev;
 RS485Driver rs485_driver_;
+
 int main(int argc,char* argv[]){
    rclcpp::init(argc,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/ttyS1", 115200)){
+    if (!rs485_driver_.openPort("/dev/ttyUSB0", 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);
+    //rs485_driver_.mt_set_pos(1,3456);
+    //usleep(2000000);
+    //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){
@@ -33,14 +35,25 @@ int main(int argc,char* argv[]){
            }
        });
    //读取电机位置
-    rclcpp::TimerBase::SharedPtr timer_=node->create_wall_timer(std::chrono::milliseconds(500), [=]() { 
+    rclcpp::TimerBase::SharedPtr timer_=node->create_wall_timer(std::chrono::milliseconds(200), [=]() { 
        //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);
+        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;
    });
+    
    ////rs485_driver_.closePort();
    rclcpp::spin(node);
    rclcpp::shutdown();
--- a/motor_dev/src/rs485_driver.cpp
+++ b/motor_dev/src/rs485_driver.cpp
@@ -279,9 +279,11 @@ namespace motor_dev
        unsigned char crc8 = CRC8_Table(command.data(), 9);
        command.push_back(crc8);
        bool ret = sendData(command);
+        if(!ret)
+            printf("bm send err!\n");
        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);
@@ -327,6 +329,7 @@ namespace motor_dev

    //
    int RS485Driver::mt_set_pos(uint8_t motor_id, float angle){
+        std::lock_guard<std::mutex> lock(uart_mutex);
        std::vector<uint8_t> packet;
        packet.push_back(0x3e);
        packet.push_back(motor_id);
@@ -335,25 +338,31 @@ namespace motor_dev
        int32_t pos = static_cast<int32_t>(angle * 100);
        packet.push_back(0xa4);
        packet.push_back(0);//方向
-        packet.push_back(0x01);
+        packet.push_back(0x10);
        packet.push_back(0x00);//速度
        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);
+        // printf("send:");
+        // for(auto d:packet)
+        //      printf("%02x ",d);
+        // std::cout<<std::endl;
        bool ret=sendData(packet);
-        if(ret)
-            printf("send ok\n");
+        if(!ret)
+            printf("send error\n");
        return 0;
    }
    float RS485Driver::mt_get_pos(uint8_t motor_id){
+        std::lock_guard<std::mutex> lock(uart_mutex);
+
        std::vector<uint8_t> packet;
        packet.push_back(0x3e);
        packet.push_back(motor_id);
        packet.push_back(0x08);
        //
-        packet.push_back(0x94);
+        packet.push_back(0x92);
        packet.push_back(0);
        packet.push_back(0);
        packet.push_back(0);
@@ -362,24 +371,31 @@ 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;
+        // 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;
-            ret=receiveData(data, 20, 100);
-            printf("xrecv:");
-            for(int i=0;i<data.size();i++)
-                printf("%02x ",data[i]);
-            printf("\n");
+            data.clear();
+            ret=receiveData(data, 20, 10);
+            // printf("recv:");
+            // for(auto d:data)
+            //     printf("%02x ",d);
+            // printf("\n");
            if(ret){
-                uint32_t pos = (data[7]<<8)+(data[6]&0xff);
-                float angle = pos /100.0;
-                return angle;
+                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);
+                    ///printf("pos:%d\n",pos);
+                    float angle = pos /100.0;
+                    return angle;
+                }
+                return 9999;
            }
            #else
+            static uint8_t com_buf[128];
            memset(com_buf,0,128);
            ret = read(com_fd_, com_buf, 128);
            ///bool ret=receiveData(data, 20, 1000);
@@ -389,7 +405,7 @@ namespace motor_dev
            printf("\n");
            #endif
        }
-        return -1;
+        return 9999;
    }
    int RS485Driver::mt_query_id(){
        std::vector<uint8_t> packet;
@@ -406,17 +422,16 @@ 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;
+        // 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);
            if(ret){
-                uint32_t pos = (data[7]<<8)+(data[6]&0xff);
-                float angle = pos /100.0;
-                return angle;
+                uint8_t id= data[10];
+                return id;
            }
        }
        return -1;