From 62549172ddc06a3aa9de697eeb4915ba1aff0c1b Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sat, 9 Aug 2025 03:30:14 +0800 Subject: [PATCH] =?UTF-8?q?=E8=83=BD=E7=94=A8=E4=BA=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/code_page/bsp_interface.py | 4 +- assets/User_code/bsp/can.c | 102 +++------- assets/User_code/bsp/can.h | 17 +- assets/User_code/config.csv | 2 +- assets/User_code/device/device.h | 3 + assets/User_code/device/motor.c | 41 ++++ assets/User_code/device/motor.h | 35 ++++ assets/User_code/device/motor_rm.c | 311 +++++++++++++++++++++++++++++ assets/User_code/device/motor_rm.h | 128 ++++++++++++ 9 files changed, 559 insertions(+), 84 deletions(-) create mode 100644 assets/User_code/device/motor.c create mode 100644 assets/User_code/device/motor.h create mode 100644 assets/User_code/device/motor_rm.c create mode 100644 assets/User_code/device/motor_rm.h diff --git a/app/code_page/bsp_interface.py b/app/code_page/bsp_interface.py index cea567f..582899a 100644 --- a/app/code_page/bsp_interface.py +++ b/app/code_page/bsp_interface.py @@ -474,8 +474,8 @@ class bsp_can(BspPeripheralBase): " HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO0_MSG_PENDING); // 激活 FIFO0 中断", "", " // 注册回调函数", - f" BSP_CAN_RegisterCallback({self.enum_prefix}_CAN1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifoCallback);", - f" BSP_CAN_RegisterCallback({self.enum_prefix}_CAN2, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifoCallback);", + f" BSP_CAN_RegisterCallback({self.enum_prefix}_{name}, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifoCallback);", + f" BSP_CAN_RegisterCallback({self.enum_prefix}_{name}, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifoCallback);", ]) else: # 只有单个CAN的情况 diff --git a/assets/User_code/bsp/can.c b/assets/User_code/bsp/can.c index 65ac35f..7ddc47b 100644 --- a/assets/User_code/bsp/can.c +++ b/assets/User_code/bsp/can.c @@ -11,6 +11,7 @@ /* Private macro ------------------------------------------------------------ */ /* Private typedef ---------------------------------------------------------- */ typedef struct BSP_CAN_QueueNode { + BSP_CAN_t can; /* CAN通道 */ uint32_t can_id; /* 解析后的CAN ID */ osMessageQueueId_t queue; /* 消息队列ID */ uint8_t queue_size; /* 队列大小 */ @@ -26,9 +27,9 @@ static BSP_CAN_IdParser_t id_parser = NULL; /* ID解析器 */ /* Private function prototypes ---------------------------------------------- */ static BSP_CAN_t CAN_Get(CAN_HandleTypeDef *hcan); -static osMessageQueueId_t BSP_CAN_FindQueue(uint32_t can_id); -static int8_t BSP_CAN_CreateIdQueue(uint32_t can_id, uint8_t queue_size); -static int8_t BSP_CAN_DeleteIdQueue(uint32_t can_id); +static osMessageQueueId_t BSP_CAN_FindQueue(BSP_CAN_t can, uint32_t can_id); +static int8_t BSP_CAN_CreateIdQueue(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size); +static int8_t BSP_CAN_DeleteIdQueue(BSP_CAN_t can, uint32_t can_id); static void BSP_CAN_RxFifoCallback(void); static BSP_CAN_FrameType_t BSP_CAN_GetFrameType(CAN_RxHeaderTypeDef *header); static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type); @@ -50,10 +51,10 @@ static BSP_CAN_t CAN_Get(CAN_HandleTypeDef *hcan) { * @brief 查找指定CAN ID的消息队列 * @note 调用前需要获取互斥锁 */ -static osMessageQueueId_t BSP_CAN_FindQueue(uint32_t can_id) { +static osMessageQueueId_t BSP_CAN_FindQueue(BSP_CAN_t can, uint32_t can_id) { BSP_CAN_QueueNode_t *node = queue_list; while (node != NULL) { - if (node->can_id == can_id) { + if (node->can == can && node->can_id == can_id) { return node->queue; } node = node->next; @@ -65,47 +66,37 @@ static osMessageQueueId_t BSP_CAN_FindQueue(uint32_t can_id) { * @brief 创建指定CAN ID的消息队列 * @note 内部函数，已包含互斥锁保护 */ -static int8_t BSP_CAN_CreateIdQueue(uint32_t can_id, uint8_t queue_size) { +static int8_t BSP_CAN_CreateIdQueue(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size) { if (queue_size == 0) { queue_size = BSP_CAN_DEFAULT_QUEUE_SIZE; } - - // 获取互斥锁 if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) { return BSP_ERR_TIMEOUT; } - - // 检查是否已存在 BSP_CAN_QueueNode_t *node = queue_list; while (node != NULL) { - if (node->can_id == can_id) { + if (node->can == can && node->can_id == can_id) { osMutexRelease(queue_mutex); return BSP_ERR; // 已存在 } node = node->next; } - - // 创建新节点 BSP_CAN_QueueNode_t *new_node = (BSP_CAN_QueueNode_t *)BSP_Malloc(sizeof(BSP_CAN_QueueNode_t)); if (new_node == NULL) { osMutexRelease(queue_mutex); return BSP_ERR_NULL; } - - // 创建消息队列 new_node->queue = osMessageQueueNew(queue_size, sizeof(BSP_CAN_Message_t), NULL); if (new_node->queue == NULL) { BSP_Free(new_node); osMutexRelease(queue_mutex); return BSP_ERR; } - - // 初始化节点 + new_node->can = can; new_node->can_id = can_id; new_node->queue_size = queue_size; new_node->next = queue_list; queue_list = new_node; - osMutexRelease(queue_mutex); return BSP_OK; } @@ -114,32 +105,25 @@ static int8_t BSP_CAN_CreateIdQueue(uint32_t can_id, uint8_t queue_size) { * @brief 删除指定CAN ID的消息队列 * @note 内部函数，已包含互斥锁保护 */ -static int8_t BSP_CAN_DeleteIdQueue(uint32_t can_id) { - // 获取互斥锁 +static int8_t BSP_CAN_DeleteIdQueue(BSP_CAN_t can, uint32_t can_id) { if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) { return BSP_ERR_TIMEOUT; } - BSP_CAN_QueueNode_t **current = &queue_list; while (*current != NULL) { - if ((*current)->can_id == can_id) { + if ((*current)->can == can && (*current)->can_id == can_id) { BSP_CAN_QueueNode_t *to_delete = *current; *current = (*current)->next; - - // 删除队列和节点 osMessageQueueDelete(to_delete->queue); BSP_Free(to_delete); - osMutexRelease(queue_mutex); return BSP_OK; } current = &(*current)->next; } - osMutexRelease(queue_mutex); return BSP_ERR; // 未找到 } - /** * @brief 获取帧类型 */ @@ -165,26 +149,15 @@ static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_ static void BSP_CAN_RxFifoCallback(void) { CAN_RxHeaderTypeDef rx_header; uint8_t rx_data[BSP_CAN_MAX_DLC]; - - // 遍历所有CAN接口处理FIFO0 for (int can_idx = 0; can_idx < BSP_CAN_NUM; can_idx++) { CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle((BSP_CAN_t)can_idx); if (hcan == NULL) continue; - while (HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO0) > 0) { if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data) == HAL_OK) { - // 获取原始ID uint32_t original_id = (rx_header.IDE == CAN_ID_STD) ? rx_header.StdId : rx_header.ExtId; - - // 获取帧类型 BSP_CAN_FrameType_t frame_type = BSP_CAN_GetFrameType(&rx_header); - - // 解析ID uint32_t parsed_id = BSP_CAN_ParseId(original_id, frame_type); - - // 直接查找队列，不使用互斥锁（中断中快速执行） - osMessageQueueId_t queue = BSP_CAN_FindQueue(parsed_id); - + osMessageQueueId_t queue = BSP_CAN_FindQueue((BSP_CAN_t)can_idx, parsed_id); if (queue != NULL) { BSP_CAN_Message_t msg = {0}; msg.frame_type = frame_type; @@ -194,9 +167,7 @@ static void BSP_CAN_RxFifoCallback(void) { if (rx_header.RTR == CAN_RTR_DATA) { memcpy(msg.data, rx_data, rx_header.DLC); } - msg.timestamp = HAL_GetTick(); // 添加时间戳 - - // 非阻塞发送，如果队列满了就丢弃 + msg.timestamp = HAL_GetTick(); osMessageQueuePut(queue, &msg, 0, BSP_CAN_TIMEOUT_IMMEDIATE); } } @@ -476,89 +447,70 @@ int8_t BSP_CAN_TransmitRemoteFrame(BSP_CAN_t can, BSP_CAN_RemoteFrame_t *frame) return BSP_CAN_Transmit(can, format, frame->id, NULL, frame->dlc); } -int8_t BSP_CAN_RegisterId(uint32_t can_id, uint8_t queue_size) { +int8_t BSP_CAN_RegisterId(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size) { if (!inited) { return BSP_ERR_INITED; } - - return BSP_CAN_CreateIdQueue(can_id, queue_size); + return BSP_CAN_CreateIdQueue(can, can_id, queue_size); } -int8_t BSP_CAN_UnregisterIdQueue(uint32_t can_id) { +int8_t BSP_CAN_UnregisterIdQueue(BSP_CAN_t can, uint32_t can_id) { if (!inited) { return BSP_ERR_INITED; } - - return BSP_CAN_DeleteIdQueue(can_id); + return BSP_CAN_DeleteIdQueue(can, can_id); } -int8_t BSP_CAN_GetMessage(uint32_t can_id, BSP_CAN_Message_t *msg, uint32_t timeout) { +int8_t BSP_CAN_GetMessage(BSP_CAN_t can, uint32_t can_id, BSP_CAN_Message_t *msg, uint32_t timeout) { if (!inited) { return BSP_ERR_INITED; } if (msg == NULL) { return BSP_ERR_NULL; } - - // 线程安全地查找队列 if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) { return BSP_ERR_TIMEOUT; } - - osMessageQueueId_t queue = BSP_CAN_FindQueue(can_id); + osMessageQueueId_t queue = BSP_CAN_FindQueue(can, can_id); osMutexRelease(queue_mutex); - if (queue == NULL) { - return BSP_ERR_NO_DEV; // 队列不存在 + return BSP_ERR_NO_DEV; } - osStatus_t result = osMessageQueueGet(queue, msg, NULL, timeout); return (result == osOK) ? BSP_OK : BSP_ERR; } -int32_t BSP_CAN_GetQueueCount(uint32_t can_id) { +int32_t BSP_CAN_GetQueueCount(BSP_CAN_t can, uint32_t can_id) { if (!inited) { return -1; } - - // 线程安全地查找队列 if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) { return -1; } - - osMessageQueueId_t queue = BSP_CAN_FindQueue(can_id); + osMessageQueueId_t queue = BSP_CAN_FindQueue(can, can_id); osMutexRelease(queue_mutex); - if (queue == NULL) { - return -1; // 队列不存在 + return -1; } - return (int32_t)osMessageQueueGetCount(queue); } -int8_t BSP_CAN_FlushQueue(uint32_t can_id) { +int8_t BSP_CAN_FlushQueue(BSP_CAN_t can, uint32_t can_id) { if (!inited) { return BSP_ERR_INITED; } - - // 线程安全地查找队列 if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) { return BSP_ERR_TIMEOUT; } - - osMessageQueueId_t queue = BSP_CAN_FindQueue(can_id); + osMessageQueueId_t queue = BSP_CAN_FindQueue(can, can_id); osMutexRelease(queue_mutex); - if (queue == NULL) { - return BSP_ERR_NO_DEV; // 队列不存在 + return BSP_ERR_NO_DEV; } - - // 清空队列中的所有消息 BSP_CAN_Message_t temp_msg; while (osMessageQueueGet(queue, &temp_msg, NULL, BSP_CAN_TIMEOUT_IMMEDIATE) == osOK) { - // 继续取出消息直到队列为空 + // 清空 } - return BSP_OK; } diff --git a/assets/User_code/bsp/can.h b/assets/User_code/bsp/can.h index efa2c07..d348131 100644 --- a/assets/User_code/bsp/can.h +++ b/assets/User_code/bsp/can.h @@ -162,41 +162,46 @@ int8_t BSP_CAN_TransmitRemoteFrame(BSP_CAN_t can, BSP_CAN_RemoteFrame_t *frame); /** * @brief 注册 CAN ID 接收队列 + * @param can CAN 枚举 * @param can_id 解析后的CAN ID * @param queue_size 队列大小，0使用默认值 * @return BSP_OK 成功，其他值失败 */ -int8_t BSP_CAN_RegisterId(uint32_t can_id, uint8_t queue_size); +int8_t BSP_CAN_RegisterId(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size); /** * @brief 注销 CAN ID 接收队列 + * @param can CAN 枚举 * @param can_id 解析后的CAN ID * @return BSP_OK 成功，其他值失败 */ -int8_t BSP_CAN_UnregisterIdQueue(uint32_t can_id); +int8_t BSP_CAN_UnregisterIdQueue(BSP_CAN_t can, uint32_t can_id); /** - * @brief 获取 CAN 消息（阻塞方式） + * @brief 获取 CAN 消息 + * @param can CAN 枚举 * @param can_id 解析后的CAN ID * @param msg 存储消息的结构体指针 * @param timeout 超时时间（毫秒），0为立即返回，osWaitForever为永久等待 * @return BSP_OK 成功，其他值失败 */ -int8_t BSP_CAN_GetMessage(uint32_t can_id, BSP_CAN_Message_t *msg, uint32_t timeout); +int8_t BSP_CAN_GetMessage(BSP_CAN_t can, uint32_t can_id, BSP_CAN_Message_t *msg, uint32_t timeout); /** * @brief 获取指定ID队列中的消息数量 + * @param can CAN 枚举 * @param can_id 解析后的CAN ID * @return 消息数量，-1表示队列不存在 */ -int32_t BSP_CAN_GetQueueCount(uint32_t can_id); +int32_t BSP_CAN_GetQueueCount(BSP_CAN_t can, uint32_t can_id); /** * @brief 清空指定ID队列中的所有消息 + * @param can CAN 枚举 * @param can_id 解析后的CAN ID * @return BSP_OK 成功，其他值失败 */ -int8_t BSP_CAN_FlushQueue(uint32_t can_id); +int8_t BSP_CAN_FlushQueue(BSP_CAN_t can, uint32_t can_id); /** * @brief 注册ID解析器 diff --git a/assets/User_code/config.csv b/assets/User_code/config.csv index 51cc2f5..08417cb 100644 --- a/assets/User_code/config.csv +++ b/assets/User_code/config.csv @@ -1,4 +1,4 @@ bsp,can,dwt,gpio,i2c,mm,spi,uart,pwm,time component,ahrs,ballistics,capacity,cmd,crc8,crc16,error_detect,filter,FreeRTOS_CLI,limiter,mixer,pid,ui,user_math -device,dr16,bmi088,ist8310 +device,dr16,bmi088,ist8310,motor,motor_rm module, \ No newline at end of file diff --git a/assets/User_code/device/device.h b/assets/User_code/device/device.h index 9ac8953..79f1140 100644 --- a/assets/User_code/device/device.h +++ b/assets/User_code/device/device.h @@ -4,6 +4,9 @@ extern "C" { #endif +#include +#include + #define DEVICE_OK (0) #define DEVICE_ERR (-1) #define DEVICE_ERR_NULL (-2) diff --git a/assets/User_code/device/motor.c b/assets/User_code/device/motor.c new file mode 100644 index 0000000..9b4bcb6 --- /dev/null +++ b/assets/User_code/device/motor.c @@ -0,0 +1,41 @@ +/* + DR16接收机 +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "motor.h" + +#include + + +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* Private variables -------------------------------------------------------- */ + +/* Private function -------------------------------------------------------- */ + +/* Exported functions ------------------------------------------------------- */ +float MOTOR_GetRotorAbsAngle(const MOTOR_t *motor) { + if (motor == NULL) return DEVICE_ERR_NULL; + + return motor->feedback.rotor_abs_angle; +} + +float MOTOR_GetRotorSpeed(const MOTOR_t *motor) { + if (motor == NULL) return DEVICE_ERR_NULL; + + return motor->feedback.rotor_speed; +} + +float MOTOR_GetTorqueCurrent(const MOTOR_t *motor) { + if (motor == NULL) return DEVICE_ERR_NULL; + + return motor->feedback.torque_current; +} + +float MOTOR_GetTemp(const MOTOR_t *motor) { + if (motor == NULL) return DEVICE_ERR_NULL; + + return motor->feedback.temp; +} \ No newline at end of file diff --git a/assets/User_code/device/motor.h b/assets/User_code/device/motor.h new file mode 100644 index 0000000..d6b7b34 --- /dev/null +++ b/assets/User_code/device/motor.h @@ -0,0 +1,35 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/device.h" + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef struct { + float rotor_abs_angle; /* 转子绝对角度 */ + float rotor_speed; /* 实际转子转速 */ + float torque_current; /* 转矩电流 */ + float temp; /* 温度 */ +} MOTOR_Feedback_t; + +typedef struct { + DEVICE_Header_t header; + bool reverse; /* 是否反装 */ + MOTOR_Feedback_t feedback; +} MOTOR_t; + +/* Exported functions prototypes -------------------------------------------- */ +float MOTOR_GetRotorAbsAngle(const MOTOR_t *motor); +float MOTOR_GetRotorSpeed(const MOTOR_t *motor); +float MOTOR_GetTorqueCurrent(const MOTOR_t *motor); +float MOTOR_GetTemp(const MOTOR_t *motor); + + +#ifdef __cplusplus +} +#endif diff --git a/assets/User_code/device/motor_rm.c b/assets/User_code/device/motor_rm.c new file mode 100644 index 0000000..05fb89a --- /dev/null +++ b/assets/User_code/device/motor_rm.c @@ -0,0 +1,311 @@ +/* + CAN总线上的设备 + 将所有CAN总线上挂载的设备抽象成一个设备进行配置和控制 +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "motor_rm.h" + +#include +#include + +#include "bsp/can.h" +#include "bsp/mm.h" +#include "bsp/time.h" +#include "component/user_math.h" + +/* Private define ----------------------------------------------------------- */ +/* Motor id */ +/* id feedback id control id */ +/* 1-4 0x205 to 0x208 0x1ff */ +/* 5-7 0x209 to 0x20B 0x2ff */ +#define GM6020_FB_ID_BASE (0x205) +#define GM6020_CTRL_ID_BASE (0x1ff) +#define GM6020_CTRL_ID_EXTAND (0x2ff) + +/* id feedback id control id */ +/* 1-4 0x201 to 0x204 0x200 */ +/* 5-8 0x205 to 0x208 0x1ff */ +#define M3508_M2006_FB_ID_BASE (0x201) +#define M3508_M2006_CTRL_ID_BASE (0x200) +#define M3508_M2006_CTRL_ID_EXTAND (0x1ff) +#define M3508_M2006_ID_SETTING_ID (0x700) + +#define GM6020_MAX_ABS_LSB (30000) +#define M3508_MAX_ABS_LSB (16384) +#define M2006_MAX_ABS_LSB (10000) + +#define MOTOR_TX_BUF_SIZE (8) +#define MOTOR_RX_BUF_SIZE (8) + +#define MOTOR_ENC_RES (8192) /* 电机编码器分辨率 */ +#define MOTOR_CUR_RES (16384) /* 电机转矩电流分辨率 */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* Private variables -------------------------------------------------------- */ +static MOTOR_RM_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL}; + +/* Private function -------------------------------------------------------- */ +static int8_t MOTOR_RM_GetLogicalIndex(uint16_t can_id, MOTOR_RM_Module_t module) { + switch (module) { + case MOTOR_M2006: + case MOTOR_M3508: + if (can_id >= M3508_M2006_FB_ID_BASE && can_id < M3508_M2006_FB_ID_BASE + 7) { + return can_id - M3508_M2006_FB_ID_BASE; // 返回1-8 + } + break; + case MOTOR_GM6020: + if (can_id >= GM6020_FB_ID_BASE && can_id < GM6020_FB_ID_BASE + 6) { + return can_id - GM6020_FB_ID_BASE + 4; // 返回5-11 + } + break; + default: + break; + } + return DEVICE_ERR; +} + +static int16_t MOTOR_RM_GetLSB(MOTOR_RM_Module_t module) { + switch (module) { + case MOTOR_M2006: + return M2006_MAX_ABS_LSB; + case MOTOR_M3508: + return M3508_MAX_ABS_LSB; + case MOTOR_GM6020: + return GM6020_MAX_ABS_LSB; + default: + return DEVICE_ERR; + } +} + +static MOTOR_RM_CANManager_t* MOTOR_RM_GetCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return NULL; + return can_managers[can]; +} + +static int8_t MOTOR_RM_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_RM_CANManager_t*)BSP_Malloc(sizeof(MOTOR_RM_CANManager_t)); + if (can_managers[can] == NULL) return DEVICE_ERR; + + memset(can_managers[can], 0, sizeof(MOTOR_RM_CANManager_t)); + can_managers[can]->can = can; + + return 0; +} + +static void Motor_RM_Decode(MOTOR_RM_t *motor, BSP_CAN_Message_t *msg) { + uint16_t raw_angle = (uint16_t)((msg->data[0] << 8) | msg->data[1]); + int16_t raw_current = (int16_t)((msg->data[4] << 8) | msg->data[5]); + + motor->motor.feedback.rotor_abs_angle = raw_angle / (float)MOTOR_ENC_RES * M_2PI; + motor->motor.feedback.rotor_speed = (int16_t)((msg->data[2] << 8) | msg->data[3]); + + // 根据电机类型选择正确的LSB + int16_t lsb = MOTOR_RM_GetLSB(motor->param.module); + motor->motor.feedback.torque_current = raw_current * lsb / (float)MOTOR_CUR_RES; + motor->motor.feedback.temp = msg->data[6]; +} + +/* Exported functions ------------------------------------------------------- */ + +int8_t MOTOR_RM_Register(MOTOR_RM_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + // 1. 检查并创建CAN管理器 + if (MOTOR_RM_CreateCANManager(param->can) != DEVICE_OK) { + return DEVICE_ERR; + } + + // 获取对应的CAN管理器 + MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR; + + // 2. 检查电机是否已注册 + for (int i = 0; i < manager->motor_count; i++) { + if (manager->motors[i] != NULL && + manager->motors[i]->param.id == param->id) { + return DEVICE_ERR_INITED; // 电机已存在 + } + } + + // 3. 检查是否还能添加更多电机 + if (manager->motor_count >= MOTOR_RM_MAX_MOTORS) { + return DEVICE_ERR; // 电机数量已满 + } + + // 4. 创建新的电机实例 + MOTOR_RM_t *new_motor = (MOTOR_RM_t*)BSP_Malloc(sizeof(MOTOR_RM_t)); + if (new_motor == NULL) return DEVICE_ERR; + + // 5. 初始化电机参数 + memcpy(&new_motor->param, param, sizeof(MOTOR_RM_Param_t)); + memset(&new_motor->motor, 0, sizeof(MOTOR_t)); + new_motor->motor.reverse = param->reverse; + + // 6. 注册CAN接收ID（直接使用实际CAN ID） + if (BSP_CAN_RegisterId(param->can, param->id, 3) != BSP_OK) { + BSP_Free(new_motor); + return DEVICE_ERR; + } + // 7. 将电机添加到管理器中 + manager->motors[manager->motor_count] = new_motor; + manager->motor_count++; + + return DEVICE_OK; +} + +int8_t MOTOR_RM_Update(BSP_CAN_t can, uint16_t id) { + MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(can); + if (manager == NULL) return DEVICE_ERR_NO_DEV; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_RM_t *motor = manager->motors[i]; + if (motor != NULL && motor->param.id == id) { + BSP_CAN_Message_t rx_msg; + if (BSP_CAN_GetMessage(can, id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) { + uint64_t now_time = BSP_TIME_Get(); + if (now_time - motor->motor.header.last_online_time > 1000) { + motor->motor.header.online = false; + return DEVICE_ERR_NO_DEV; // 电机不在线 + } + return DEVICE_ERR; // 没有新数据但电机在线 + } + // 解码接收到的消息 + motor->motor.header.online = true; + motor->motor.header.last_online_time = BSP_TIME_Get(); + Motor_RM_Decode(motor, &rx_msg); + return DEVICE_OK; // 成功更新 + } + } + return DEVICE_ERR_NO_DEV; // 未找到对应电机 +} + +int8_t MOTOR_RM_UpdateAll(void) { + int8_t ret = DEVICE_OK; + for (int can = 0; can < BSP_CAN_NUM; can++) { + MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager((BSP_CAN_t)can); + if (manager == NULL) continue; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_RM_t *motor = manager->motors[i]; + if (motor != NULL) { + if (MOTOR_RM_Update((BSP_CAN_t)can, motor->param.id) != DEVICE_OK) { + ret = DEVICE_ERR; + } + } + } + } + return ret; +} + +int8_t MOTOR_RM_SetOutput(BSP_CAN_t can, uint16_t id, float value) { + MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(can); + if (manager == NULL) return DEVICE_ERR_NO_DEV; + + if (value > 1.0f) value = 1.0f; + if (value < -1.0f) value = -1.0f; + + MOTOR_RM_t *motor = MOTOR_RM_GetMotor(can, id); + if (motor == NULL) return DEVICE_ERR_NO_DEV; + + int8_t logical_index = MOTOR_RM_GetLogicalIndex(id, motor->param.module); + if (logical_index < 0) return DEVICE_ERR; + + MOTOR_RM_MsgOutput_t *output_msg = &manager->output_msg; + int16_t output_value = (int16_t)(value * (float)MOTOR_RM_GetLSB(motor->param.module)); + output_msg->output[logical_index] = output_value; + + return DEVICE_OK; +} + +int8_t MOTOR_RM_Ctrl(BSP_CAN_t can, uint16_t id) { + MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(can); + if (manager == NULL) return DEVICE_ERR_NO_DEV; + + MOTOR_RM_MsgOutput_t *output_msg = &manager->output_msg; + BSP_CAN_StdDataFrame_t tx_frame; + + switch (id) { + case M3508_M2006_FB_ID_BASE: + case M3508_M2006_FB_ID_BASE+1: + case M3508_M2006_FB_ID_BASE+2: + case M3508_M2006_FB_ID_BASE+3: + tx_frame.id = M3508_M2006_CTRL_ID_BASE; + tx_frame.dlc = MOTOR_TX_BUF_SIZE; + tx_frame.data[0] = (uint8_t)((output_msg->output[0] >> 8) & 0xFF); + tx_frame.data[1] = (uint8_t)(output_msg->output[0] & 0xFF); + tx_frame.data[2] = (uint8_t)((output_msg->output[1] >> 8) & 0xFF); + tx_frame.data[3] = (uint8_t)(output_msg->output[1] & 0xFF); + tx_frame.data[4] = (uint8_t)((output_msg->output[2] >> 8) & 0xFF); + tx_frame.data[5] = (uint8_t)(output_msg->output[2] & 0xFF); + tx_frame.data[6] = (uint8_t)((output_msg->output[3] >> 8) & 0xFF); + tx_frame.data[7] = (uint8_t)(output_msg->output[3] & 0xFF); + break; + case M3508_M2006_FB_ID_BASE+4: + case M3508_M2006_FB_ID_BASE+5: + case M3508_M2006_FB_ID_BASE+6: + case M3508_M2006_FB_ID_BASE+7: + tx_frame.id = M3508_M2006_CTRL_ID_EXTAND; + tx_frame.dlc = MOTOR_TX_BUF_SIZE; + tx_frame.data[0] = (uint8_t)((output_msg->output[4] >> 8) & 0xFF); + tx_frame.data[1] = (uint8_t)(output_msg->output[4] & 0xFF); + tx_frame.data[2] = (uint8_t)((output_msg->output[5] >> 8) & 0xFF); + tx_frame.data[3] = (uint8_t)(output_msg->output[5] & 0xFF); + tx_frame.data[4] = (uint8_t)((output_msg->output[6] >> 8) & 0xFF); + tx_frame.data[5] = (uint8_t)(output_msg->output[6] & 0xFF); + tx_frame.data[6] = (uint8_t)((output_msg->output[7] >> 8) & 0xFF); + tx_frame.data[7] = (uint8_t)(output_msg->output[7] & 0xFF); + break; + + case GM6020_FB_ID_BASE+4: + case GM6020_FB_ID_BASE+5: + case GM6020_FB_ID_BASE+6: + tx_frame.id = GM6020_CTRL_ID_EXTAND; + tx_frame.dlc = MOTOR_TX_BUF_SIZE; + tx_frame.data[0] = (uint8_t)((output_msg->output[8] >> 8) & 0xFF); + tx_frame.data[1] = (uint8_t)(output_msg->output[8] & 0xFF); + tx_frame.data[2] = (uint8_t)((output_msg->output[9] >> 8) & 0xFF); + tx_frame.data[3] = (uint8_t)(output_msg->output[9] & 0xFF); + tx_frame.data[4] = (uint8_t)((output_msg->output[10] >> 8) & 0xFF); + tx_frame.data[5] = (uint8_t)(output_msg->output[10] & 0xFF); + tx_frame.data[6] = 0; + tx_frame.data[7] = 0; + break; + default: + return DEVICE_ERR; // 不支持的控制ID + } + + return BSP_CAN_TransmitStdDataFrame(can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +MOTOR_RM_t* MOTOR_RM_GetMotor(BSP_CAN_t can, uint16_t id) { + MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(can); + if (manager == NULL) return NULL; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_RM_t *motor = manager->motors[i]; + if (motor != NULL && motor->param.id == id) { + return motor; + } + } + return NULL; +} + +int8_t MOTOR_RM_Relax(BSP_CAN_t can, uint16_t id) { + int8_t ret = MOTOR_RM_SetOutput(can, id, 0.0f); + if (ret != DEVICE_OK) return ret; + return DEVICE_OK; +} + +int8_t MOTOR_RM_Offine(BSP_CAN_t can, uint16_t id) { + MOTOR_RM_t *motor = MOTOR_RM_GetMotor(can, id); + if (motor != NULL) { + motor->motor.header.online = false; + return DEVICE_OK; + } + return DEVICE_ERR_NO_DEV; +} \ No newline at end of file diff --git a/assets/User_code/device/motor_rm.h b/assets/User_code/device/motor_rm.h new file mode 100644 index 0000000..6d2dc42 --- /dev/null +++ b/assets/User_code/device/motor_rm.h @@ -0,0 +1,128 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/device.h" +#include "device/motor.h" +#include "bsp/can.h" + +/* Exported constants ------------------------------------------------------- */ +#define MOTOR_RM_MAX_MOTORS 11 /* 最大电机数量 */ + +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef enum { + MOTOR_M2006, + MOTOR_M3508, + MOTOR_GM6020, +} MOTOR_RM_Module_t; + +/*一个can最多控制11个电机*/ +typedef union { + int16_t output[MOTOR_RM_MAX_MOTORS]; + struct { + int16_t m3508_m2006_id201; + int16_t m3508_m2006_id202; + int16_t m3508_m2006_id203; + int16_t m3508_m2006_id204; + int16_t m3508_m2006_gm6020_id205; + int16_t m3508_m2006_gm6020_id206; + int16_t m3508_m2006_gm6020_id207; + int16_t m3508_m2006_gm6020_id208; + int16_t gm6020_id209; + int16_t gm6020_id20A; + int16_t gm6020_id20B; + } named; +} MOTOR_RM_MsgOutput_t; +/*注册电机时候每个电机需要的参数*/ +typedef struct { + BSP_CAN_t can; + uint16_t id; // 实际CAN ID，如0x201, 0x205等 + MOTOR_RM_Module_t module; + bool reverse; +} MOTOR_RM_Param_t; + +typedef struct MOTOR_RM_t { + MOTOR_RM_Param_t param; + MOTOR_t motor; +} MOTOR_RM_t; + +typedef struct { + BSP_CAN_t can; + MOTOR_RM_MsgOutput_t output_msg; + MOTOR_RM_t *motors[MOTOR_RM_MAX_MOTORS]; // 最多15个电机 + uint8_t motor_count; +} MOTOR_RM_CANManager_t; + + + + +/* Exported functions prototypes -------------------------------------------- */ +/** + * @brief 注册一个RM电机 + * @param param 电机参数 + * @return 0 成功，其他值失败 + */ +int8_t MOTOR_RM_Register(MOTOR_RM_Param_t *param); + +/** + * @brief 更新单个电机数据 + * @param can CAN通道 + * @param id 电机实际CAN ID + * @return 0 成功，其他值失败 + */ +int8_t MOTOR_RM_Update(BSP_CAN_t can, uint16_t id); + +/** + * @brief 更新所有注册了的电机数据 + * @return 0 成功，其他值失败 + */ +int8_t MOTOR_RM_UpdateAll(void); + +/** + * @brief 设置电机输出值 + * @param can CAN通道 + * @param id 电机实际CAN ID + * @param value 输出值（归一化，-1.0到1.0） + * @return 0 成功，其他值失败 + */ +int8_t MOTOR_RM_SetOutput(BSP_CAN_t can, uint16_t id, float value); + +/** + * @brief 发送电机控制命令 + * @param can CAN通道 + * @param ctrl_id 控制帧ID（0x200, 0x1ff, 0x2ff） + * @return 0 成功，其他值失败 + */ +int8_t MOTOR_RM_Ctrl(BSP_CAN_t can, uint16_t ctrl_id); + +/** + * @brief 获取电机实例 + * @param can CAN通道 + * @param id 电机实际CAN ID + * @return 电机实例指针，失败返回NULL + */ +MOTOR_RM_t* MOTOR_RM_GetMotor(BSP_CAN_t can, uint16_t id); + +/** + * @brief 使电机松弛（设置输出为0） + * @param can CAN通道 + * @param id 电机实际CAN ID + * @return 0 成功，其他值失败 + */ +int8_t MOTOR_RM_Relax(BSP_CAN_t can, uint16_t id); + +/** + * @brief 使电机离线（输出为0并标记为不在线） + * @param can CAN通道 + * @param id 电机实际CAN ID + * @return 0 成功，其他值失败 + */ +int8_t MOTOR_RM_Offine(BSP_CAN_t can, uint16_t id); + +#ifdef __cplusplus +} +#endif \ No newline at end of file