diff --git a/CMakeLists.txt b/CMakeLists.txt index c840b11..54210d8 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,12 +1,13 @@ -cmake_minimum_required(VERSION 3.10.2) -project(UnitreeMotorSDK_M80106) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3") +cmake_minimum_required(VERSION 2.8.3) +project(UnitreeMotorA1B1) + +include_directories( + /home/$ENV{USER}/unitree_actuator_sdk/include/ +) -include_directories("include/") link_directories( lib/ ) -#example add_executable(motorctrl example/main.cpp) -target_link_libraries(motorctrl libUnitreeMotorSDK_M80106_Linux64.so) +target_link_libraries(motorctrl libunitreeMotorSDK_Linux64.so) \ No newline at end of file diff --git a/README.md b/README.md index 5546500..45b6569 100644 --- a/README.md +++ b/README.md @@ -2,9 +2,9 @@ ### 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) ### Build ```bash diff --git a/example/changeID.cpp b/example/changeID.cpp new file mode 100755 index 0000000..cc09380 --- /dev/null +++ b/example/changeID.cpp @@ -0,0 +1,55 @@ +#include +#include +#include +#include +#include "serialPort/SerialPort.h" + +#define BroadAllMotorID 0xBB +#define MotorPulsator 11 + +int main(){ + char serial_name[100]; + + MOTOR_send motor_s; + MOTOR_recv motor_r; + + motor_s.motorType = MotorType::B1; // set the motor type, A1 or B1 + motor_r.motorType = motor_s.motorType; + + 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(&motor_s), 0, sizeof(motor_s)); + motor_s.id = BroadAllMotorID; + motor_s.mode = 10; + modify_data(&motor_s); + // printf("The motor ID is: %X\n", motor_s.motor_send_data.head.motorID); + + //进入伺服模式 + SerialPort serial(serial_name); // set the serial port name + serial.send((uint8_t*)&(motor_s.motor_send_data), motor_s.hex_len); + + usleep(100000); //sleep 0.1s + + //进入拨轮模式（修改ID） + motor_s.mode = MotorPulsator; + modify_data(&motor_s); + serial.send((uint8_t*)&(motor_s.motor_send_data), motor_s.hex_len); + + 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; + modify_data(&motor_s); + serial.send((uint8_t*)&(motor_s.motor_send_data), motor_s.hex_len); + + return 0; +} diff --git a/example/main.cpp b/example/main.cpp index 513352c..4ab1bb5 100644 --- a/example/main.cpp +++ b/example/main.cpp @@ -1,36 +1,49 @@ #include "serialPort/SerialPort.h" -#include +#include -int main() { +int main(){ + // set the serial port name + SerialPort serial("/dev/ttyUSB0"); - SerialPort serial("/dev/ttyUSB0"); - MotorCmd cmd; - MotorData data; + // send message struct + MOTOR_send motor_run, motor_stop; + // receive message struct + MOTOR_recv motor_r; - 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); + // set the id of motor + motor_run.id = 0; + // set the motor type, A1 or B1 + motor_run.motorType = MotorType::A1; + motor_run.mode = 5; + motor_run.T = 0.0; + motor_run.W = 0.0; + motor_run.Pos = 0.0; + motor_run.K_P = 0.0; + motor_run.K_W = 0.0; - 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"< + +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 \ No newline at end of file diff --git a/include/IOPort/IOPort.h b/include/IOPort/IOPort.h index 173b617..4b5aaa4 100755 --- a/include/IOPort/IOPort.h +++ b/include/IOPort/IOPort.h @@ -1,5 +1,5 @@ -#ifndef __IOPORT_H -#define __IOPORT_H +#ifndef BIANLIB_IOPORT_H +#define BIANLIB_IOPORT_H #include #include @@ -13,20 +13,16 @@ enum class BlockYN{ class IOPort{ public: - IOPort(BlockYN blockYN, size_t recvLength, size_t timeOutUs){ - resetIO(blockYN, recvLength, timeOutUs); - } + IOPort(){} 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 &sendVec, std::vector &recvVec) = 0; + virtual bool sendRecv(std::vector &sendVec, std::vector &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){ @@ -34,8 +30,7 @@ inline void IOPort::resetIO(BlockYN blockYN, size_t recvLength, size_t timeOutUs _recvLength = recvLength; _timeout.tv_sec = timeOutUs / 1000000; _timeout.tv_usec = timeOutUs % 1000000; - _timeoutSaved = _timeout; } -#endif // z1_lib_IOPORT_H \ No newline at end of file +#endif // BIANLIB_IOPORT_H \ No newline at end of file diff --git a/include/crc/crc_ccitt.h b/include/crc/crc_ccitt.h deleted file mode 100644 index 2930e24..0000000 --- a/include/crc/crc_ccitt.h +++ /dev/null @@ -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 diff --git a/include/serialPort/SerialPort.h b/include/serialPort/SerialPort.h index 7bfe5e9..880e4c4 100755 --- a/include/serialPort/SerialPort.h +++ b/include/serialPort/SerialPort.h @@ -1,18 +1,20 @@ -#ifndef __SERIALPORT_H -#define __SERIALPORT_H +#ifndef SERIALPORT_H +#define SERIALPORT_H /* High frequency serial communication, Not that common, but useful for motor communication. */ #include +// #include #include + #include #include #include #include #include -#include +#include //serial驱动相关 #include #include @@ -47,20 +49,22 @@ enum class flowcontrol_t{ flowcontrol_hardware }; + + class SerialPort : public IOPort{ public: SerialPort(const std::string &portName, - size_t recvLength = 16, - uint32_t baudrate = 4000000, - size_t timeOutUs = 20000, + 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, @@ -68,17 +72,16 @@ public: 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 &sendVec, std::vector &recvVec); + bool sendRecv(MOTOR_send* sendMsg, MOTOR_recv* recvMsg); + bool sendRecv(std::vector &sendVec, std::vector &recvVec); private: void _open(); void _set(); void _close(); - size_t _nonBlockRecv(uint8_t *recvMsg, size_t readLen); + size_t _nonBlockRecv(uint8_t *recvMsg, size_t readLen); // only visual in pubSDK std::string _portName; uint32_t _baudrate; bytesize_t _bytesize; @@ -87,12 +90,11 @@ private: flowcontrol_t _flowcontrol; bool _xonxoff; bool _rtscts; + int _fd; fd_set _rSet; }; - - #endif // SERIALPORT_H \ No newline at end of file diff --git a/include/serialPort/include/errorClass.h b/include/serialPort/include/errorClass.h index 48313c3..92af50b 100755 --- a/include/serialPort/include/errorClass.h +++ b/include/serialPort/include/errorClass.h @@ -1,5 +1,5 @@ -#ifndef __ERRORCLASS_H -#define __ERRORCLASS_H +#ifndef ERRORCLASS_H +#define ERRORCLASS_H #include #include diff --git a/include/unitreeMotor/include/motor_msg.h b/include/unitreeMotor/include/motor_msg.h index ef57ff0..168b589 100755 --- a/include/unitreeMotor/include/motor_msg.h +++ b/include/unitreeMotor/include/motor_msg.h @@ -1,90 +1,162 @@ -#ifndef __MOTOR_MSG_H -#define __MOTOR_MSG_H +#ifndef MOTOR_MSG +#define MOTOR_MSG #include -#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 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 +// 发送用单个数据数据结构 +typedef union{ + int32_t L; + uint8_t u8[4]; + uint16_t u16[2]; + uint32_t u32; + float F; +}COMData32; -/** - * @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) +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度分辨率 -} RIS_Comd_t; // 控制参数 12Byte + int16_t Force16; // 力传感器高16位数据 + int8_t Force8; // 力传感器低8位数据 + + uint8_t FError; // 足端传感器错误标识 + + int8_t Res[1]; // 通讯 保留字节 + +}ServoComdV3; // 加上数据包的包头 和CRC 78字节（4+70+4） -/** - * @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 +typedef struct { + // 定义 电机控制命令数据包 + COMHead head; + ServoComdV3 Mdata; + + COMData32 CRCdata; + +}ServoComdDataV3; //发送数据 + +// typedef struct { +// // 定义 总的485 接受数据包 + +// ServoComdDataV3 M[3]; +// // uint8_t nullbyte1; + +// }DMA485RxDataV3; #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 - - - - +// 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 \ No newline at end of file diff --git a/include/unitreeMotor/unitreeMotor.h b/include/unitreeMotor/unitreeMotor.h index d6deb87..8964299 100755 --- a/include/unitreeMotor/unitreeMotor.h +++ b/include/unitreeMotor/unitreeMotor.h @@ -1,54 +1,116 @@ -#ifndef __UNITREEMOTOR_H -#define __UNITREEMOTOR_H +#ifndef UNITREEMOTOR_H +#define UNITREEMOTOR_H #include "unitreeMotor/include/motor_msg.h" // 电机通信协议 +#include "CRC/crc32.h" // CRC32校验算法 #include #include enum class MotorType{ - GO_M8010_6, + A1, // 4.8M baudrate, K_W x1024 + B1 // 6.0M baudrate, K_W x512 }; -struct MotorCmd{ +struct MOTOR_send{ // 定义 发送格式化数据 - 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 + MasterComdDataV3 motor_send_data; //电机控制数据结构体，详见motor_msg.h + MotorType motorType = MotorType::A1; + int hex_len = 34; //发送的16进制命令数组长度, 34 + // long long send_time; //发送该命令的时间, 微秒(us) + // 待发送的各项数据 + unsigned short id; //电机ID，0xBB代表全部电机 + unsigned short mode; //0:空闲, 5:开环转动, 10:闭环FOC控制 + //实际给FOC的指令力矩为： + //K_P*delta_Pos + K_W*delta_W + T + float T; //期望关节的输出力矩（电机本身的力矩）（Nm） + float W; //期望关节速度（电机本身的速度）(rad/s) + float Pos; //期望关节位置（rad） + float K_P; //关节刚度系数 + float K_W; //关节速度系数 + COMData32 Res; // 通讯 保留字节 用于实现别的一些通讯内容 }; -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(); +struct MOTOR_recv{ + // 定义 接收数据 + ServoComdDataV3 motor_recv_data; //电机接收数据结构体，详见motor_msg.h + MotorType motorType = MotorType::A1; + int hex_len = 78; //接收的16进制命令数组长度, 78 + // long long resv_time; //接收该命令的时间, 微秒(us) + bool correct = false; //接收数据是否完整（true完整，false不完整） + //解读得出的电机数据 + unsigned char motor_id; //电机ID + unsigned char mode; //0:空闲, 5:开环转动, 10:闭环FOC控制 + int Temp; //温度 + unsigned char MError; //错误码 - private: - MotorData_t motor_recv_data; //电机接收数据结构体，详见motor_msg.h + float T; // 当前实际电机输出力矩 + float W; // 当前实际电机速度（高速） + float LW; // 当前实际电机速度（低速） + int Acc; // 电机转子加速度 + float Pos; // 当前电机位置（主控0点修正，电机关节还是以编码器0点为准） + float gyro[3]; // 电机驱动板6轴传感器数据 + float acc[3]; }; +inline void modify_data(MOTOR_send* motor_s){ + motor_s->hex_len = 34; + motor_s->motor_send_data.head.start[0] = 0xFE; + motor_s->motor_send_data.head.start[1] = 0xEE; + motor_s->motor_send_data.head.motorID = motor_s->id; + motor_s->motor_send_data.head.reserved = 0x0; + motor_s->motor_send_data.Mdata.mode = motor_s->mode; + motor_s->motor_send_data.Mdata.ModifyBit = 0xFF; + motor_s->motor_send_data.Mdata.ReadBit = 0x0; + motor_s->motor_send_data.Mdata.reserved = 0x0; + motor_s->motor_send_data.Mdata.Modify.L = 0; + motor_s->motor_send_data.Mdata.T = motor_s->T*256; + motor_s->motor_send_data.Mdata.W = motor_s->W*128; + motor_s->motor_send_data.Mdata.Pos = (int)((motor_s->Pos/6.2832)*16384.0); + motor_s->motor_send_data.Mdata.K_P = motor_s->K_P*2048; + + if(motor_s->motorType == MotorType::A1){ + motor_s->motor_send_data.Mdata.K_W = motor_s->K_W*1024; + } + else if(motor_s->motorType == MotorType::B1){ + motor_s->motor_send_data.Mdata.K_W = motor_s->K_W*512; + } + + motor_s->motor_send_data.Mdata.LowHzMotorCmdIndex = 0; + motor_s->motor_send_data.Mdata.LowHzMotorCmdByte = 0; + motor_s->motor_send_data.Mdata.Res[0] = motor_s->Res; + motor_s->motor_send_data.CRCdata.u32 = crc32_core((uint32_t*)(&(motor_s->motor_send_data)), 7); +} + +inline bool extract_data(MOTOR_recv* motor_r){ + if(motor_r->motor_recv_data.CRCdata.u32 != + crc32_core((uint32_t*)(&(motor_r->motor_recv_data)), 18)){ + std::cout << "[WARNING] Receive data CRC error" << std::endl; + motor_r->correct = false; + return motor_r->correct; + }else{ + motor_r->motor_id = motor_r->motor_recv_data.head.motorID; + motor_r->mode = motor_r->motor_recv_data.Mdata.mode; + motor_r->Temp = motor_r->motor_recv_data.Mdata.Temp; + motor_r->MError = motor_r->motor_recv_data.Mdata.MError; + motor_r->T = ((float)motor_r->motor_recv_data.Mdata.T) / 256; + motor_r->W = ((float)motor_r->motor_recv_data.Mdata.W) / 128; + motor_r->LW = motor_r->motor_recv_data.Mdata.LW; + + motor_r->Acc = (int)motor_r->motor_recv_data.Mdata.Acc; + motor_r->Pos = 6.2832*((float)motor_r->motor_recv_data.Mdata.Pos) / 16384; + + motor_r->gyro[0] = ((float)motor_r->motor_recv_data.Mdata.gyro[0]) * 0.00107993176; + motor_r->gyro[1] = ((float)motor_r->motor_recv_data.Mdata.gyro[1]) * 0.00107993176; + motor_r->gyro[2] = ((float)motor_r->motor_recv_data.Mdata.gyro[2]) * 0.00107993176; + + motor_r->acc[0] = ((float)motor_r->motor_recv_data.Mdata.acc[0]) * 0.0023911132; + motor_r->acc[1] = ((float)motor_r->motor_recv_data.Mdata.acc[1]) * 0.0023911132; + motor_r->acc[2] = ((float)motor_r->motor_recv_data.Mdata.acc[2]) * 0.0023911132; + + motor_r->correct = true; + return motor_r->correct; + } +} #endif // UNITREEMOTOR_H \ No newline at end of file diff --git a/lib/libUnitreeMotorSDK_M80106_Arm64.so b/lib/libUnitreeMotorSDK_M80106_Arm64.so deleted file mode 100755 index 576a40a..0000000 Binary files a/lib/libUnitreeMotorSDK_M80106_Arm64.so and /dev/null differ diff --git a/lib/libUnitreeMotorSDK_M80106_Linux64.so b/lib/libUnitreeMotorSDK_M80106_Linux64.so deleted file mode 100755 index 553fe12..0000000 Binary files a/lib/libUnitreeMotorSDK_M80106_Linux64.so and /dev/null differ diff --git a/lib/libunitreeMotorSDK_Linux64.so b/lib/libunitreeMotorSDK_Linux64.so new file mode 100755 index 0000000..4dd6a1a Binary files /dev/null and b/lib/libunitreeMotorSDK_Linux64.so differ