delete build folder

CMakeLists.txt

@@ -1,12 +1,22 @@
-cmake_minimum_required(VERSION 3.10.2)
-project(UnitreeMotorSDK_M80106)
+cmake_minimum_required(VERSION 2.8.2)
+project(UnitreeMotorA1B1)
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3")
 
-include_directories("include/")
-link_directories(
-    lib/
-)
+include_directories(include)
+link_directories(lib)
 
-#example
-add_executable(motorctrl example/main.cpp)
-target_link_libraries(motorctrl libUnitreeMotorSDK_M80106_Linux64.so)
+if(CMAKE_HOST_SYSTEM_PROCESSOR MATCHES "aarch64") 
+    add_executable(motorctrl example/main.cpp )
+    target_link_libraries(motorctrl unitreeMotorSDK_Arm64)
+
+    add_executable(changeID example/changeID.cpp)
+    target_link_libraries(changeID unitreeMotorSDK_Arm64)
+
+else()
+    add_executable(motorctrl example/main.cpp )
+    target_link_libraries(motorctrl unitreeMotorSDK_Linux64)
+
+    add_executable(changeID example/changeID.cpp)
+    target_link_libraries(changeID unitreeMotorSDK_Linux64)
+
+endif()

README.md

@@ -2,9 +2,15 @@
 
 ### Notice
 
-support motor: GO-M8010-6 motor
+support motor: A1 motor、 B1 motor 
 
-not support motor: A1 motor、 B1 motor (Check A1B1 branch for support)
+not support motor: GO-M8010-6 motor (Check GO-M8010-6 branch for support)
+
+gcc >= 5.4.0 (for libunitreeMotorSDK_Linux64.so)
+
+gcc >= 8.3.0 (for libunitreeMotorSDK_Arm64.so)
+
+run gcc -v  command to check your gcc version
 
 ### Build
 ```bash

example/changeID.cpp

@@ -0,0 +1,61 @@
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include "serialPort/SerialPort.h"
+
+#define BroadAllMotorID 0xBB
+#define MotorPulsator 11
+
+int main()
+{
+    char serial_name[100];
+
+    SerialPort *serial;
+    MotorCmd motor_s;
+    motor_s.motorType = MotorType::A1; // set the motor type, A1 or B1
+
+    printf("Please input the name of serial port.(e.g. Linux:/dev/ttyUSB0, Windows:\\\\.\\COM3)\n");
+    scanf("%s", serial_name);
+    printf("The serial port is %s\n", serial_name);
+
+    memset(static_cast<void *>(&motor_s), 0, sizeof(motor_s));
+    motor_s.id = BroadAllMotorID;
+    motor_s.mode = 10;
+    motor_s.modify_data(&motor_s);
+
+    printf("The motor ID is: %X\n", motor_s.motor_send_data.head.motorID);
+
+    //进入伺服模式
+    if (motor_s.motorType == MotorType::A1)
+        serial = new SerialPort(serial_name, 78, 4800000); // set the serial port name
+    else
+        serial = new SerialPort(serial_name, 78, 6000000); // set the serial port name
+
+    serial->send((uint8_t *)&(motor_s.motor_send_data), 34);
+
+    usleep(100000); // sleep 0.1s
+
+    //进入拨轮模式（修改ID）
+    motor_s.mode = MotorPulsator;
+    motor_s.modify_data(&motor_s);
+    serial->send((uint8_t *)&(motor_s.motor_send_data), 34);
+
+    printf("Please turn the motor.\n");
+    printf("One time: id=0; Two times: id=1, Three times: id=2\n");
+    printf("ID can only be 0, 1, 2\n");
+    printf("Once finished, press 'a'\n");
+
+    // int c;
+    while (getchar() != (int)'a')
+    {
+    }
+    printf("Turn finished\n");
+
+    //保存ID
+    motor_s.mode = 0;
+    motor_s.modify_data(&motor_s);
+    serial->send((uint8_t *)&(motor_s.motor_send_data), 34);
+
+    return 0;
+}

example/main.cpp

@@ -1,36 +1,34 @@
 #include "serialPort/SerialPort.h"
 #include <unistd.h>
 
-int main() {
+int main()
+{
 
-  SerialPort  serial("/dev/ttyUSB0");
-  MotorCmd    cmd;
-  MotorData   data;
+  SerialPort serial("/dev/ttyUSB0");
+  MotorCmd cmd;
+  MotorData data;
 
-  while(true) {
-    cmd.motorType = MotorType::GO_M8010_6;
-    cmd.id    = 0;
-    cmd.mode  = 1;
-    cmd.K_P   = 0.0;
-    cmd.K_W   = 0.05;
-    cmd.Pos   = 0.0;
-    cmd.W     = 6.28*6.33;
-    cmd.T     = 0.0;
-    serial.sendRecv(&cmd,&data);
+  while (true)
+  {
+    cmd.motorType = MotorType::A1;
+    data.motorType = MotorType::A1;
+    cmd.id = 1;
+    cmd.mode = 5;
+    cmd.K_P = 0.0;
+    cmd.K_W = 0.1;
+    cmd.Pos = 0.0;
+    cmd.W = 6.28;
+    cmd.T = 0.0;
+    serial.sendRecv(&cmd, &data);
 
-    if(data.correct == true)
-    {
-      std::cout <<  std::endl;
-      std::cout <<  "motor.Pos: "    << data.Pos    << " rad" << std::endl;
-      std::cout <<  "motor.Temp: "   << data.Temp   << " ℃"  << std::endl;
-      std::cout <<  "motor.W: "      << data.W      << " rad/s"<<std::endl;
-      std::cout <<  "motor.T: "      << data.T      << " N.m" << std::endl;
-      std::cout <<  "motor.MError: " << data.MError <<  std::endl;
-      std::cout <<  std::endl;
-    }
+    std::cout << std::endl;
+    std::cout << "motor.Pos: " << data.Pos << std::endl;
+    std::cout << "motor.Temp: " << data.Temp << std::endl;
+    std::cout << "motor.W: " << data.W << std::endl;
+    std::cout << "motor.T: " << data.T << std::endl;
+    std::cout << "motor.MError: " << data.MError << std::endl;
+    std::cout << std::endl;
 
     usleep(200);
-
   }
-
 }

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

include/crc/crc_ccitt.h

@@ -1,67 +0,0 @@
-#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

include/serialPort/SerialPort.h

@@ -20,14 +20,16 @@ Not that common, but useful for motor communication.
 #include "unitreeMotor/unitreeMotor.h"
 #include "IOPort/IOPort.h"
 
-enum class bytesize_t{
+enum class bytesize_t
+{
     fivebits,
     sixbits,
     sevenbits,
     eightbits
 };
 
-enum class parity_t{
+enum class parity_t
+{
     parity_none,
     parity_odd,
     parity_even,
@@ -35,32 +37,35 @@ enum class parity_t{
     parity_space
 };
 
-enum class stopbits_t{
+enum class stopbits_t
+{
     stopbits_one,
     stopbits_two,
     stopbits_one_point_five
 };
 
-enum class flowcontrol_t{
+enum class flowcontrol_t
+{
     flowcontrol_none,
     flowcontrol_software,
     flowcontrol_hardware
 };
 
-class SerialPort : public IOPort{
+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);
+               size_t recvLength = 78,
+               uint32_t baudrate = 4800000,
+               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,
+    void resetSerial(size_t recvLength = 78,
+                     uint32_t baudrate = 4800000,
                      size_t timeOutUs = 20000,
                      BlockYN blockYN = BlockYN::NO,
                      bytesize_t bytesize = bytesize_t::eightbits,
@@ -71,7 +76,7 @@ public:
     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(MotorCmd *sendMsg, MotorData *recvMsg);
     bool sendRecv(std::vector<MotorCmd> &sendVec, std::vector<MotorData> &recvVec);
 
 private:
@@ -89,10 +94,6 @@ private:
     bool _rtscts;
     int _fd;
     fd_set _rSet;
-
 };
 
-
-
-
-#endif  // SERIALPORT_H
+#endif // SERIALPORT_H

include/unitreeMotor/include/motor_msg.h

@@ -1,90 +1,167 @@
-#ifndef __MOTOR_MSG_H
-#define __MOTOR_MSG_H
+#ifndef MOTOR_MSG
+#define MOTOR_MSG
 
 #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
+typedef int16_t q15_t;
 
 #pragma pack(1)
 
-/**
- * @brief 电机模式控制信息
- * 
- */
+// 发送用单个数据数据结构
+typedef union
+{
+    int32_t L;
+    uint8_t u8[4];
+    uint16_t u16[2];
+    uint32_t u32;
+    float F;
+} COMData32;
+
 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   (q8)
-    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   (q8)
-    int32_t  pos;           // 实际关节输出位置 unit: W       (q15)
-    int8_t   temp;          // 电机温度: -50~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
-
+    // 定义 数据包头
+    unsigned char start[2]; // 包头
+    unsigned char motorID;  // 电机ID  0,1,2,3 ...   0xBB 表示向所有电机广播（此时无返回）
+    unsigned char reserved;
+} COMHead;
 
 #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
+    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字节
 
-/**
- * @brief 电机反馈数据包格式
- * 
- */
 typedef struct
 {
-    uint8_t head[2];    // 包头         2Byte
-    RIS_Mode_t mode;    // 电机控制模式  1Byte
-    RIS_Fbk_t   fbk;    // 电机反馈数据 11Byte
-    uint16_t  CRC16;    // CRC          2Byte
+    // 定义 电机控制命令数据包
+    COMHead head;
+    MasterComdV3 Mdata;
+    COMData32 CRCdata;
+} MasterComdDataV3; //返回数据
 
-} MotorData_t;      // 电机返回数据     16Byte
+// 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
-
-
-
-
-
-

include/unitreeMotor/unitreeMotor.h

@@ -1,54 +1,57 @@
 #ifndef __UNITREEMOTOR_H
 #define __UNITREEMOTOR_H
 
-#include "unitreeMotor/include/motor_msg.h"  // 电机通信协议
+#include "unitreeMotor/include/motor_msg.h" // 电机通信协议
 #include <stdint.h>
 #include <iostream>
 
-enum class MotorType{
-    GO_M8010_6,   
-};
-
-struct MotorCmd{
-	// 定义 发送格式化数据
-    public:
-        MotorType motorType = MotorType::GO_M8010_6;
-	    int hex_len = 17;                   
-        unsigned short id;              // 电机ID 0~14 15:广播ID 此时电机无返回
-        unsigned short mode;            // 电机模式 0:刹车 1:FOC闭环 2:电机标定(发送后等待5sec,期间禁止给电机发送消息)
-        float T;                        // 期望关节的输出力矩(电机转子转矩 N.m) 范围: ±127.99
-        float W;                        // 期望关节速度(电机转子转速 rad/s)         ±804.00
-        float Pos;                      // 期望关节位置(电机转子位置 rad)           ±411774
-        float K_P;                      // 关节刚度系数                           0~25.599
-        float K_W;                      // 关节速度系数                           0~25.599
-        void modify_data(MotorCmd* motor_s); 
-        uint8_t* get_motor_send_data();
-
-    private:
-        ControlData_t  motor_send_data;  //电机控制数据结构体，详见motor_msg.h
-};
-
-struct MotorData{
-    // 定义 接收数据     
-    public:
-        MotorType motorType = MotorType::GO_M8010_6;
-        int hex_len = 16;               // 接收的命令长度: 16Byte
-        bool correct = false;           // 接收数据是否完整(true完整，false不完整或断联)
-        unsigned char motor_id;         // 电机ID 0~14 15:广播ID 此时电机无返回
-        unsigned char mode;             // 电机模式 0:刹车 1:FOC闭环 2:电机标定
-        int Temp;                       // 温度 -50~127 ℃
-        int MError;                     // 错误标志 0.正常 1.过热 2.过流 3.过压 4.编码器故障
-        float T;                        // 关节的输出力矩(电机转子转矩 N.m) 范围: ±127.99
-        float W;                        // 关节速度(电机转子转速 rad/s)         ±804.00
-        float Pos;                      // 关节位置(电机转子位置 rad)           ±411774
-        int footForce;                  // 足端气压传感器接口 ADC原始值
-        bool extract_data(MotorData* motor_r);
-        uint8_t* get_motor_recv_data();
-
-    private:
-        MotorData_t motor_recv_data;     //电机接收数据结构体，详见motor_msg.h
+enum class MotorType
+{
+        A1, // 4.8M baudrate, K_W x1024
+        B1  // 6.0M baudrate, K_W x512
 
 };
 
+struct MotorCmd
+{
+        // 定义 发送格式化数据
+public:
+        MotorType motorType;
+        int hex_len = 34;
+        unsigned short id;   //电机ID，0xBB代表全部电机
+        unsigned short mode; // 0:空闲, 5:开环转动, 10:闭环FOC控制
+        float T;             //期望关节的输出力矩（电机本身的力矩）（Nm）
+        float W;             //期望关节速度（电机本身的速度）(rad/s)
+        float Pos;           //期望关节位置（rad）
+        float K_P;           //关节刚度系数
+        float K_W;           //关节速度系数
+        void modify_data(MotorCmd *motor_s);
+        uint8_t *get_motor_send_data();
+        COMData32 Res;                    // 通讯 保留字节  用于实现别的一些通讯内容
+        MasterComdDataV3 motor_send_data; //电机控制数据结构体，详见motor_msg.h
+};
 
-#endif  // UNITREEMOTOR_H
+struct MotorData
+{
+        // 定义 接收数据
+public:
+        MotorType motorType;
+        int hex_len = 78;       //接收的16进制命令数组长度, 78
+        unsigned char motor_id; //电机ID
+        unsigned char mode;     // 0:空闲, 5:开环转动, 10:闭环FOC控制
+        int Temp;               //温度
+        int MError;             //错误码
+        float T;                // 当前实际电机输出力矩
+        float W;                // 当前实际电机速度（高速）
+        float Pos;              // 当前电机位置
+        bool correct = false;   //接收数据是否完整（true完整，false不完整）
+        bool extract_data(MotorData *motor_r);
+        uint8_t *get_motor_recv_data();
+        float LW;      // 当前实际电机速度（低速）
+        int Acc;       // 电机转子加速度
+        float gyro[3]; // 电机驱动板6轴传感器数据
+        float acc[3];
+        ServoComdDataV3 motor_recv_data; //电机接收数据结构体，详见motor_msg_A1B1.h
+};
+
+#endif // UNITREEMOTOR_H