Skip to content

Commit

Permalink
unitree motor update to current bsp_uart and add daemon
Browse files Browse the repository at this point in the history
  • Loading branch information
@jia-xie
jia-xie committed Apr 5, 2024
1 parent 9e85b14 commit 789a8f5
Show file tree
Hide file tree
Showing 3 changed files with 223 additions and 308 deletions.
1 change: 1 addition & 0 deletions Makefile
View file
Original file line number Diff line number Diff line change
Expand Up @@ -134,6 +134,7 @@ src/devices/src/remote.c \
src/devices/src/imu_task.c \
src/devices/src/referee_system.c \
src/devices/src/jetson_orin.c \
src/devices/src/unitree_motor.c \
src/app/src/motor_task.c \
src/app/src/chassis_task.c \
src/app/src/gimbal_task.c \
Expand Down
249 changes: 80 additions & 169 deletions src/devices/inc/unitree_motor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,12 @@

#include <stdint.h>
#include "main.h"
#include "bsp_uart.h"

#define UNITREE_FOC_MODE (10)
#define UNITREE_OPEN_LOOP_MODE (5)
#define UNITREE_IDLE_MODE (0)
#define UNITREE_TIMEOUT_MS (1000)

/*
* This mysterious table is just the CRC of each possible byte. It can be
Expand Down Expand Up @@ -65,48 +71,9 @@ static uint16_t crc_ccitt(uint16_t crc, uint8_t const *buffer, size_t len)
return tmp;
}

#pragma pack(1)

/**
* @brief 电机模式控制信息
*
*/
typedef struct
{
uint8_t id : 4; // 电机ID: 0,1...,13,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)
int16_t k_pos; // 期望关节刚度系数 unit: -1.0-1.0 (q15)
int16_t k_spd; // 期望关节阻尼系数 unit: -1.0-1.0 (q15)

} RIS_Comd_t; // 控制参数 12Byte

/**
* @brief 电机状态反馈信息
*
/** Motor Protocol Data Structure, Reference:
* https://support.unitree.com/home/en/Motor_SDK_Dev_Guide/communication_protocol
*/
typedef struct
{
int16_t torque; // 实际关节输出扭矩 unit: N.m (q8)
int16_t speed; // 实际关节输出速度 unit: rad/s (q8)
int32_t pos; // 实际关节输出位置 unit: rad (q15)
int8_t temp; // 电机温度: -128~127°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(1)
typedef union
{
Expand All @@ -119,153 +86,97 @@ typedef union

typedef struct
{
// 定义 数据包头
unsigned char start[2]; // 包头
unsigned char motorID; // 电机ID 0,1,2,3 ... 0xBB 表示向所有电机广播（此时无返回）
unsigned char reserved;
} COMHead;

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）
uint8_t head[2]; // Header 2Byte
struct
{
uint8_t id : 4; // Motor ID: 0,1...,13,14 15 represents broadcasting data to all motors (no return at this time)
uint8_t mode : 3; // Working mode: 0.Locked 1.FOC closed-loop 2.Encoder calibration 3.Reserved
uint8_t none : 1; // Reserved bit
} id_mode;

struct
{
int16_t torque; // Actual joint output torque unit: N.m (q8)
int16_t speed; // Actual joint output speed unit: rad/s (q8)
int32_t position; // Actual joint output position unit: rad (q15)
int8_t temp; // Motor temperature: -128~127°C
uint8_t error_code : 3; // Motor error code: 0.Normal 1.Overheat 2.Overcurrent 3.Overvoltage 4.Encoder fault 5-7.Reserved
uint16_t force : 12; // Foot pressure sensor data 12bit (0-4095)
uint8_t none : 1; // Reserved bit
} feedback; // Motor feedback data 11Byte

uint16_t CRC16; // CRC 2Byte

} Unitree_Motor_Rx_Buffer_t; // Return data

typedef struct
{
uint8_t head[2]; // 包头 2Byte
RIS_Mode_t mode; // 电机控制模式 1Byte
RIS_Fbk_t fbk; // 电机反馈数据 11Byte
uint16_t CRC16; // CRC 2Byte
} MotorData_t; // 返回数据
// Define motor control command data package
uint8_t head[2]; // Header 2Byte

typedef struct
{
uint8_t none[8]; // 保留
struct
{
uint8_t id : 4; // Motor ID: 0,1...,13,14 15 represents broadcasting data to all motors (no return at this time)
uint8_t mode : 3; // Working mode: 0.Idle 1.FOC closed-loop 2.Encoder calibration 3.Reserved
uint8_t none : 1; // Reserved bit
} id_mode; // Motor control mode 1Byte

} LowHzMotorCmd;
struct
{
int16_t tor_des; // Desired joint output torque unit: N.m (q8)
int16_t spd_des; // Desired joint output speed unit: rad/s (q8)
int32_t pos_des; // Desired joint output position unit: rad (q15)
int16_t k_pos; // Desired joint stiffness coefficient unit: -1.0-1.0 (q15)
int16_t k_spd; // Desired joint damping coefficient unit: -1.0-1.0 (q15)

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
int16_t T; // 期望关节的输出力矩（电机本身的力矩）x256, 7 + 8 描述
int16_t W; // 期望关节速度 （电机本身的速度） x128, 8 + 7描述
int32_t Pos; // 期望关节位置 x 16384/6.2832, 14位编码器（主控0点修正，电机关节还是以编码器0点为准）

int16_t K_P; // 关节刚度系数 x2048 4+11 描述
int16_t K_W; // 关节速度系数 x1024 5+10 描述

uint8_t LowHzMotorCmdIndex; // 保留
uint8_t LowHzMotorCmdByte; // 保留

COMData32 Res[1]; // 通讯 保留字节 用于实现别的一些通讯内容
} cmd;

} MasterComdV3; // 加上数据包的包头 和CRC 34字节

typedef struct
{
// 定义 电机控制命令数据包
uint8_t head[2]; // 包头 2Byte
RIS_Mode_t mode; // 电机控制模式 1Byte
RIS_Comd_t comd; // 电机期望数据 12Byte
uint16_t CRC16; // CRC 2Byte
} ControlData_t; // 电机控制命令数据包
uint16_t CRC16; // CRC 2Byte
} Unitree_Motor_Tx_Buffer; // Motor control command data package

#pragma pack()

typedef struct
{
// 定义 发送格式化数据
ControlData_t motor_send_data; // 电机控制数据结构体
int hex_len; // 发送的16进制命令数组长度, 34
long long send_time; // 发送该命令的时间, 微秒(us)
// 待发送的各项数据
unsigned short id; // 电机ID，0代表全部电机
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; // 通讯 保留字节 用于实现别的一些通讯内容
} MOTOR_send;
// Define formatted data to be sent
Unitree_Motor_Tx_Buffer tx_buffer; // Motor control data structure
int hex_len; // Length of the hexadecimal command array to be sent, 34
long long send_time; // Time when the command is sent, in microseconds (us)
// Data to be sent
unsigned short id; // Motor ID, 0 represents all motors
unsigned short mode; // 0: Idle, 5: Open-loop rotation, 10: Closed-loop FOC control
float torque; // Desired joint output torque (motor's own torque) (Nm)
float velocity; // Desired joint velocity (motor's own velocity) (rad/s)
float position; // Desired joint position (rad)
float k_pos; // Joint stiffness coefficient
float k_vel; // Joint velocity coefficient
COMData32 Res; // Communication reserved bytes, used for other communication content implementation
} Unitree_Motor_Cmd_Package_t;

typedef struct
{
// 定义 接收数据
MotorData_t motor_recv_data; // 电机接收数据结构体，详见motor_msg.h
int hex_len; // 接收的16进制命令数组长度, 78
long long resv_time; // 接收该命令的时间, 微秒(us)
int correct; // 接收数据是否完整（1完整，0不完整）
// 解读得出的电机数据
unsigned char motor_id; // 电机ID
unsigned char mode; // 0:空闲, 5:开环转动, 10:闭环FOC控制
int Temp; // 温度
unsigned char MError; // 错误码
float T; // 当前实际电机输出力矩
float W; // speed
float Pos; // 当前电机位置（主控0点修正，电机关节还是以编码器0点为准）
float footForce; // 足端气压传感器数据 12bit (0-4095)

} MOTOR_recv;
int hex_len; // Length of the hexadecimal command array received, 78
long long resv_time; // Time when the command is received, in microseconds (us)
int correct; // Whether the received data is complete (1 for complete, 0 for incomplete)
// Motor data obtained from interpretation
unsigned char motor_id; // Motor ID
unsigned char mode; // 0: Idle, 5: Open-loop rotation, 10: Closed-loop FOC control
int Temp; // Temperature
unsigned char error_code; // Error code
float torque; // Current actual motor output torque
float velocity; // speed
float position; // Current motor position (based on encoder zero point correction, the motor joint is based on the encoder zero point)
float foot_force; // Foot pressure sensor data 12bit (0-4095)

} Unitree_Motor_Stats_t;

typedef struct _Unitree_Motor
{
uint8_t uart;
Unitree_Motor_Cmd_Package_t cmd;
Unitree_Motor_Stats_t stats;
UART_Instance_t *uart_instance;
uint8_t motor_id;
MOTOR_send motor_send;
MOTOR_recv motor_recv;
} Unitree_Motor;

uint32_t crc32_core(uint32_t *ptr, uint32_t len);
int modify_data(MOTOR_send *motor_s);
int extract_data(MOTOR_recv *motor_r);
HAL_StatusTypeDef SERVO_Send_recv(MOTOR_send *pData, MOTOR_recv *rData);
} Unitree_Motor_t;

#pragma pack()

Expand Down
Loading

0 comments on commit 789a8f5

Please sign in to comment.