配置can
This commit is contained in:
parent
00cfee37c7
commit
3f43126f13
@ -63,6 +63,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
|
|||||||
User/device/bmi088.c
|
User/device/bmi088.c
|
||||||
User/device/dr16.c
|
User/device/dr16.c
|
||||||
User/device/motor.c
|
User/device/motor.c
|
||||||
|
User/device/motor_lz.c
|
||||||
|
|
||||||
# User/module sources
|
# User/module sources
|
||||||
User/module/config.c
|
User/module/config.c
|
||||||
|
|||||||
79
User/bsp/can.h
Normal file
79
User/bsp/can.h
Normal file
@ -0,0 +1,79 @@
|
|||||||
|
/**
|
||||||
|
* @file can.h
|
||||||
|
* @brief CAN兼容层 - 将CAN接口映射到FDCAN
|
||||||
|
* @note 本文件用于FDCAN兼容CAN接口，设备层代码可以继续使用BSP_CAN_xxx接口
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* Includes ----------------------------------------------------------------- */
|
||||||
|
#include "bsp/fdcan.h"
|
||||||
|
|
||||||
|
/* 类型映射 */
|
||||||
|
typedef BSP_FDCAN_t BSP_CAN_t;
|
||||||
|
typedef BSP_FDCAN_Callback_t BSP_CAN_Callback_t;
|
||||||
|
typedef BSP_FDCAN_Format_t BSP_CAN_Format_t;
|
||||||
|
typedef BSP_FDCAN_FrameType_t BSP_CAN_FrameType_t;
|
||||||
|
typedef BSP_FDCAN_Message_t BSP_CAN_Message_t;
|
||||||
|
typedef BSP_FDCAN_StdDataFrame_t BSP_CAN_StdDataFrame_t;
|
||||||
|
typedef BSP_FDCAN_ExtDataFrame_t BSP_CAN_ExtDataFrame_t;
|
||||||
|
typedef BSP_FDCAN_RemoteFrame_t BSP_CAN_RemoteFrame_t;
|
||||||
|
typedef BSP_FDCAN_IdParser_t BSP_CAN_IdParser_t;
|
||||||
|
|
||||||
|
/* 常量映射 */
|
||||||
|
#define BSP_CAN_MAX_DLC BSP_FDCAN_MAX_DLC
|
||||||
|
#define BSP_CAN_DEFAULT_QUEUE_SIZE BSP_FDCAN_DEFAULT_QUEUE_SIZE
|
||||||
|
#define BSP_CAN_TIMEOUT_IMMEDIATE BSP_FDCAN_TIMEOUT_IMMEDIATE
|
||||||
|
#define BSP_CAN_TIMEOUT_FOREVER BSP_FDCAN_TIMEOUT_FOREVER
|
||||||
|
#define BSP_CAN_TX_QUEUE_SIZE BSP_FDCAN_TX_QUEUE_SIZE
|
||||||
|
|
||||||
|
/* 枚举值映射 */
|
||||||
|
#define BSP_CAN_1 BSP_FDCAN_1
|
||||||
|
#define BSP_CAN_2 BSP_FDCAN_2
|
||||||
|
#define BSP_CAN_3 BSP_FDCAN_3
|
||||||
|
#define BSP_CAN_NUM BSP_FDCAN_NUM
|
||||||
|
#define BSP_CAN_ERR BSP_FDCAN_ERR
|
||||||
|
|
||||||
|
#define BSP_CAN_FORMAT_STD_DATA BSP_FDCAN_FORMAT_STD_DATA
|
||||||
|
#define BSP_CAN_FORMAT_EXT_DATA BSP_FDCAN_FORMAT_EXT_DATA
|
||||||
|
#define BSP_CAN_FORMAT_STD_REMOTE BSP_FDCAN_FORMAT_STD_REMOTE
|
||||||
|
#define BSP_CAN_FORMAT_EXT_REMOTE BSP_FDCAN_FORMAT_EXT_REMOTE
|
||||||
|
|
||||||
|
#define BSP_CAN_FRAME_STD_DATA BSP_FDCAN_FRAME_STD_DATA
|
||||||
|
#define BSP_CAN_FRAME_EXT_DATA BSP_FDCAN_FRAME_EXT_DATA
|
||||||
|
#define BSP_CAN_FRAME_STD_REMOTE BSP_FDCAN_FRAME_STD_REMOTE
|
||||||
|
#define BSP_CAN_FRAME_EXT_REMOTE BSP_FDCAN_FRAME_EXT_REMOTE
|
||||||
|
|
||||||
|
/* 函数映射 */
|
||||||
|
#define BSP_CAN_Init() BSP_FDCAN_Init()
|
||||||
|
#define BSP_CAN_GetHandle(can) BSP_FDCAN_GetHandle(can)
|
||||||
|
#define BSP_CAN_RegisterCallback(can, type, callback) \
|
||||||
|
BSP_FDCAN_RegisterCallback(can, type, callback)
|
||||||
|
#define BSP_CAN_Transmit(can, format, id, data, dlc) \
|
||||||
|
BSP_FDCAN_Transmit(can, format, id, data, dlc)
|
||||||
|
#define BSP_CAN_TransmitStdDataFrame(can, frame) \
|
||||||
|
BSP_FDCAN_TransmitStdDataFrame(can, frame)
|
||||||
|
#define BSP_CAN_TransmitExtDataFrame(can, frame) \
|
||||||
|
BSP_FDCAN_TransmitExtDataFrame(can, frame)
|
||||||
|
#define BSP_CAN_TransmitRemoteFrame(can, frame) \
|
||||||
|
BSP_FDCAN_TransmitRemoteFrame(can, frame)
|
||||||
|
#define BSP_CAN_RegisterId(can, can_id, queue_size) \
|
||||||
|
BSP_FDCAN_RegisterId(can, can_id, queue_size)
|
||||||
|
#define BSP_CAN_GetMessage(can, can_id, msg, timeout) \
|
||||||
|
BSP_FDCAN_GetMessage(can, can_id, msg, timeout)
|
||||||
|
#define BSP_CAN_GetQueueCount(can, can_id) \
|
||||||
|
BSP_FDCAN_GetQueueCount(can, can_id)
|
||||||
|
#define BSP_CAN_FlushQueue(can, can_id) \
|
||||||
|
BSP_FDCAN_FlushQueue(can, can_id)
|
||||||
|
#define BSP_CAN_RegisterIdParser(parser) \
|
||||||
|
BSP_FDCAN_RegisterIdParser(parser)
|
||||||
|
#define BSP_CAN_ParseId(original_id, frame_type) \
|
||||||
|
BSP_FDCAN_ParseId(original_id, frame_type)
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
@ -13,3 +13,6 @@ dr16:
|
|||||||
motor:
|
motor:
|
||||||
bsp_config: {}
|
bsp_config: {}
|
||||||
enabled: true
|
enabled: true
|
||||||
|
motor_lz:
|
||||||
|
bsp_config: {}
|
||||||
|
enabled: true
|
||||||
|
|||||||
440
User/device/motor_lz.c
Normal file
440
User/device/motor_lz.c
Normal file
@ -0,0 +1,440 @@
|
|||||||
|
/*
|
||||||
|
灵足电机驱动
|
||||||
|
|
||||||
|
灵足电机通信协议：
|
||||||
|
- CAN 2.0通信接口，波特率1Mbps
|
||||||
|
- 采用扩展帧格式(29位ID)
|
||||||
|
- ID格式：Bit28~24(通信类型) + Bit23~8(数据区2) + Bit7~0(目标地址)
|
||||||
|
*/
|
||||||
|
/* Includes ----------------------------------------------------------------- */
|
||||||
|
#include "motor_lz.h"
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <stdbool.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include "bsp/can.h"
|
||||||
|
#include "bsp/mm.h"
|
||||||
|
#include "bsp/time.h"
|
||||||
|
#include "component/user_math.h"
|
||||||
|
|
||||||
|
/* Private define ----------------------------------------------------------- */
|
||||||
|
// 灵足电机协议参数
|
||||||
|
#define LZ_ANGLE_RANGE_RAD (12.57f) /* 角度范围 ±12.57 rad */
|
||||||
|
#define LZ_VELOCITY_RANGE_RAD_S (20.0f) /* 角速度范围 ±20 rad/s */
|
||||||
|
#define LZ_TORQUE_RANGE_NM (60.0f) /* 力矩范围 ±60 Nm */
|
||||||
|
#define LZ_KP_MAX (5000.0f) /* Kp最大值 */
|
||||||
|
#define LZ_KD_MAX (100.0f) /* Kd最大值 */
|
||||||
|
|
||||||
|
#define LZ_RAW_VALUE_MAX (65535) /* 16位原始值最大值 */
|
||||||
|
#define LZ_TEMP_SCALE (10.0f) /* 温度缩放因子 */
|
||||||
|
|
||||||
|
#define LZ_MAX_RECOVER_DIFF_RAD (0.28f)
|
||||||
|
#define MOTOR_TX_BUF_SIZE (8)
|
||||||
|
#define MOTOR_RX_BUF_SIZE (8)
|
||||||
|
|
||||||
|
/* Private macro ------------------------------------------------------------ */
|
||||||
|
|
||||||
|
MOTOR_LZ_MotionParam_t lz_relax_param = {
|
||||||
|
.target_angle = 0.0f,
|
||||||
|
.target_velocity = 0.0f,
|
||||||
|
.kp = 0.0f,
|
||||||
|
.kd = 0.0f,
|
||||||
|
.torque = 0.0f,
|
||||||
|
};
|
||||||
|
/* Private typedef ---------------------------------------------------------- */
|
||||||
|
/* Private variables -------------------------------------------------------- */
|
||||||
|
static MOTOR_LZ_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
|
||||||
|
|
||||||
|
/* Private function prototypes ---------------------------------------------- */
|
||||||
|
static MOTOR_LZ_CANManager_t* MOTOR_LZ_GetCANManager(BSP_CAN_t can);
|
||||||
|
static int8_t MOTOR_LZ_CreateCANManager(BSP_CAN_t can);
|
||||||
|
static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg);
|
||||||
|
static uint32_t MOTOR_LZ_BuildExtID(MOTOR_LZ_CmdType_t cmd_type, uint16_t data2, uint8_t target_id);
|
||||||
|
static uint16_t MOTOR_LZ_FloatToRaw(float value, float max_value);
|
||||||
|
static float MOTOR_LZ_RawToFloat(uint16_t raw_value, float max_value);
|
||||||
|
static int8_t MOTOR_LZ_SendExtFrame(BSP_CAN_t can, uint32_t ext_id, uint8_t *data, uint8_t dlc);
|
||||||
|
static uint32_t MOTOR_LZ_IdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
|
||||||
|
|
||||||
|
/* Private functions -------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 获取CAN管理器
|
||||||
|
*/
|
||||||
|
static MOTOR_LZ_CANManager_t* MOTOR_LZ_GetCANManager(BSP_CAN_t can) {
|
||||||
|
if (can >= BSP_CAN_NUM) return NULL;
|
||||||
|
return can_managers[can];
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 创建CAN管理器
|
||||||
|
*/
|
||||||
|
static int8_t MOTOR_LZ_CreateCANManager(BSP_CAN_t can) {
|
||||||
|
if (can >= BSP_CAN_NUM) return DEVICE_ERR;
|
||||||
|
if (can_managers[can] != NULL) return DEVICE_OK;
|
||||||
|
|
||||||
|
can_managers[can] = (MOTOR_LZ_CANManager_t*)BSP_Malloc(sizeof(MOTOR_LZ_CANManager_t));
|
||||||
|
if (can_managers[can] == NULL) return DEVICE_ERR;
|
||||||
|
|
||||||
|
memset(can_managers[can], 0, sizeof(MOTOR_LZ_CANManager_t));
|
||||||
|
can_managers[can]->can = can;
|
||||||
|
return DEVICE_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 构建扩展ID
|
||||||
|
*/
|
||||||
|
static uint32_t MOTOR_LZ_BuildExtID(MOTOR_LZ_CmdType_t cmd_type, uint16_t data2, uint8_t target_id) {
|
||||||
|
uint32_t ext_id = 0;
|
||||||
|
ext_id |= ((uint32_t)cmd_type & 0x1F) << 24; // Bit28~24: 通信类型
|
||||||
|
ext_id |= ((uint32_t)data2 & 0xFFFF) << 8; // Bit23~8: 数据区2
|
||||||
|
ext_id |= ((uint32_t)target_id & 0xFF); // Bit7~0: 目标地址
|
||||||
|
return ext_id;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 浮点值转换为原始值（对称范围：-max_value ~ +max_value）
|
||||||
|
*/
|
||||||
|
static uint16_t MOTOR_LZ_FloatToRaw(float value, float max_value) {
|
||||||
|
// 限制范围
|
||||||
|
if (value > max_value) value = max_value;
|
||||||
|
if (value < -max_value) value = -max_value;
|
||||||
|
|
||||||
|
// 转换为0~65535范围，对应-max_value~max_value
|
||||||
|
return (uint16_t)((value + max_value) / (2.0f * max_value) * (float)LZ_RAW_VALUE_MAX);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 浮点值转换为原始值（单向范围：0 ~ +max_value）
|
||||||
|
*/
|
||||||
|
static uint16_t MOTOR_LZ_FloatToRawPositive(float value, float max_value) {
|
||||||
|
// 限制范围
|
||||||
|
if (value > max_value) value = max_value;
|
||||||
|
if (value < 0.0f) value = 0.0f;
|
||||||
|
|
||||||
|
// 转换为0~65535范围，对应0~max_value
|
||||||
|
return (uint16_t)(value / max_value * (float)LZ_RAW_VALUE_MAX);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 原始值转换为浮点值
|
||||||
|
*/
|
||||||
|
static float MOTOR_LZ_RawToFloat(uint16_t raw_value, float max_value) {
|
||||||
|
// 将0~65535范围转换为-max_value~max_value
|
||||||
|
return ((float)raw_value / (float)LZ_RAW_VALUE_MAX) * (2.0f * max_value) - max_value;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 发送扩展帧
|
||||||
|
*/
|
||||||
|
static int8_t MOTOR_LZ_SendExtFrame(BSP_CAN_t can, uint32_t ext_id, uint8_t *data, uint8_t dlc) {
|
||||||
|
BSP_CAN_ExtDataFrame_t tx_frame;
|
||||||
|
tx_frame.id = ext_id;
|
||||||
|
tx_frame.dlc = dlc;
|
||||||
|
if (data != NULL) {
|
||||||
|
memcpy(tx_frame.data, data, dlc);
|
||||||
|
} else {
|
||||||
|
memset(tx_frame.data, 0, dlc);
|
||||||
|
}
|
||||||
|
return BSP_CAN_TransmitExtDataFrame(can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 灵足电机ID解析器
|
||||||
|
* @param original_id 原始CAN ID（29位扩展帧）
|
||||||
|
* @param frame_type 帧类型
|
||||||
|
* @return 解析后的ID（用于队列匹配）
|
||||||
|
*
|
||||||
|
* 灵足电机扩展ID格式：
|
||||||
|
* Bit28~24: 通信类型 (0x1=运控控制, 0x2=反馈数据, 0x3=使能, 0x4=停止, 0x6=设零位)
|
||||||
|
* Bit23~8: 数据区2 (根据通信类型而定)
|
||||||
|
* Bit7~0: 目标地址 (目标电机CAN ID)
|
||||||
|
*/
|
||||||
|
static uint32_t MOTOR_LZ_IdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type) {
|
||||||
|
// 只处理扩展数据帧
|
||||||
|
if (frame_type != BSP_CAN_FRAME_EXT_DATA) {
|
||||||
|
return original_id; // 非扩展帧直接返回原始ID
|
||||||
|
}
|
||||||
|
|
||||||
|
// 解析扩展ID各个字段
|
||||||
|
uint8_t cmd_type = (original_id >> 24) & 0x1F; // Bit28~24: 通信类型
|
||||||
|
uint16_t data2 = (original_id >> 8) & 0xFFFF; // Bit23~8: 数据区2
|
||||||
|
uint8_t host_id = (uint8_t)(original_id & 0xFF); // Bit7~0: 主机CAN ID
|
||||||
|
|
||||||
|
// 对于反馈数据帧，我们使用特殊的解析规则
|
||||||
|
if (cmd_type == MOTOR_LZ_CMD_FEEDBACK) {
|
||||||
|
// 反馈数据的data2字段包含：
|
||||||
|
// Bit8~15: 当前电机CAN ID
|
||||||
|
// Bit16~21: 故障信息
|
||||||
|
// Bit22~23: 模式状态
|
||||||
|
uint8_t motor_can_id = data2 & 0xFF; // bit8~15: 当前电机CAN ID
|
||||||
|
|
||||||
|
// 返回格式化的ID，便于匹配
|
||||||
|
// 格式：0x02HHMMTT (02=反馈命令, HH=主机ID, MM=电机ID, TT=主机ID)
|
||||||
|
return (0x02000000) | (host_id << 16) | (motor_can_id << 8) | host_id;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 对于其他命令类型，直接返回原始ID
|
||||||
|
return original_id;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 解码灵足电机反馈数据
|
||||||
|
*/
|
||||||
|
static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg) {
|
||||||
|
if (motor == NULL || msg == NULL) return;
|
||||||
|
uint8_t cmd_type = (msg->original_id >> 24) & 0x1F;
|
||||||
|
if (cmd_type != MOTOR_LZ_CMD_FEEDBACK) return;
|
||||||
|
uint16_t id_data2 = (msg->original_id >> 8) & 0xFFFF;
|
||||||
|
uint8_t motor_can_id = id_data2 & 0xFF;
|
||||||
|
uint8_t fault_info = (id_data2 >> 8) & 0x3F;
|
||||||
|
uint8_t mode_state = (id_data2 >> 14) & 0x03;
|
||||||
|
motor->lz_feedback.motor_can_id = motor_can_id;
|
||||||
|
motor->lz_feedback.fault.under_voltage = (fault_info & 0x01) != 0;
|
||||||
|
motor->lz_feedback.fault.driver_fault = (fault_info & 0x02) != 0;
|
||||||
|
motor->lz_feedback.fault.over_temp = (fault_info & 0x04) != 0;
|
||||||
|
motor->lz_feedback.fault.encoder_fault = (fault_info & 0x08) != 0;
|
||||||
|
motor->lz_feedback.fault.stall_overload = (fault_info & 0x10) != 0;
|
||||||
|
motor->lz_feedback.fault.uncalibrated = (fault_info & 0x20) != 0;
|
||||||
|
motor->lz_feedback.state = (MOTOR_LZ_State_t)mode_state;
|
||||||
|
|
||||||
|
// 反馈解码并自动反向
|
||||||
|
uint16_t raw_angle = (uint16_t)((msg->data[0] << 8) | msg->data[1]);
|
||||||
|
float angle = MOTOR_LZ_RawToFloat(raw_angle, LZ_ANGLE_RANGE_RAD);
|
||||||
|
uint16_t raw_velocity = (uint16_t)((msg->data[2] << 8) | msg->data[3]);
|
||||||
|
float velocity = MOTOR_LZ_RawToFloat(raw_velocity, LZ_VELOCITY_RANGE_RAD_S);
|
||||||
|
uint16_t raw_torque = (uint16_t)((msg->data[4] << 8) | msg->data[5]);
|
||||||
|
float torque = MOTOR_LZ_RawToFloat(raw_torque, LZ_TORQUE_RANGE_NM);
|
||||||
|
|
||||||
|
while (angle <0){
|
||||||
|
angle += M_2PI;
|
||||||
|
}
|
||||||
|
while (angle > M_2PI){
|
||||||
|
angle -= M_2PI;
|
||||||
|
}
|
||||||
|
// 自动反向
|
||||||
|
if (motor->param.reverse) {
|
||||||
|
angle = M_2PI - angle;
|
||||||
|
velocity = -velocity;
|
||||||
|
torque = -torque;
|
||||||
|
}
|
||||||
|
|
||||||
|
motor->lz_feedback.current_angle = angle;
|
||||||
|
motor->lz_feedback.current_velocity = velocity;
|
||||||
|
motor->lz_feedback.current_torque = torque;
|
||||||
|
|
||||||
|
uint16_t raw_temp = (uint16_t)((msg->data[6] << 8) | msg->data[7]);
|
||||||
|
motor->lz_feedback.temperature = (float)raw_temp / LZ_TEMP_SCALE;
|
||||||
|
|
||||||
|
motor->motor.feedback.rotor_abs_angle = angle;
|
||||||
|
motor->motor.feedback.rotor_speed = velocity;
|
||||||
|
motor->motor.feedback.torque_current = torque;
|
||||||
|
motor->motor.feedback.temp = (int8_t)motor->lz_feedback.temperature;
|
||||||
|
motor->motor.header.online = true;
|
||||||
|
motor->motor.header.last_online_time = BSP_TIME_Get();
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Exported functions ------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 初始化灵足电机驱动系统
|
||||||
|
* @return 设备状态码
|
||||||
|
*/
|
||||||
|
int8_t MOTOR_LZ_Init(void) {
|
||||||
|
// 注册灵足电机专用的ID解析器
|
||||||
|
return BSP_CAN_RegisterIdParser(MOTOR_LZ_IdParser) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
int8_t MOTOR_LZ_Register(MOTOR_LZ_Param_t *param) {
|
||||||
|
if (param == NULL) return DEVICE_ERR_NULL;
|
||||||
|
|
||||||
|
if (MOTOR_LZ_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR;
|
||||||
|
MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can);
|
||||||
|
if (manager == NULL) return DEVICE_ERR;
|
||||||
|
|
||||||
|
// 检查是否已注册
|
||||||
|
for (int i = 0; i < manager->motor_count; i++) {
|
||||||
|
if (manager->motors[i] && manager->motors[i]->param.motor_id == param->motor_id) {
|
||||||
|
return DEVICE_ERR; // 已注册
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 检查数量
|
||||||
|
if (manager->motor_count >= MOTOR_LZ_MAX_MOTORS) return DEVICE_ERR;
|
||||||
|
|
||||||
|
// 创建新电机实例
|
||||||
|
MOTOR_LZ_t *new_motor = (MOTOR_LZ_t*)BSP_Malloc(sizeof(MOTOR_LZ_t));
|
||||||
|
if (new_motor == NULL) return DEVICE_ERR;
|
||||||
|
|
||||||
|
memcpy(&new_motor->param, param, sizeof(MOTOR_LZ_Param_t));
|
||||||
|
memset(&new_motor->motor, 0, sizeof(MOTOR_t));
|
||||||
|
memset(&new_motor->lz_feedback, 0, sizeof(MOTOR_LZ_Feedback_t));
|
||||||
|
memset(&new_motor->motion_param, 0, sizeof(MOTOR_LZ_MotionParam_t));
|
||||||
|
|
||||||
|
new_motor->motor.reverse = param->reverse;
|
||||||
|
|
||||||
|
// 注册CAN接收ID - 使用解析后的反馈数据ID
|
||||||
|
// 构建反馈数据的原始扩展ID
|
||||||
|
// 反馈数据：data2包含电机ID(bit8~15)，target_id是主机ID
|
||||||
|
uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->motor_id, param->host_id);
|
||||||
|
// 通过ID解析器得到解析后的ID
|
||||||
|
uint32_t parsed_feedback_id = MOTOR_LZ_IdParser(original_feedback_id, BSP_CAN_FRAME_EXT_DATA);
|
||||||
|
|
||||||
|
if (BSP_CAN_RegisterId(param->can, parsed_feedback_id, 3) != BSP_OK) {
|
||||||
|
BSP_Free(new_motor);
|
||||||
|
return DEVICE_ERR;
|
||||||
|
}
|
||||||
|
|
||||||
|
manager->motors[manager->motor_count] = new_motor;
|
||||||
|
manager->motor_count++;
|
||||||
|
return DEVICE_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
int8_t MOTOR_LZ_Update(MOTOR_LZ_Param_t *param) {
|
||||||
|
if (param == NULL) return DEVICE_ERR_NULL;
|
||||||
|
|
||||||
|
MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can);
|
||||||
|
if (manager == NULL) return DEVICE_ERR_NO_DEV;
|
||||||
|
|
||||||
|
for (int i = 0; i < manager->motor_count; i++) {
|
||||||
|
MOTOR_LZ_t *motor = manager->motors[i];
|
||||||
|
if (motor && motor->param.motor_id == param->motor_id) {
|
||||||
|
// 获取反馈数据 - 使用解析后的ID
|
||||||
|
uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->motor_id, param->host_id);
|
||||||
|
uint32_t parsed_feedback_id = MOTOR_LZ_IdParser(original_feedback_id, BSP_CAN_FRAME_EXT_DATA);
|
||||||
|
BSP_CAN_Message_t msg;
|
||||||
|
|
||||||
|
while (BSP_CAN_GetMessage(param->can, parsed_feedback_id, &msg, 0) == BSP_OK) {
|
||||||
|
MOTOR_LZ_Decode(motor, &msg);
|
||||||
|
}
|
||||||
|
return DEVICE_OK;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return DEVICE_ERR_NO_DEV;
|
||||||
|
}
|
||||||
|
|
||||||
|
int8_t MOTOR_LZ_UpdateAll(void) {
|
||||||
|
int8_t ret = DEVICE_OK;
|
||||||
|
for (int can = 0; can < BSP_CAN_NUM; can++) {
|
||||||
|
MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager((BSP_CAN_t)can);
|
||||||
|
if (manager == NULL) continue;
|
||||||
|
|
||||||
|
for (int i = 0; i < manager->motor_count; i++) {
|
||||||
|
MOTOR_LZ_t *motor = manager->motors[i];
|
||||||
|
if (motor) {
|
||||||
|
if (MOTOR_LZ_Update(&motor->param) != DEVICE_OK) {
|
||||||
|
ret = DEVICE_ERR;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *motion_param) {
|
||||||
|
if (param == NULL || motion_param == NULL) return DEVICE_ERR_NULL;
|
||||||
|
MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
|
||||||
|
if (motor == NULL) return DEVICE_ERR_NO_DEV;
|
||||||
|
|
||||||
|
// 自动反向控制
|
||||||
|
MOTOR_LZ_MotionParam_t send_param = *motion_param;
|
||||||
|
if (param->reverse) {
|
||||||
|
send_param.target_angle = -send_param.target_angle;
|
||||||
|
send_param.target_velocity = -send_param.target_velocity;
|
||||||
|
send_param.torque = -send_param.torque;
|
||||||
|
}
|
||||||
|
|
||||||
|
memcpy(&motor->motion_param, motion_param, sizeof(MOTOR_LZ_MotionParam_t));
|
||||||
|
|
||||||
|
uint16_t raw_torque = MOTOR_LZ_FloatToRaw(send_param.torque, LZ_TORQUE_RANGE_NM);
|
||||||
|
uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_MOTION, raw_torque, param->motor_id);
|
||||||
|
uint8_t data[8];
|
||||||
|
uint16_t raw_angle = MOTOR_LZ_FloatToRaw(send_param.target_angle, LZ_ANGLE_RANGE_RAD);
|
||||||
|
data[0] = (raw_angle >> 8) & 0xFF;
|
||||||
|
data[1] = raw_angle & 0xFF;
|
||||||
|
uint16_t raw_velocity = MOTOR_LZ_FloatToRaw(send_param.target_velocity, LZ_VELOCITY_RANGE_RAD_S);
|
||||||
|
data[2] = (raw_velocity >> 8) & 0xFF;
|
||||||
|
data[3] = raw_velocity & 0xFF;
|
||||||
|
uint16_t raw_kp = MOTOR_LZ_FloatToRawPositive(send_param.kp, LZ_KP_MAX);
|
||||||
|
data[4] = (raw_kp >> 8) & 0xFF;
|
||||||
|
data[5] = raw_kp & 0xFF;
|
||||||
|
uint16_t raw_kd = MOTOR_LZ_FloatToRawPositive(send_param.kd, LZ_KD_MAX);
|
||||||
|
data[6] = (raw_kd >> 8) & 0xFF;
|
||||||
|
data[7] = raw_kd & 0xFF;
|
||||||
|
return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
int8_t MOTOR_LZ_Enable(MOTOR_LZ_Param_t *param) {
|
||||||
|
if (param == NULL) return DEVICE_ERR_NULL;
|
||||||
|
|
||||||
|
// 构建扩展ID - 使能命令
|
||||||
|
uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_ENABLE, param->host_id, param->motor_id);
|
||||||
|
|
||||||
|
// 数据区清零
|
||||||
|
uint8_t data[8] = {0};
|
||||||
|
|
||||||
|
return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
|
||||||
|
}
|
||||||
|
|
||||||
|
int8_t MOTOR_LZ_Disable(MOTOR_LZ_Param_t *param, bool clear_fault) {
|
||||||
|
if (param == NULL) return DEVICE_ERR_NULL;
|
||||||
|
|
||||||
|
// 构建扩展ID - 停止命令
|
||||||
|
uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_DISABLE, param->host_id, param->motor_id);
|
||||||
|
|
||||||
|
// 数据区
|
||||||
|
uint8_t data[8] = {0};
|
||||||
|
if (clear_fault) {
|
||||||
|
data[0] = 1; // Byte[0]=1时清故障
|
||||||
|
}
|
||||||
|
|
||||||
|
return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
|
||||||
|
}
|
||||||
|
|
||||||
|
int8_t MOTOR_LZ_SetZero(MOTOR_LZ_Param_t *param) {
|
||||||
|
if (param == NULL) return DEVICE_ERR_NULL;
|
||||||
|
|
||||||
|
// 构建扩展ID - 设置零位命令
|
||||||
|
uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_SET_ZERO, param->host_id, param->motor_id);
|
||||||
|
|
||||||
|
// 数据区 - Byte[0]=1
|
||||||
|
uint8_t data[8] = {1, 0, 0, 0, 0, 0, 0, 0};
|
||||||
|
|
||||||
|
return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
|
||||||
|
}
|
||||||
|
|
||||||
|
MOTOR_LZ_t* MOTOR_LZ_GetMotor(MOTOR_LZ_Param_t *param) {
|
||||||
|
if (param == NULL) return NULL;
|
||||||
|
|
||||||
|
MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can);
|
||||||
|
if (manager == NULL) return NULL;
|
||||||
|
|
||||||
|
for (int i = 0; i < manager->motor_count; i++) {
|
||||||
|
MOTOR_LZ_t *motor = manager->motors[i];
|
||||||
|
if (motor && motor->param.motor_id == param->motor_id) {
|
||||||
|
return motor;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
int8_t MOTOR_LZ_Relax(MOTOR_LZ_Param_t *param) {
|
||||||
|
return MOTOR_LZ_MotionControl(param, &lz_relax_param);
|
||||||
|
}
|
||||||
|
|
||||||
|
int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param) {
|
||||||
|
MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
|
||||||
|
if (motor) {
|
||||||
|
motor->motor.header.online = false;
|
||||||
|
return DEVICE_OK;
|
||||||
|
}
|
||||||
|
return DEVICE_ERR_NO_DEV;
|
||||||
|
}
|
||||||
|
|
||||||
|
static MOTOR_LZ_Feedback_t* MOTOR_LZ_GetFeedback(MOTOR_LZ_Param_t *param) {
|
||||||
|
MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
|
||||||
|
if (motor && motor->motor.header.online) {
|
||||||
|
return &motor->lz_feedback;
|
||||||
|
}
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
212
User/device/motor_lz.h
Normal file
212
User/device/motor_lz.h
Normal file
@ -0,0 +1,212 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* Includes ----------------------------------------------------------------- */
|
||||||
|
#include "device/device.h"
|
||||||
|
#include "device/motor.h"
|
||||||
|
#include "bsp/can.h"
|
||||||
|
|
||||||
|
/* Exported constants ------------------------------------------------------- */
|
||||||
|
#define MOTOR_LZ_MAX_MOTORS 32
|
||||||
|
|
||||||
|
/* Exported macro ----------------------------------------------------------- */
|
||||||
|
/* Exported types ----------------------------------------------------------- */
|
||||||
|
typedef enum {
|
||||||
|
MOTOR_LZ_RSO0,
|
||||||
|
MOTOR_LZ_RSO1,
|
||||||
|
MOTOR_LZ_RSO2,
|
||||||
|
MOTOR_LZ_RSO3,
|
||||||
|
MOTOR_LZ_RSO4,
|
||||||
|
MOTOR_LZ_RSO5,
|
||||||
|
MOTOR_LZ_RSO6,
|
||||||
|
} MOTOR_LZ_Module_t;
|
||||||
|
|
||||||
|
/* 灵足电机控制模式 */
|
||||||
|
typedef enum {
|
||||||
|
MOTOR_LZ_MODE_MOTION = 0x1, /* 运控模式 */
|
||||||
|
MOTOR_LZ_MODE_CURRENT = 0x2, /* 电流模式 */
|
||||||
|
MOTOR_LZ_MODE_VELOCITY = 0x3, /* 速度模式 */
|
||||||
|
MOTOR_LZ_MODE_POSITION = 0x4, /* 位置模式 */
|
||||||
|
} MOTOR_LZ_ControlMode_t;
|
||||||
|
|
||||||
|
/* 灵足电机通信类型 */
|
||||||
|
typedef enum {
|
||||||
|
MOTOR_LZ_CMD_MOTION = 0x1, /* 运控模式控制 */
|
||||||
|
MOTOR_LZ_CMD_FEEDBACK = 0x2, /* 电机反馈数据 */
|
||||||
|
MOTOR_LZ_CMD_ENABLE = 0x3, /* 电机使能运行 */
|
||||||
|
MOTOR_LZ_CMD_DISABLE = 0x4, /* 电机停止运行 */
|
||||||
|
MOTOR_LZ_CMD_SET_ZERO = 0x6, /* 设置电机机械零位 */
|
||||||
|
} MOTOR_LZ_CmdType_t;
|
||||||
|
|
||||||
|
/* 灵足电机运行状态 */
|
||||||
|
typedef enum {
|
||||||
|
MOTOR_LZ_STATE_RESET = 0, /* Reset模式[复位] */
|
||||||
|
MOTOR_LZ_STATE_CALI = 1, /* Cali模式[标定] */
|
||||||
|
MOTOR_LZ_STATE_MOTOR = 2, /* Motor模式[运行] */
|
||||||
|
} MOTOR_LZ_State_t;
|
||||||
|
|
||||||
|
/* 灵足电机故障信息 */
|
||||||
|
typedef struct {
|
||||||
|
bool uncalibrated; /* bit21: 未标定 */
|
||||||
|
bool stall_overload; /* bit20: 堵转过载故障 */
|
||||||
|
bool encoder_fault; /* bit19: 磁编码故障 */
|
||||||
|
bool over_temp; /* bit18: 过温 */
|
||||||
|
bool driver_fault; /* bit17: 驱动故障 */
|
||||||
|
bool under_voltage; /* bit16: 欠压故障 */
|
||||||
|
} MOTOR_LZ_Fault_t;
|
||||||
|
|
||||||
|
/* 灵足电机运控参数 */
|
||||||
|
typedef struct {
|
||||||
|
float target_angle; /* 目标角度 (-12.57f~12.57f rad) */
|
||||||
|
float target_velocity; /* 目标角速度 (-20~20 rad/s) */
|
||||||
|
float kp; /* 位置增益 (0.0~5000.0) */
|
||||||
|
float kd; /* 微分增益 (0.0~100.0) */
|
||||||
|
float torque; /* 力矩 (-60~60 Nm) */
|
||||||
|
} MOTOR_LZ_MotionParam_t;
|
||||||
|
|
||||||
|
/*每个电机需要的参数*/
|
||||||
|
typedef struct {
|
||||||
|
BSP_CAN_t can; /* CAN总线 */
|
||||||
|
uint8_t motor_id; /* 电机CAN ID */
|
||||||
|
uint8_t host_id; /* 主机CAN ID */
|
||||||
|
MOTOR_LZ_Module_t module; /* 电机型号 */
|
||||||
|
bool reverse; /* 是否反向 */
|
||||||
|
MOTOR_LZ_ControlMode_t mode; /* 控制模式 */
|
||||||
|
} MOTOR_LZ_Param_t;
|
||||||
|
|
||||||
|
/*电机反馈信息扩展*/
|
||||||
|
typedef struct {
|
||||||
|
float current_angle; /* 当前角度 (-12.57f~12.57f rad) */
|
||||||
|
float current_velocity; /* 当前角速度 (-20~20 rad/s) */
|
||||||
|
float current_torque; /* 当前力矩 (-60~60 Nm) */
|
||||||
|
float temperature; /* 当前温度 (摄氏度) */
|
||||||
|
MOTOR_LZ_State_t state; /* 运行状态 */
|
||||||
|
MOTOR_LZ_Fault_t fault; /* 故障信息 */
|
||||||
|
uint8_t motor_can_id; /* 当前电机CAN ID */
|
||||||
|
} MOTOR_LZ_Feedback_t;
|
||||||
|
|
||||||
|
/*电机实例*/
|
||||||
|
typedef struct {
|
||||||
|
MOTOR_LZ_Param_t param;
|
||||||
|
MOTOR_t motor;
|
||||||
|
MOTOR_LZ_Feedback_t lz_feedback; /* 灵足电机特有反馈信息 */
|
||||||
|
MOTOR_LZ_MotionParam_t motion_param; /* 运控模式参数 */
|
||||||
|
} MOTOR_LZ_t;
|
||||||
|
|
||||||
|
/*CAN管理器，管理一个CAN总线上所有的电机*/
|
||||||
|
typedef struct {
|
||||||
|
BSP_CAN_t can;
|
||||||
|
MOTOR_LZ_t *motors[MOTOR_LZ_MAX_MOTORS];
|
||||||
|
uint8_t motor_count;
|
||||||
|
} MOTOR_LZ_CANManager_t;
|
||||||
|
|
||||||
|
/* Exported functions prototypes -------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 初始化灵足电机驱动系统
|
||||||
|
* @return 设备状态码
|
||||||
|
*/
|
||||||
|
int8_t MOTOR_LZ_Init(void);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 注册一个灵足电机
|
||||||
|
* @param param 电机参数
|
||||||
|
* @return 设备状态码
|
||||||
|
*/
|
||||||
|
int8_t MOTOR_LZ_Register(MOTOR_LZ_Param_t *param);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 更新指定电机数据
|
||||||
|
* @param param 电机参数
|
||||||
|
* @return 设备状态码
|
||||||
|
*/
|
||||||
|
int8_t MOTOR_LZ_Update(MOTOR_LZ_Param_t *param);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 更新所有电机数据
|
||||||
|
* @return 设备状态码
|
||||||
|
*/
|
||||||
|
int8_t MOTOR_LZ_UpdateAll(void);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 运控模式控制电机
|
||||||
|
* @param param 电机参数
|
||||||
|
* @param motion_param 运控参数
|
||||||
|
* @return 设备状态码
|
||||||
|
*/
|
||||||
|
int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *motion_param);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 电流(力矩)模式控制电机
|
||||||
|
* @param param 电机参数
|
||||||
|
* @param torque 目标力矩 (-60~60 Nm)
|
||||||
|
* @return 设备状态码
|
||||||
|
*/
|
||||||
|
int8_t MOTOR_LZ_TorqueControl(MOTOR_LZ_Param_t *param, float torque);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 位置模式控制电机
|
||||||
|
* @param param 电机参数
|
||||||
|
* @param target_angle 目标角度 (-12.57~12.57 rad)
|
||||||
|
* @param max_velocity 最大速度 (0~20 rad/s)
|
||||||
|
* @return 设备状态码
|
||||||
|
*/
|
||||||
|
int8_t MOTOR_LZ_PositionControl(MOTOR_LZ_Param_t *param, float target_angle, float max_velocity);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 速度模式控制电机
|
||||||
|
* @param param 电机参数
|
||||||
|
* @param target_velocity 目标速度 (-20~20 rad/s)
|
||||||
|
* @return 设备状态码
|
||||||
|
*/
|
||||||
|
int8_t MOTOR_LZ_VelocityControl(MOTOR_LZ_Param_t *param, float target_velocity);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 电机使能运行
|
||||||
|
* @param param 电机参数
|
||||||
|
* @return 设备状态码
|
||||||
|
*/
|
||||||
|
int8_t MOTOR_LZ_Enable(MOTOR_LZ_Param_t *param);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 电机停止运行
|
||||||
|
* @param param 电机参数
|
||||||
|
* @param clear_fault 是否清除故障
|
||||||
|
* @return 设备状态码
|
||||||
|
*/
|
||||||
|
int8_t MOTOR_LZ_Disable(MOTOR_LZ_Param_t *param, bool clear_fault);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 设置电机机械零位
|
||||||
|
* @param param 电机参数
|
||||||
|
* @return 设备状态码
|
||||||
|
*/
|
||||||
|
int8_t MOTOR_LZ_SetZero(MOTOR_LZ_Param_t *param);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 获取指定电机的实例指针
|
||||||
|
* @param param 电机参数
|
||||||
|
* @return 电机实例指针，失败返回NULL
|
||||||
|
*/
|
||||||
|
MOTOR_LZ_t* MOTOR_LZ_GetMotor(MOTOR_LZ_Param_t *param);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 使电机松弛（发送停止命令）
|
||||||
|
* @param param 电机参数
|
||||||
|
* @return 设备状态码
|
||||||
|
*/
|
||||||
|
int8_t MOTOR_LZ_Relax(MOTOR_LZ_Param_t *param);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 使电机离线（设置在线状态为false）
|
||||||
|
* @param param 电机参数
|
||||||
|
* @return 设备状态码
|
||||||
|
*/
|
||||||
|
int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param);
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
Loading…
Reference in New Issue
Block a user