基本通讯通了

2025-04-08 05:09:23 +08:00 · 2025-04-08 05:09:23 +08:00 · eaf598eb0b
commit eaf598eb0b
parent 99faad4200
17 changed files with 874 additions and 1 deletions
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@ -0,0 +1,8 @@
+{
+    "files.associations": {
+        "array": "cpp",
+        "string": "cpp",
+        "string_view": "cpp",
+        "chrono": "cpp"
+    }
+}
--- a/README.md
+++ b/README.md
@ -1,3 +1,19 @@
 # CM_DOG

-A simple ros2 program for legged robot . Robocon2025
+A simple ROS2 program for a legged robot. Developed for Robocon2025.
+
+## 硬件配置
+
+- **关节电机**: Unitree GO-8010-6 (12个)
+- **IMU传感器**: 轮趣科技 N100陀螺仪
+- **通信接口**: USB转思路RS485
+
+## 软件依赖
+
+- Ros2 humble
+
+## 使用说明
+
+1. 克隆仓库：
+
+2. 构建项目：
--- a/src/unitree_motor_serial_driver/CMakeLists.txt
+++ b/src/unitree_motor_serial_driver/CMakeLists.txt
@ -0,0 +1,54 @@
+cmake_minimum_required(VERSION 3.8)
+project(unitree_motor_serial_driver)
+
+# 设置 C++ 标准
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3")
+
+# 包含 ROS 2 依赖
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+
+# 包含目录和库路径
+include_directories(include)
+link_directories(lib)
+
+# 设置全局 RPATH
+set(CMAKE_INSTALL_RPATH "${CMAKE_CURRENT_SOURCE_DIR}/lib")
+set(CMAKE_BUILD_RPATH "${CMAKE_CURRENT_SOURCE_DIR}/lib")
+
+# 根据系统架构选择库
+if(CMAKE_HOST_SYSTEM_PROCESSOR MATCHES "aarch64") 
+    set(EXTRA_LIBS libUnitreeMotorSDK_Arm64.so)
+else()
+    set(EXTRA_LIBS libUnitreeMotorSDK_Linux64.so)
+endif()
+
+# 添加可执行文件和链接库
+add_executable(goM8010_6_motor src/goM8010_6_motor.cpp)
+ament_target_dependencies(goM8010_6_motor rclcpp)
+target_link_libraries(goM8010_6_motor ${EXTRA_LIBS})
+
+add_executable(unitree_motor_serial_driver src/unitree_motor_serial_driver.cpp)
+ament_target_dependencies(unitree_motor_serial_driver rclcpp)
+target_link_libraries(unitree_motor_serial_driver ${EXTRA_LIBS})
+
+# 安装目标
+install(TARGETS
+  goM8010_6_motor
+  unitree_motor_serial_driver
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY launch/
+  DESTINATION share/${PROJECT_NAME}/launch
+)
+
+# 安装头文件
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+# ROS 2 包配置
+ament_package()
--- a/src/unitree_motor_serial_driver/include/IOPort/IOPort.h
+++ b/src/unitree_motor_serial_driver/include/IOPort/IOPort.h
@ -0,0 +1,41 @@
+#ifndef __IOPORT_H
+#define __IOPORT_H
+
+#include <stdint.h>
+#include <unistd.h>
+#include <vector>
+#include "unitreeMotor/unitreeMotor.h"
+
+enum class BlockYN{
+    YES,
+    NO
+};
+
+class IOPort{
+public:
+    IOPort(BlockYN blockYN, size_t recvLength, size_t timeOutUs){
+        resetIO(blockYN, recvLength, timeOutUs);
+    }
+    virtual ~IOPort(){}
+    virtual size_t send(uint8_t *sendMsg, size_t sendLength) = 0;
+    virtual size_t recv(uint8_t *recvMsg, size_t recvLength) = 0;
+    virtual size_t recv(uint8_t *recvMsg) = 0;
+    virtual bool sendRecv(std::vector<MotorCmd> &sendVec, std::vector<MotorData> &recvVec) = 0;
+    void resetIO(BlockYN blockYN, size_t recvLength, size_t timeOutUs);
+protected:
+    BlockYN _blockYN = BlockYN::NO;
+    size_t _recvLength;
+    timeval _timeout;
+    timeval _timeoutSaved;
+};
+
+inline void IOPort::resetIO(BlockYN blockYN, size_t recvLength, size_t timeOutUs){
+    _blockYN = blockYN;
+    _recvLength = recvLength;
+    _timeout.tv_sec  = timeOutUs / 1000000;
+    _timeout.tv_usec = timeOutUs % 1000000;
+    _timeoutSaved = _timeout;
+}
+
+
+#endif  // z1_lib_IOPORT_H
--- a/src/unitree_motor_serial_driver/include/crc/crc32.h
+++ b/src/unitree_motor_serial_driver/include/crc/crc32.h
@ -0,0 +1,33 @@
+#ifndef CRC32_H
+#define CRC32_H
+
+#include <stdint.h>
+
+inline uint32_t crc32_core(uint32_t* ptr, uint32_t len){
+    uint32_t xbit = 0;
+    uint32_t data = 0;
+    uint32_t CRC32 = 0xFFFFFFFF;
+    const uint32_t dwPolynomial = 0x04c11db7;
+    for (uint32_t i = 0; i < len; i++)
+    {
+        xbit = 1 << 31;
+        data = ptr[i];
+        for (uint32_t bits = 0; bits < 32; bits++)
+        {
+            if (CRC32 & 0x80000000)
+            {
+                CRC32 <<= 1;
+                CRC32 ^= dwPolynomial;
+            }
+            else
+                CRC32 <<= 1;
+            if (data & xbit)
+                CRC32 ^= dwPolynomial;
+
+            xbit >>= 1;
+        }
+    }
+    return CRC32;
+}
+
+#endif  // CRC32_H
--- a/src/unitree_motor_serial_driver/include/crc/crc_ccitt.h
+++ b/src/unitree_motor_serial_driver/include/crc/crc_ccitt.h
@ -0,0 +1,67 @@
+#ifndef __CRC_CCITT_H
+#define __CRC_CCITT_H
+
+/*
+ * This mysterious table is just the CRC of each possible byte. It can be
+ * computed using the standard bit-at-a-time methods. The polynomial can
+ * be seen in entry 128, 0x8408. This corresponds to x^0 + x^5 + x^12.
+ * Add the implicit x^16, and you have the standard CRC-CCITT.
+ * https://github.com/torvalds/linux/blob/5bfc75d92efd494db37f5c4c173d3639d4772966/lib/crc-ccitt.c
+ */
+uint16_t const crc_ccitt_table[256] = {
+	0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf,
+	0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7,
+	0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e,
+	0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876,
+	0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd,
+	0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5,
+	0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c,
+	0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974,
+	0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb,
+	0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3,
+	0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a,
+	0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72,
+	0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9,
+	0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1,
+	0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738,
+	0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70,
+	0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7,
+	0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
+	0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036,
+	0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e,
+	0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5,
+	0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd,
+	0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134,
+	0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c,
+	0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3,
+	0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb,
+	0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
+	0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a,
+	0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1,
+	0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9,
+	0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330,
+	0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78
+};
+
+
+static uint16_t crc_ccitt_byte(uint16_t crc, const uint8_t c)
+{
+	return (crc >> 8) ^ crc_ccitt_table[(crc ^ c) & 0xff];
+}
+
+/**
+ *	crc_ccitt - recompute the CRC (CRC-CCITT variant) for the data
+ *	buffer
+ *	@crc: previous CRC value
+ *	@buffer: data pointer
+ *	@len: number of bytes in the buffer
+ */
+inline uint16_t crc_ccitt(uint16_t crc, uint8_t const *buffer, size_t len)
+{
+	while (len--)
+		crc = crc_ccitt_byte(crc, *buffer++);
+	return crc;
+}
+
+
+#endif
--- a/src/unitree_motor_serial_driver/include/serialPort/SerialPort.h
+++ b/src/unitree_motor_serial_driver/include/serialPort/SerialPort.h
@ -0,0 +1,98 @@
+#ifndef __SERIALPORT_H
+#define __SERIALPORT_H
+
+/* 
+High frequency serial communication,
+Not that common, but useful for motor communication.
+*/
+#include <termios.h>
+#include <sys/select.h>
+#include <string>
+#include <string.h>
+#include <stdint.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <linux/serial.h>    
+#include <unistd.h>
+#include <iostream>
+
+#include "serialPort/include/errorClass.h"
+#include "unitreeMotor/unitreeMotor.h"
+#include "IOPort/IOPort.h"
+
+enum class bytesize_t{
+    fivebits,
+    sixbits,
+    sevenbits,
+    eightbits
+};
+
+enum class parity_t{
+    parity_none,
+    parity_odd,
+    parity_even,
+    parity_mark,
+    parity_space
+};
+
+enum class stopbits_t{
+    stopbits_one,
+    stopbits_two,
+    stopbits_one_point_five
+};
+
+enum class flowcontrol_t{
+    flowcontrol_none,
+    flowcontrol_software,
+    flowcontrol_hardware
+};
+
+class SerialPort : public IOPort{
+public:
+    SerialPort(const std::string &portName,
+            size_t recvLength = 16,
+            uint32_t baudrate = 4000000,
+            size_t timeOutUs = 20000, 
+            BlockYN blockYN = BlockYN::NO,
+            bytesize_t bytesize = bytesize_t::eightbits,
+            parity_t parity = parity_t::parity_none,
+            stopbits_t stopbits = stopbits_t::stopbits_one,
+            flowcontrol_t flowcontrol = flowcontrol_t::flowcontrol_none);
+    virtual ~SerialPort();
+    void resetSerial(size_t recvLength = 16,
+                     uint32_t baudrate = 4000000,
+                     size_t timeOutUs = 20000,
+                     BlockYN blockYN = BlockYN::NO,
+                     bytesize_t bytesize = bytesize_t::eightbits,
+                     parity_t parity = parity_t::parity_none,
+                     stopbits_t stopbits = stopbits_t::stopbits_one,
+                     flowcontrol_t flowcontrol = flowcontrol_t::flowcontrol_none);
+    size_t send(uint8_t *sendMsg, size_t sendLength);
+    size_t recv(uint8_t *recvMsg, size_t recvLength);
+    size_t recv(uint8_t *recvMsg);
+    bool sendRecv(uint8_t *sendMsg, uint8_t *recvMsg, size_t sendLength);
+    bool sendRecv(MotorCmd* sendMsg, MotorData* recvMsg);
+    bool sendRecv(std::vector<MotorCmd> &sendVec, std::vector<MotorData> &recvVec);
+    void test();
+private:
+    void _open();
+    void _set();
+    void _close();
+    size_t _nonBlockRecv(uint8_t *recvMsg, size_t readLen);
+    std::string _portName;
+    uint32_t _baudrate;
+    bytesize_t _bytesize;
+    parity_t _parity;
+    stopbits_t _stopbits;
+    flowcontrol_t _flowcontrol;
+    bool _xonxoff;
+    bool _rtscts;
+    int _fd;
+    fd_set _rSet;
+
+};
+
+
+
+
+#endif  // SERIALPORT_H
--- a/src/unitree_motor_serial_driver/include/serialPort/include/errorClass.h
+++ b/src/unitree_motor_serial_driver/include/serialPort/include/errorClass.h
@ -0,0 +1,52 @@
+#ifndef __ERRORCLASS_H
+#define __ERRORCLASS_H
+
+#include <string>
+#include <exception>
+#include <stdexcept>
+#include <sstream>
+#include <cstring>
+
+#define THROW(exceptionClass, message) throw exceptionClass(__FILE__, \
+__LINE__, (message) )
+
+class IOException : public std::exception
+{
+  // Disable copy constructors
+  IOException& operator=(const IOException&);
+  std::string file_;
+  int line_;
+  std::string e_what_;
+  int errno_;
+public:
+  explicit IOException (std::string file, int line, int errnum)
+    : file_(file), line_(line), errno_(errnum) {
+      std::stringstream ss;
+#if defined(_WIN32) && !defined(__MINGW32__)
+      char error_str [1024];
+      strerror_s(error_str, 1024, errnum);
+#else
+      char * error_str = strerror(errnum);
+#endif
+      ss << "IO Exception (" << errno_ << "): " << error_str;
+      ss << ", file " << file_ << ", line " << line_ << ".";
+      e_what_ = ss.str();
+  }
+  explicit IOException (std::string file, int line, const char * description)
+    : file_(file), line_(line), errno_(0) {
+      std::stringstream ss;
+      ss << "IO Exception: " << description;
+      ss << ", file " << file_ << ", line " << line_ << ".";
+      e_what_ = ss.str();
+  }
+  virtual ~IOException() throw() {}
+  IOException (const IOException& other) : line_(other.line_), e_what_(other.e_what_), errno_(other.errno_) {}
+
+  int getErrorNumber () const { return errno_; }
+
+  virtual const char* what () const throw () {
+    return e_what_.c_str();
+  }
+};
+
+#endif  // ERRORCLASS_H
--- a/src/unitree_motor_serial_driver/include/unitreeMotor/include/motor_msg_A1B1.h
+++ b/src/unitree_motor_serial_driver/include/unitreeMotor/include/motor_msg_A1B1.h
@ -0,0 +1,162 @@
+#ifndef MOTOR_A1B1_MSG
+#define MOTOR_A1B1_MSG
+
+#include <stdint.h>
+typedef int16_t q15_t;
+
+#pragma pack(1)
+
+// 发送用单个数据数据结构
+typedef union{
+        int32_t           L;
+        uint8_t       u8[4];
+       uint16_t      u16[2];
+       uint32_t         u32;
+          float           F;
+}COMData32;
+
+typedef struct {
+	// 定义 数据包头
+    unsigned char  start[2];     // 包头
+	unsigned char  motorID;      // 电机ID  0,1,2,3 ...   0xBB 表示向所有电机广播（此时无返回）
+	unsigned char  reserved;
+}COMHead;
+
+#pragma pack()
+
+#pragma pack(1)
+
+typedef struct { 
+	
+	   uint8_t  fan_d;       // 关节上的散热风扇转速
+	   uint8_t  Fmusic;      // 电机发声频率   /64*1000   15.625f 频率分度
+	   uint8_t  Hmusic;      // 电机发声强度   推荐值4  声音强度 0.1 分度  
+	   uint8_t  reserved4;
+	
+	   uint8_t  FRGB[4];     // 足端LED 
+	
+}LowHzMotorCmd;
+
+typedef struct {  // 以 4个字节一组排列 ，不然编译器会凑整
+	// 定义 数据
+    uint8_t  mode;        // 关节模式选择
+    uint8_t  ModifyBit;   // 电机控制参数修改位
+    uint8_t  ReadBit;     // 电机控制参数发送位
+    uint8_t  reserved;
+
+    COMData32  Modify;     // 电机参数修改 的数据 
+    //实际给FOC的指令力矩为：
+    //K_P*delta_Pos + K_W*delta_W + T
+    q15_t     T;      // 期望关节的输出力矩（电机本身的力矩）x256, 7 + 8 描述
+    q15_t     W;      // 期望关节速度 （电机本身的速度） x128,       8 + 7描述	
+    int32_t   Pos;      // 期望关节位置 x 16384/6.2832, 14位编码器（主控0点修正，电机关节还是以编码器0点为准）
+
+    q15_t    K_P;      // 关节刚度系数 x2048  4+11 描述
+    q15_t    K_W;      // 关节速度系数 x1024  5+10 描述
+
+    uint8_t LowHzMotorCmdIndex;     // 电机低频率控制命令的索引, 0-7, 分别代表LowHzMotorCmd中的8个字节
+    uint8_t LowHzMotorCmdByte;      // 电机低频率控制命令的字节
+	
+     COMData32  Res[1];    // 通讯 保留字节  用于实现别的一些通讯内容
+	
+}MasterComdV3;   // 加上数据包的包头 和CRC 34字节
+
+typedef struct {
+	// 定义 电机控制命令数据包	
+    COMHead head;    
+    MasterComdV3 Mdata;
+    COMData32 CRCdata;
+}MasterComdDataV3;//返回数据
+
+// typedef struct {
+// 	// 定义 总得485 数据包
+	
+//   MasterComdData M1;
+// 	MasterComdData M2;
+// 	MasterComdData M3;
+	
+// }DMA485TxDataV3;
+
+#pragma pack()
+
+#pragma pack(1)
+
+typedef struct {  // 以 4个字节一组排列 ，不然编译器会凑整
+    // 定义 数据
+    uint8_t  mode;        // 当前关节模式
+    uint8_t  ReadBit;     // 电机控制参数修改     是否成功位
+    int8_t  Temp;        // 电机当前平均温度   
+    uint8_t  MError;      // 电机错误 标识
+ 
+    COMData32  Read;     // 读取的当前 电机 的控制数据 
+    int16_t     T;      // 当前实际电机输出力矩       7 + 8 描述
+
+    int16_t     W;      // 当前实际电机速度（高速）   8 + 7 描述
+    float      LW;      // 当前实际电机速度（低速）   
+
+    int16_t     W2;      // 当前实际关节速度（高速）   8 + 7 描述
+    float      LW2;      // 当前实际关节速度（低速）   
+
+    int16_t    Acc;           // 电机转子加速度       15+0 描述  惯量较小
+    int16_t    OutAcc;        // 输出轴加速度         12+3 描述  惯量较大
+		 
+    int32_t   Pos;      // 当前电机位置（主控0点修正，电机关节还是以编码器0点为准）
+    int32_t   Pos2;     // 关节编码器位置(输出编码器)
+
+    int16_t     gyro[3];  // 电机驱动板6轴传感器数据
+    int16_t     acc[3];   
+
+    // 力传感器的数据   
+    int16_t     Fgyro[3];  //  
+    int16_t     Facc[3];
+    int16_t     Fmag[3];
+    uint8_t     Ftemp;     // 8位表示的温度  7位（-28~100度）  1位0.5度分辨率
+    
+    int16_t     Force16;   // 力传感器高16位数据
+    int8_t      Force8;    // 力传感器低8位数据
+		
+    uint8_t     FError;    //  足端传感器错误标识
+		
+    int8_t      Res[1];    // 通讯 保留字节
+	
+}ServoComdV3;  // 加上数据包的包头 和CRC 78字节（4+70+4）
+
+typedef struct {
+    // 定义 电机控制命令数据包	
+    COMHead        head;
+    ServoComdV3      Mdata;
+
+    COMData32    CRCdata;
+
+}ServoComdDataV3;	//发送数据
+
+// typedef struct {
+// 	// 定义 总的485 接受数据包
+	
+//   ServoComdDataV3 M[3];
+//  // uint8_t  nullbyte1;
+	
+// }DMA485RxDataV3;
+
+
+#pragma pack()
+
+//  00 00 00 00 00 
+//  00 00 00 00 00 
+//  00 00 00 00 00 
+//  00 00 00
+// 数据包默认初始化 
+// 主机发送的数据包 
+/*
+                 Tx485Data[_FR][i].head.start[0] = 0xFE ;     Tx485Data[_FR][i].head.start[1] = 0xEE; // 数据包头					 
+				 Tx485Data[_FR][i].Mdata.ModifyBit = 0xFF;    Tx485Data[_FR][i].Mdata.mode = 0;   // 默认不修改数据 和 电机的默认工作模式				
+				 Tx485Data[_FR][i].head.motorID = i;    0                                          // 目标电机标号
+				 Tx485Data[_FR][i].Mdata.T = 0.0f;                           // 默认目标关节输出力矩                      motor1.Extra_Torque = motorRxData[1].Mdata.T*0.390625f;     // N.M  转化为 N.CM   IQ8描述
+				 Tx485Data[_FR][i].Mdata.Pos = 0x7FE95C80;                   // 默认目标关节位置  不启用位置环          14位分辨率 
+				 Tx485Data[_FR][i].Mdata.W = 16000.0f;                       // 默认目标关节速度  不启用速度环          1+8+7描述     motor1.Target_Speed =  motorRxData[1].Mdata.W*0.0078125f;   // 单位 rad/s	       IQ7描述
+				 Tx485Data[_FR][i].Mdata.K_P = (q15_t)(0.6f*(1<<11));        // 默认关节刚度系数   4+11 描述                     motor1.K_Pos = ((float)motorRxData[1].Mdata.K_P)/(1<<11);      // 描述刚度的通讯数据格式  4+11
+				 Tx485Data[_FR][i].Mdata.K_W = (q15_t)(1.0f*(1<<10));        // 默认关节速度系数   5+10 描述                    motor1.K_Speed = ((float)motorRxData[1].Mdata.K_W)/(1<<10);    // 描述阻尼的通讯数据格式  5+10
+*/ 
+
+
+#endif
--- a/src/unitree_motor_serial_driver/include/unitreeMotor/include/motor_msg_GO-M8010-6.h
+++ b/src/unitree_motor_serial_driver/include/unitreeMotor/include/motor_msg_GO-M8010-6.h
@ -0,0 +1,90 @@
+#ifndef __MOTOR_MSG_GO_M8010_6_H
+#define __MOTOR_MSG_GO_M8010_6_H
+
+#include <stdint.h>
+#define CRC_SIZE        2
+#define CTRL_DAT_SIZE   sizeof(ControlData_t) - CRC_SIZE
+#define DATA_DAT_SIZE   sizeof(MotorData_t) - CRC_SIZE
+
+#pragma pack(1)
+
+/**
+ * @brief 电机模式控制信息
+ * 
+ */
+typedef struct
+{
+    uint8_t id     :4;      // 电机ID: 0,1...,14 15表示向所有电机广播数据(此时无返回)
+    uint8_t status :3;      // 工作模式: 0.锁定 1.FOC闭环 2.编码器校准 3.保留
+    uint8_t none   :1;      // 保留位
+} RIS_Mode_t;   // 控制模式 1Byte
+
+/**
+ * @brief 电机状态控制信息
+ * 
+ */
+typedef struct
+{
+    int16_t tor_des;        // 期望关节输出扭矩 unit: N.m     (q8)
+    int16_t spd_des;        // 期望关节输出速度 unit: rad/s   (q7)
+    int32_t pos_des;        // 期望关节输出位置 unit: rad     (q15)
+    uint16_t  k_pos;        // 期望关节刚度系数 unit: 0.0-1.0 (q15)
+    uint16_t  k_spd;        // 期望关节阻尼系数 unit: 0.0-1.0 (q15)
+    
+} RIS_Comd_t;   // 控制参数 12Byte
+
+/**
+ * @brief 电机状态反馈信息
+ * 
+ */
+typedef struct
+{
+    int16_t  torque;        // 实际关节输出扭矩 unit: N.m     (q8)
+    int16_t  speed;         // 实际关节输出速度 unit: rad/s   (q7)
+    int32_t  pos;           // 实际关节输出位置 unit: W       (q15)
+    int8_t   temp;          // 电机温度: -128~127°C 90°C时触发温度保护
+    uint8_t  MError :3;     // 电机错误标识: 0.正常 1.过热 2.过流 3.过压 4.编码器故障 5-7.保留
+    uint16_t force  :12;    // 足端气压传感器数据 12bit (0-4095)
+    uint8_t  none   :1;     // 保留位
+} RIS_Fbk_t;   // 状态数据 11Byte
+
+
+#pragma pack()
+
+#pragma pack(1)
+
+/**
+ * @brief 控制数据包格式
+ * 
+ */
+typedef struct
+{
+    uint8_t head[2];    // 包头         2Byte
+    RIS_Mode_t mode;    // 电机控制模式  1Byte
+    RIS_Comd_t comd;    // 电机期望数据 12Byte
+    uint16_t   CRC16;   // CRC          2Byte
+
+} ControlData_t;    // 主机控制命令     17Byte
+
+/**
+ * @brief 电机反馈数据包格式
+ * 
+ */
+typedef struct
+{
+    uint8_t head[2];    // 包头         2Byte
+    RIS_Mode_t mode;    // 电机控制模式  1Byte
+    RIS_Fbk_t   fbk;    // 电机反馈数据 11Byte
+    uint16_t  CRC16;    // CRC          2Byte
+
+} MotorData_t;      // 电机返回数据     16Byte
+
+#pragma pack()
+
+#endif
+
+
+
+
+
+
--- a/src/unitree_motor_serial_driver/include/unitreeMotor/unitreeMotor.h
+++ b/src/unitree_motor_serial_driver/include/unitreeMotor/unitreeMotor.h
@ -0,0 +1,74 @@
+#ifndef __UNITREEMOTOR_H
+#define __UNITREEMOTOR_H
+
+#include "unitreeMotor/include/motor_msg_GO-M8010-6.h" 
+#include "unitreeMotor/include/motor_msg_A1B1.h"  
+#include <stdint.h>
+#include <iostream>
+
+enum class MotorType{
+    A1, // 4.8M baudrate
+    B1, // 6.0M baudrate
+    GO_M8010_6
+};
+
+enum class MotorMode{
+    BRAKE, 
+    FOC, 
+    CALIBRATE
+};
+
+struct MotorCmd{
+    public:
+        MotorCmd(){}
+        MotorType motorType;
+	    int hex_len;                   
+        unsigned short id;              
+        unsigned short mode;           
+        float tau;                        
+        float dq;                       
+        float q;                     
+        float kp;                     
+        float kd;                     
+
+        void modify_data(MotorCmd* motor_s); 
+        uint8_t* get_motor_send_data();
+
+        COMData32 Res;                
+    private:
+        ControlData_t  GO_M8010_6_motor_send_data;  
+        MasterComdDataV3  A1B1_motor_send_data; 
+};
+
+struct MotorData{
+    public:
+        MotorData(){}
+        MotorType motorType;
+        int hex_len;                   
+        unsigned char motor_id;         
+        unsigned char mode;             
+        int temp;                      
+        int merror;          
+        float tau;                       
+        float dq;                       
+        float q;                     
+       
+        bool correct = false;                  
+        bool extract_data(MotorData* motor_r);
+        uint8_t* get_motor_recv_data();
+       
+        int footForce;
+        float LW;                      
+        int Acc;                      
+
+        float gyro[3];                  
+        float acc[3];
+    private:
+        MotorData_t GO_M8010_6_motor_recv_data;    
+        ServoComdDataV3 A1B1_motor_recv_data;     
+};
+
+// Utility Function
+int queryMotorMode(MotorType motortype,MotorMode motormode);
+float queryGearRatio(MotorType motortype);
+#endif  // UNITREEMOTOR_H
--- a/src/unitree_motor_serial_driver/include/unitree_motor_serial_driver/unitree_motor_serial_driver.hpp
+++ b/src/unitree_motor_serial_driver/include/unitree_motor_serial_driver/unitree_motor_serial_driver.hpp
@ -0,0 +1,34 @@
+#ifndef UNITREE_MOTOR_SERIAL_DRIVER_HPP
+#define UNITREE_MOTOR_SERIAL_DRIVER_HPP
+
+#include <chrono>
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "serialPort/SerialPort.h"
+#include "unitreeMotor/unitreeMotor.h"
+
+
+namespace unitree_motor_serial_driver
+{
+
+struct Motor_t
+{
+    MotorCmd cmd;
+    MotorData data;
+};
+
+class MotorControlNode : public rclcpp::Node
+{
+public:
+    MotorControlNode();
+
+private:
+    void control_motor();
+
+    SerialPort serial_;
+    rclcpp::TimerBase::SharedPtr timer_;
+};
+
+}
+
+#endif
--- a/src/unitree_motor_serial_driver/launch/goM8010_6_motor.launch.py
+++ b/src/unitree_motor_serial_driver/launch/goM8010_6_motor.launch.py
@ -0,0 +1,15 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='unitree_motor_serial_driver',
+            executable='goM8010_6_motor',
+            name='goM8010_6_motor',
+            output='screen',
+            parameters=[
+
+            ]
+        )
+    ])
--- a/src/unitree_motor_serial_driver/launch/unitree_motor_serial_driver.launch.py
+++ b/src/unitree_motor_serial_driver/launch/unitree_motor_serial_driver.launch.py
@ -0,0 +1,15 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='unitree_motor_serial_driver',
+            executable='unitree_motor_serial_driver',
+            name='unitree_motor_serial_driver',
+            output='screen',
+            parameters=[
+
+            ]
+        )
+    ])
--- a/src/unitree_motor_serial_driver/package.xml
+++ b/src/unitree_motor_serial_driver/package.xml
@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>unitree_motor_serial_driver</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="1683502971@qq.com">robofish</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
--- a/src/unitree_motor_serial_driver/src/goM8010_6_motor.cpp
+++ b/src/unitree_motor_serial_driver/src/goM8010_6_motor.cpp
@ -0,0 +1,51 @@
+#include <chrono>
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "serialPort/SerialPort.h"
+#include "unitreeMotor/unitreeMotor.h"
+
+using namespace std::chrono_literals;
+
+class MotorControlNode : public rclcpp::Node
+{
+public:
+    MotorControlNode()
+        : Node("goM8010_6_motor"), serial_("/dev/ttyACM0")
+    {
+        timer_ = this->create_wall_timer(
+            1ms, std::bind(&MotorControlNode::control_motor, this));
+    }
+
+private:
+    void control_motor()
+    {
+        MotorCmd cmd;
+        MotorData data;
+
+        cmd.motorType = MotorType::GO_M8010_6;
+        data.motorType = MotorType::GO_M8010_6;
+        cmd.mode = queryMotorMode(MotorType::GO_M8010_6, MotorMode::FOC);
+        cmd.id = 0;
+        cmd.q = 0.0; 
+        cmd.dq = 0.0;
+        cmd.kp = 0.0;
+        cmd.kd = 0.0;
+        cmd.tau = 0.0;
+
+        serial_.sendRecv(&cmd, &data);
+
+        RCLCPP_INFO(this->get_logger(), "\nMotor.q: %f\nMotor.temp: %d\nMotor.W: %f\nMotor.tau: %f\n",
+                    data.q, data.temp, data.dq, data.tau);
+    }
+
+    SerialPort serial_;
+    rclcpp::TimerBase::SharedPtr timer_;
+};
+
+int main(int argc, char **argv)
+{
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<MotorControlNode>());
+    rclcpp::shutdown();
+    return 0;
+}
--- a/src/unitree_motor_serial_driver/src/unitree_motor_serial_driver.cpp
+++ b/src/unitree_motor_serial_driver/src/unitree_motor_serial_driver.cpp
@ -0,0 +1,42 @@
+#include "unitree_motor_serial_driver/unitree_motor_serial_driver.hpp"
+
+namespace unitree_motor_serial_driver
+{
+
+MotorControlNode::MotorControlNode()
+    : Node("unitree_motor_serial_driver"), serial_("/dev/ttyACM0")
+{
+    timer_ = this->create_wall_timer(
+        std::chrono::milliseconds(1), std::bind(&MotorControlNode::control_motor, this));
+}
+
+void MotorControlNode::control_motor()
+{
+    MotorCmd cmd;
+    MotorData data;
+
+    cmd.motorType = MotorType::GO_M8010_6;
+    data.motorType = MotorType::GO_M8010_6;
+    cmd.mode = queryMotorMode(MotorType::GO_M8010_6, MotorMode::FOC);
+    cmd.id = 0;
+    cmd.q = 0.0; 
+    cmd.dq = 0.0;
+    cmd.kp = 0.0;
+    cmd.kd = 0.0;
+    cmd.tau = 0.0;
+
+    serial_.sendRecv(&cmd, &data);
+
+    RCLCPP_INFO(this->get_logger(), "\nMotor.q: %f\nMotor.temp: %d\nMotor.W: %f\nMotor.tau: %f\n",
+                data.q, data.temp, data.dq, data.tau);
+}
+
+} // namespace unitree_motor_serial_driver
+
+int main(int argc, char **argv)
+{
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<unitree_motor_serial_driver::MotorControlNode>());
+    rclcpp::shutdown();
+    return 0;
+}