From f0ac5474fff19d806102e0e18a39c6b79531a499 Mon Sep 17 00:00:00 2001
From: hehe <you@example.com>
Date: Tue, 6 Jan 2026 17:05:24 +0800
Subject: [PATCH] motor dev support speed & pos

---
 motor_dev/include/motor_dev/rs485_driver.hpp |  11 +-
 motor_dev/src/main.cpp                       | 182 ++++++++++---
 motor_dev/src/rs485_driver.cpp               | 263 +++++++++++--------
 motor_dev_can/src/main.cpp                   |   2 +-
 4 files changed, 294 insertions(+), 164 deletions(-)

diff --git a/motor_dev/include/motor_dev/rs485_driver.hpp b/motor_dev/include/motor_dev/rs485_driver.hpp
index b531434..72ad696 100644
--- a/motor_dev/include/motor_dev/rs485_driver.hpp
+++ b/motor_dev/include/motor_dev/rs485_driver.hpp
@@ -5,7 +5,7 @@
 #include <vector>
 #include <termios.h>
 #include <mutex>
-#include<map>
+
 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<uint8_t, float> speedMap;
+        
         // 关闭串口
         void closePort();
         
@@ -25,14 +25,13 @@ namespace motor_dev
         
         // 接收数据
         bool receiveData(std::vector<uint8_t> &data, size_t max_length, int timeout_ms = 100);
-        std::vector<uint8_t> sendCan(uint8_t cmd,const std::vector<uint8_t> data);
         //本末
-        int bm_set_enable(uint8_t enable);
+        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(float angle1, float angle2);
-        int bm_set_speed(float speed1, float speed2,float speed3,float speed4);
+        int bm_set_speed(float speed1, float speed2);
         float bm_get_pos(uint8_t motor_id);
-        int bm_get_speed();
+        float bm_get_speed(uint8_t motor_id);
         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 fcca0e3..3470655 100644
--- a/motor_dev/src/main.cpp
+++ b/motor_dev/src/main.cpp
@@ -3,95 +3,195 @@
 #include"interfaces/msg/motor_cmd.hpp"
 #include"interfaces/msg/motor_pos.hpp"
 #include"interfaces/srv/motor_param.hpp"
+#include"interfaces/srv/motor_info.hpp"
+
+#include <unistd.h>
+
 using namespace motor_dev;
 RS485Driver rs485_driver_;
+#define  MOTOR_SPEED
+static auto g_program_start_time = std::chrono::steady_clock::now();
+
+// 带时间戳的 printf 风格日志函数
+void log_printf(const char* format, ...) {
+    auto now = std::chrono::steady_clock::now();
+    auto elapsed = std::chrono::duration_cast<std::chrono::duration<double>>(now - g_program_start_time);
+    double seconds = elapsed.count();
+
+    char buffer[1024];
+    va_list args;
+    va_start(args, format);
+    int n = vsnprintf(buffer, sizeof(buffer), format, args);
+    va_end(args);
+
+    if (n < 0) {
+        std::fprintf(stderr, "[%.3f] (log error)\n", seconds);
+        return;
+    }
+
+    std::fprintf(stdout, "[%.3f] %s\n", seconds, buffer);
+    std::fflush(stdout); // 确保立即输出（对调试很重要）
+}
+
+
 
 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/ttyACM0", 921600)){
+    if (!rs485_driver_.openPort("/dev/ttyS1", 115200)){
         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,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,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);
-
-
+    usleep(500000);
+    rs485_driver_.bm_set_enable(1,1);
+    rs485_driver_.bm_set_enable(2,1);
+#ifdef MOTOR_SPEED
+    rs485_driver_.bm_set_mode(1,3);
+#else
+    rs485_driver_.bm_set_mode(1,4);
+#endif
+    rs485_driver_.bm_set_enable(1,2);
+#ifdef MOTOR_SPEED
+    rs485_driver_.bm_set_mode(2,3);
+#else
+    rs485_driver_.bm_set_mode(2,4);
+#endif
+    rs485_driver_.bm_set_enable(2,2);
+#ifdef MOTOR_SPEED
+    rs485_driver_.bm_set_param(1,84,10);//速度环加速度
+    rs485_driver_.bm_set_param(2,84,10);//速度环加速度
+#else
+    rs485_driver_.bm_set_param(1,77,40);
+    rs485_driver_.bm_set_param(1,78,10);//位置加速度
+    rs485_driver_.bm_set_param(1,79,10);//位置减速度
+    //rs485_driver_.bm_set_param(1,28,4);
+    rs485_driver_.bm_set_param(2,77,40);
+    rs485_driver_.bm_set_param(2,78,10);
+    rs485_driver_.bm_set_param(2,79,10);
+#endif
+    //rs485_driver_.mt_set_pos(1,-5);
     //init
     /////rs485_driver_.bm_set_mode(1,3);//位置模式
     auto rs485_sub_=node->create_subscription<interfaces::msg::MotorCmd>("/motor_cmd",10,[](const interfaces::msg::MotorCmd::SharedPtr msg){
             ///
+     ///       printf("##############-----------------------");
             if (msg->target == "rs485") {
                 if(msg->type=="bm"){
                     size_t n = msg->motor_id.size();
                     if(n<2){
-                        printf("bm msg need four angle\n");
+                        printf("bm msg need two angle\n");
                         return;
                     }
+                    #ifdef MOTOR_SPEED
                     float speed1=msg->motor_speed[0];
                     float speed2=msg->motor_speed[1];
-                    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);
+                    log_printf("recv topic:%s,speed:%.1f,%.1f",msg->type.c_str(),speed1,speed2);
+                    rs485_driver_.bm_set_speed(speed1,speed2);
+                    #else
+                    float angle1    =msg->motor_angle[0];
+                    float angle2    =msg->motor_angle[1];
+                    log_printf("recv topic:%s,angle:%.1f,%.1f",msg->type.c_str(),angle1,angle2);
+                    rs485_driver_.bm_set_pos(angle1,angle2);
+                    #endif
+                    
                 }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
             }
         });
+#if 1
     auto motor_pub=node->create_publisher<interfaces::msg::MotorPos>("/motor_pos",10);
     //读取电机位置
-    rclcpp::TimerBase::SharedPtr timer_=node->create_wall_timer(std::chrono::milliseconds(5000), [=]() { 
+    rclcpp::TimerBase::SharedPtr timer_=node->create_wall_timer(std::chrono::milliseconds(2000), [=]() { 
         static int pub_cnt=0;
+	log_printf("query motor pos");
         interfaces::msg::MotorPos motor_pos;
-        rs485_driver_.bm_get_speed();
- 
+        
+
+        
+        ///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 
         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";
-            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_pos.motor_id.push_back(1);
+            motor_pos.motor_id.push_back(2);
+            #ifdef MOTOR_SPEED
+            float speed1=rs485_driver_.bm_get_speed(1);
+            float speed2=rs485_driver_.bm_get_speed(2);
+            motor_pos.motor_speed.push_back(speed1);
+            motor_pos.motor_speed.push_back(speed2);
+            log_printf("pub speed:%.1f,%.1f",speed1,speed2);
+            #else
+            float angle1=rs485_driver_.bm_get_pos(1);
+            float angle2=rs485_driver_.bm_get_pos(2);
+            motor_pos.motor_angle.push_back(angle1);
+            motor_pos.motor_angle.push_back(angle2);
+            log_printf("pub pos:%.1f,%.1f",angle1,angle2);
+            #endif
             motor_pub->publish(motor_pos);
-            
         }
         pub_cnt+=1;
+        #if 0
+        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);
+        }
+        #endif
     });
+#endif
     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){
-            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);
+            uint8_t mid=req->motor_id;
+            res->ret=-1;
+            log_printf("recv sv:%d,%d,%d,%d",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){
+                    #ifdef MOTOR_SPEED
+                    rs485_driver_.bm_set_param(mid,84,req->add_acc);//速度环加速度
+                    #else
+                    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);
+                    #endif
+                    res->ret=0;
+                    printf("svc:%d\n",res->ret);
+                }     
             }
 		});
+#if 0
+auto motor_info_srv=node->create_service<interfaces::srv::MotorInfo>("motor_info",[=](const std::shared_ptr<interfaces::srv::MotorInfo::Request> req,std::shared_ptr<interfaces::srv::MotorInfo::Response> res){
+            printf("recv motor_info:%s,%d\n",req->type.c_str(),req->motor_ids[0]);
+	    res->ret=false;
+	    if(req->type=="bm"){
+	    	  res->motor_angles.clear();
+	          for(auto d:req->motor_ids){
+                      float angle=rs485_driver_.bm_get_pos(d);
+	      	      res->motor_angles.push_back(angle);
+                      printf("motor_info %d, angle:%.1f\n",d,angle);
+		  }
+                  res->ret=true;
+	    }
+     });
 
-
+#endif
 
     rclcpp::spin(node);
     printf("now close rs485 port\n");
diff --git a/motor_dev/src/rs485_driver.cpp b/motor_dev/src/rs485_driver.cpp
index 0d3a46f..78ddd6e 100644
--- a/motor_dev/src/rs485_driver.cpp
+++ b/motor_dev/src/rs485_driver.cpp
@@ -10,52 +10,6 @@
 namespace motor_dev
 {
 
-    std::vector<uint8_t> RS485Driver::sendCan(uint8_t cmd,const std::vector<uint8_t> data) {
-        std::vector<uint8_t> packet;
-        packet.push_back(0x55);  // 同步字节1
-        packet.push_back(0xAA);  // 同步字节2
-        packet.push_back(0x1E);  // 30字节
-        
-        packet.push_back(0x01);
-        packet.push_back(0x01);
-        packet.push_back(0x00);
-        packet.push_back(0x00);
-        packet.push_back(0x00);
-        packet.push_back(0x0a);
-        packet.push_back(0x00);
-        packet.push_back(0x00);
-        packet.push_back(0x00);
-        packet.push_back(0x00);
-
-        packet.push_back(cmd);
-        packet.push_back(0x00);
-        packet.push_back(0x00);
-        packet.push_back(0x00);
-        packet.push_back(0x00);
-        packet.push_back(0x08);
-        packet.push_back(0x00);
-        packet.push_back(0x00);
-        
-        for (uint8_t i = 0; i < 8; i++) {
-            packet.push_back(data[i]); 
-        }
-        packet.push_back(0x00);
-        bool ret = sendData(packet);
-        std::vector<uint8_t> rx_data;
-        if(ret){
-            rx_data.clear();
-            ret=receiveData(rx_data, 100, 1);
-            if(ret){
-                printf("<---:");
-                for(auto d:rx_data)
-                    printf("%02x ",d);
-                printf("\n");
-                return rx_data;
-            }
-        }
-        return rx_data;
-    }
-
     const unsigned char CRC8Table[]={  
         0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, 65,  
         157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96, 130, 220,  
@@ -220,10 +174,12 @@ namespace motor_dev
     {
         static int idx=0;
         idx+=1;
+#ifdef hehe
         printf("[%03d]-->",idx);
         for(auto d:data)
             printf("%02x ",d);
         printf("\n");
+#endif
         ssize_t bytes_written = write(com_fd_, data.data(), data.size());
         if (bytes_written != static_cast<ssize_t>(data.size()))
         {
@@ -294,7 +250,7 @@ namespace motor_dev
         bool ret = sendData(command);
         if(ret){
             std::vector<uint8_t> data;
-            bool ret=receiveData(data, 20, 1);
+            bool ret=receiveData(data, 20, 20);
             if(ret){
                 printf("recv:%02x,%02x,%02x,%02x\n",data[0],data[1],data[2],data[3]);
             }else{
@@ -313,19 +269,19 @@ namespace motor_dev
         command.push_back(0x36);
         command.push_back(motor_id);
         command.push_back(0x1c);
+        command.push_back(mode);
         command.push_back(0);
         command.push_back(0);
         command.push_back(0);
-        command.push_back(2);
         command.push_back(0);
         command.push_back(0);
-        //uint8_t crc8 = CRC8_Table(command.data(), 10);
-        //command.push_back(crc8);
+        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, 1);
+            ret=receiveData(data, 20, 20);
             if(ret){
                 printf("<---:");
                 for(auto d:data)
@@ -337,6 +293,65 @@ namespace motor_dev
     }
     
     
+     int RS485Driver::bm_set_param(uint8_t motor_id, uint8_t param,uint32_t val){
+        std::vector<uint8_t> packet;
+        packet.push_back(0);
+        packet.push_back(0x36);
+        packet.push_back(motor_id);
+        packet.push_back(param);
+        
+        packet.push_back((uint8_t)(val));
+        packet.push_back((uint8_t)(val>>8));
+        packet.push_back((uint8_t)(val>>16));
+        packet.push_back((uint8_t)(val>>24));
+        packet.push_back(0);
+        packet.push_back(0);
+        uint8_t crc8 = CRC8_Table(packet.data(), 10);
+        packet.push_back(crc8);
+        bool ret = sendData(packet);
+        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)
@@ -362,10 +377,11 @@ namespace motor_dev
         unsigned char crc8 = CRC8_Table(command.data(), 10);
         command.push_back(crc8);
         bool ret = sendData(command);
+#if 0
         if(ret){
             std::vector<uint8_t> data;
             data.clear();
-            ret=receiveData(data, 20, 1);
+            ret=receiveData(data, 20, 20);
             if(ret){
                 printf("<---:");
                 for(auto d:data)
@@ -373,62 +389,47 @@ namespace motor_dev
                 printf("\n");
             }
         }
+#endif
         return 0;
     }
-    int RS485Driver::bm_set_speed(float speed1,float speed2,float speed3,float speed4){
+    int RS485Driver::bm_set_speed(float speed1,float speed2){
         std::vector<uint8_t> command;
+        //if(angle>360.0)
+        //    angle=360.0;
         int32_t pos=speed1*10;
         uint8_t high1 = (pos >> 8) & 0xFF;  // 高8位
         uint8_t low1  = pos & 0xFF;         // 低8位
         pos=speed2*10;
         uint8_t high2 = (pos >> 8) & 0xFF;  // 高8位
         uint8_t low2  = pos & 0xFF;         // 低8位
-        pos=speed3*10;
-        uint8_t high3 = (pos >> 8) & 0xFF;  // 高8位
-        uint8_t low3  = pos & 0xFF;         // 低8位
-        pos=speed4*10;
-        uint8_t high4 = (pos >> 8) & 0xFF;  // 高8位
-        uint8_t low4  = pos & 0xFF;         // 低8位
-
-        command.push_back(high1);
-        command.push_back(low1);
-        command.push_back(high2);
-        command.push_back(low2);
-        command.push_back(high3);
-        command.push_back(low3);
-        command.push_back(high4);
-        command.push_back(low4);
-        printf("set_speed.%.1f\n",speed1);
-        sendCan(0x32,command);
-        return 0;
-    }
-    int RS485Driver::bm_set_enable(uint8_t enable){
-        std::vector<uint8_t> command;
-        command.push_back(enable);
-        command.push_back(enable);
-        command.push_back(enable);
-        command.push_back(enable);
-        command.push_back(0);
-        command.push_back(0);
-        command.push_back(0);
-        command.push_back(0);
-        printf("set_enable\n");
-        sendCan(0x38,command);
-        return 0;
-    }
-     int RS485Driver::bm_set_param(uint8_t motor_id, uint8_t param,uint32_t val){
-        std::vector<uint8_t> command;
-        command.push_back(motor_id);
-        command.push_back(param);
-        
-        command.push_back((uint8_t)(val));
-        command.push_back((uint8_t)(val>>8));
-        command.push_back((uint8_t)(val>>16));
-        command.push_back((uint8_t)(val>>24));
-        command.push_back(0);
-        command.push_back(0);
-        printf("set_param:%d,%d,%d\n",motor_id,param,val);
-        sendCan(0x36,command);
+        //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, 5);
+            if(ret){
+#ifdef hehe
+                printf("<---:");
+                for(auto d:data)
+                    printf("%02x ",d);
+                printf("\n");
+#endif
+            }
+        }
         return 0;
     }
     float RS485Driver::bm_get_pos(uint8_t motor_id){
@@ -449,12 +450,14 @@ namespace motor_dev
         if(ret){
             std::vector<uint8_t> data;
             data.clear();
-            ret=receiveData(data, 20, 1);
+            ret=receiveData(data, 11, 1);
             if(ret){
+#ifdef hehe
                 printf("<---:");
                 for(auto d:data)
                     printf("%02x ",d);
                 printf("\n");
+#endif
                 uint8_t nid=0x70+motor_id;
                 if(data[0]==0&&data[1]==nid){
                     //uint16_t pos = (data[2]<<8)+(data[3]&0xff);
@@ -475,26 +478,54 @@ namespace motor_dev
         }
         return -1;
     }
-    int RS485Driver::bm_get_speed(){
+    float RS485Driver::bm_get_speed(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(0x0d);
+        //command.push_back(4);
+        //command.push_back(0x0b);
+        command.push_back(0);
+        command.push_back(0);
+        command.push_back(0);
         command.push_back(1);
-        command.push_back(5);
-        command.push_back(11);
-        command.push_back(13);
         command.push_back(0);
         command.push_back(0);
         command.push_back(0);
-        command.push_back(0);
-        std::vector<uint8_t> rx=sendCan(0x35,command);
-        speedMap.clear();
-        for(int x=0;x<rx.size();x++)
-            if(rx[x]==0xaa&&rx[x+1]==0x11&&rx[x+2]==0x08){
-                uint8_t motor_id=rx[x+3]-0x70;
-                int16_t speed=(rx[x+7]<<8)+(rx[x+8]&0xff);
-                printf("id:%d,speed:%d\n",motor_id,speed);
-                speedMap[motor_id]=speed/10.0f;
+        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, 50);
+            if(ret){
+                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;
+                    int16_t pos=(data[6]<<8)+(data[7]&0xff);
+                    float angle=3.60f*pos;
+                    pos=(data[8]<<8)+(data[9]&0xff);
+                    float speed=pos/10.0f;
+                    //printf("id:%d,pos:%d\n",motor_id,pos);
+                    return speed;
+                    ///printf("pos:%d,angle:%.1f\n",pos,angle);
+                }
+                else{
+
+                }
+                //float angle = pos * 360.0 /255.0;
+                ///float angle=0;
+                
             }
-        return 0;
+        }
+        return -1;
     }
     //mt
     void RS485Driver::add_crc16(std::vector<uint8_t>& data) {
@@ -559,7 +590,7 @@ namespace motor_dev
             #if 1
             std::vector<uint8_t> data;
             data.clear();
-            ret=receiveData(data, 20, 1);
+            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);
@@ -600,7 +631,7 @@ namespace motor_dev
         bool ret=sendData(packet);
         if(ret){
             std::vector<uint8_t> data;
-            ret=receiveData(data, 20, 1);
+            ret=receiveData(data, 20, 20);
             if(ret){
                 uint8_t id= data[10];
                 return id;
diff --git a/motor_dev_can/src/main.cpp b/motor_dev_can/src/main.cpp
index fcca0e3..3997f1a 100644
--- a/motor_dev_can/src/main.cpp
+++ b/motor_dev_can/src/main.cpp
@@ -36,7 +36,7 @@ int main(int argc,char* argv[]){
     /////rs485_driver_.bm_set_mode(1,3);//位置模式
     auto rs485_sub_=node->create_subscription<interfaces::msg::MotorCmd>("/motor_cmd",10,[](const interfaces::msg::MotorCmd::SharedPtr msg){
             ///
-            if (msg->target == "rs485") {
+            if (msg->target == "can") {
                 if(msg->type=="bm"){
                     size_t n = msg->motor_id.size();
                     if(n<2){