robofish/MRobot
1
0
Fork 0
mirror of https://github.com/goldenfishs/MRobot.git synced 2025-11-01 20:13:11 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

能用了

Browse Source
This commit is contained in:
Robofish 2025-08-09 03:30:14 +08:00
parent f9f0d93b95
commit 62549172dd
9 changed files with 559 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
app/code_page/bsp_interface.py
View File

@ -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的情况

102
assets/User_code/bsp/can.c
View File

@ -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;
}

17
assets/User_code/bsp/can.h
View File

@ -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解析器

2
assets/User_code/config.csv
View File

@ -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,
1 bsp,can,dwt,gpio,i2c,mm,spi,uart,pwm,time
2 component,ahrs,ballistics,capacity,cmd,crc8,crc16,error_detect,filter,FreeRTOS_CLI,limiter,mixer,pid,ui,user_math
3 device,dr16,bmi088,ist8310 device,dr16,bmi088,ist8310,motor,motor_rm
4 module,

3
assets/User_code/device/device.h
View File

@ -4,6 +4,9 @@
extern "C" {
#endif
#include <stdbool.h>
#include <stdint.h>
#define DEVICE_OK (0)
#define DEVICE_ERR (-1)
#define DEVICE_ERR_NULL (-2)

41
assets/User_code/device/motor.c Normal file
View File

@ -0,0 +1,41 @@
/*
DR16接收机
*/
/* Includes ----------------------------------------------------------------- */
#include "motor.h"
#include <string.h>
/* 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;
}

35
assets/User_code/device/motor.h Normal file
View File

@ -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

311
assets/User_code/device/motor_rm.c Normal file
View File

@ -0,0 +1,311 @@
/*
CAN总线上的设备
将所有CAN总线上挂载的设备抽象成一个设备进行配置和控制
*/
/* Includes ----------------------------------------------------------------- */
#include "motor_rm.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 ----------------------------------------------------------- */
/* 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;
}

128
assets/User_code/device/motor_rm.h Normal file
View File

@ -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