20 changed files with 2585 additions and 52 deletions
@ -0,0 +1,16 @@ |
|||||
|
cmake_minimum_required(VERSION 3.10) |
||||
|
|
||||
|
get_filename_component(TARGET_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) |
||||
|
set(COMMON_CMAKE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/../library/CMakeLists.txt") |
||||
|
|
||||
|
if(EXISTS ${COMMON_CMAKE_PATH}) |
||||
|
include(${COMMON_CMAKE_PATH}) |
||||
|
else() |
||||
|
message(FATAL_ERROR "Cannot find common build logic at ${COMMON_CMAKE_PATH}") |
||||
|
endif() |
||||
|
|
||||
|
if(TARGET bspMCU) |
||||
|
target_link_libraries(${TARGET_NAME} PUBLIC bspMCU) |
||||
|
else() |
||||
|
message(FATAL_ERROR "[${TARGET_NAME}] Dependency 'bspMCU' not found.") |
||||
|
endif() |
||||
@ -0,0 +1,134 @@ |
|||||
|
/******************************************************************************
|
||||
|
|
||||
|
版权所有 (C), 2018-2099, Radkil |
||||
|
|
||||
|
****************************************************************************** |
||||
|
文 件 名 : BHBF.h |
||||
|
版 本 号 : 初稿 |
||||
|
作 者 : radkil |
||||
|
生成日期 : 2026年6月8日 |
||||
|
最近修改 : |
||||
|
功能描述 : BHBF.c 的头文件 |
||||
|
|
||||
|
修改历史 : |
||||
|
1.日 期 : 2026年6月8日 |
||||
|
作 者 : radkil |
||||
|
修改内容 : 创建文件 |
||||
|
|
||||
|
******************************************************************************/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 外部变量说明 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 外部函数原型说明 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 内部函数原型说明 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 全局变量 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 模块级变量 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 常量定义 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 宏定义 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
#ifndef __BHBF_H__ |
||||
|
#define __BHBF_H__ |
||||
|
|
||||
|
|
||||
|
#ifdef __cplusplus |
||||
|
#if __cplusplus |
||||
|
extern "C"{ |
||||
|
#endif |
||||
|
#endif /* __cplusplus */ |
||||
|
|
||||
|
|
||||
|
/*==============================================*
|
||||
|
* include header files * |
||||
|
*----------------------------------------------*/ |
||||
|
#include "drv_interface.h" |
||||
|
#include "rd_time.h" |
||||
|
#include "msg_center.h" |
||||
|
#include "cmsis_os.h" |
||||
|
#include "FreeRTOS.h" |
||||
|
|
||||
|
/*==============================================*
|
||||
|
* constants or macros define * |
||||
|
*----------------------------------------------*/ |
||||
|
#define MODULE_NAME_MOTOR "motor" |
||||
|
|
||||
|
typedef enum { |
||||
|
MOTOR_CMD_SET_SPEED = 0x01, // 设置速度
|
||||
|
MOTOR_CMD_SET_POSITION = 0x02, // 设置位置
|
||||
|
MOTOR_CMD_STOP = 0x03, // 停止
|
||||
|
MOTOR_CMD_GET_STATUS = 0x04, // 获取状态
|
||||
|
MOTOR_CMD_SET_HOME = 0x05, // 设置零点
|
||||
|
MOTOR_CMD_SPEED_MODE = 0x06, // 速度模式
|
||||
|
MOTOR_CMD_POSITION_MODE = 0x07, // 位置模式
|
||||
|
} Motor_Cmd_e; |
||||
|
|
||||
|
// 电机命令数据结构
|
||||
|
typedef struct { |
||||
|
uint8_t m_ucMotorIndex; // 电机索引（0=左轮，1=右轮）
|
||||
|
int32_t m_iValue; // 值（速度/位置等）
|
||||
|
} Motor_CmdData_t; |
||||
|
|
||||
|
// 电机状态数据结构
|
||||
|
typedef struct { |
||||
|
uint8_t m_ucMotorIndex; |
||||
|
int32_t m_iPosition; |
||||
|
int32_t m_iVelocity; |
||||
|
uint32_t m_uiFaultCode; |
||||
|
} Motor_StatusData_t; |
||||
|
|
||||
|
#define MODULE_NAME_RBCORE "RBcore" |
||||
|
|
||||
|
typedef enum { |
||||
|
GF_CMD_DEFAULT_NULL = 0, |
||||
|
GF_CMD_STOP_ALL, |
||||
|
GF_CMD_MANUAL_FORWARD, |
||||
|
GF_CMD_MANUAL_BACKWARD, |
||||
|
GF_CMD_AUTO_FORWARD |
||||
|
} GF_CMD; |
||||
|
|
||||
|
typedef struct _GV_struct_define |
||||
|
{ |
||||
|
int m_iWorkMode; |
||||
|
GF_CMD m_eGFCmd; |
||||
|
int32_t m_iMoveSpeed[2];//索引1对应左轮，索引2对应右轮
|
||||
|
} GV_define; |
||||
|
|
||||
|
/*==============================================*
|
||||
|
* project-wide global variables * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
|
||||
|
|
||||
|
/*==============================================*
|
||||
|
* routines' or functions' implementations * |
||||
|
*----------------------------------------------*/ |
||||
|
GV_define *GetGV(void); |
||||
|
void SetGV(GV_define *_pGV); |
||||
|
// 待实现接口
|
||||
|
|
||||
|
#ifdef __cplusplus |
||||
|
#if __cplusplus |
||||
|
} |
||||
|
#endif |
||||
|
#endif /* __cplusplus */ |
||||
|
|
||||
|
|
||||
|
#endif /* __BHBF_ROBOT_H__ */ |
||||
@ -0,0 +1,135 @@ |
|||||
|
/******************************************************************************
|
||||
|
|
||||
|
版权所有 (C), 2018-2099, Radkil |
||||
|
|
||||
|
****************************************************************************** |
||||
|
文 件 名 : msg_center.h |
||||
|
版 本 号 : 初稿 |
||||
|
作 者 : radkil |
||||
|
生成日期 : 2026年7月13日 |
||||
|
最近修改 : |
||||
|
功能描述 : 模块间消息分发中心（跨线程通信） |
||||
|
|
||||
|
修改历史 : |
||||
|
1.日 期 : 2026年7月13日 |
||||
|
作 者 : radkil |
||||
|
修改内容 : 创建文件 |
||||
|
|
||||
|
******************************************************************************/ |
||||
|
|
||||
|
#ifndef __MSG_CENTER_H__ |
||||
|
#define __MSG_CENTER_H__ |
||||
|
|
||||
|
#ifdef __cplusplus |
||||
|
#if __cplusplus |
||||
|
extern "C"{ |
||||
|
#endif |
||||
|
#endif /* __cplusplus */ |
||||
|
|
||||
|
/*==============================================*
|
||||
|
* include header files * |
||||
|
*----------------------------------------------*/ |
||||
|
#include "common.h" |
||||
|
|
||||
|
/*==============================================*
|
||||
|
* constants or macros define * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
#define MSG_CENTER_MAX_MODULES 16 // 最大模块数
|
||||
|
#define MSG_CENTER_MAX_NAME_LEN 16 // 模块名称最大长度
|
||||
|
#define MSG_CENTER_QUEUE_SIZE 64 // 消息队列大小
|
||||
|
#define MSG_CENTER_MAX_DATA_SIZE 32 // 消息数据最大大小
|
||||
|
|
||||
|
/*==============================================*
|
||||
|
* data structures * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
// 消息ID定义（各模块自己的命令字）
|
||||
|
typedef uint32_t MsgID_t; |
||||
|
|
||||
|
// 消息结构
|
||||
|
typedef struct { |
||||
|
uint32_t m_uiSrcModule; // 源模块ID
|
||||
|
uint32_t m_uiDstModule; // 目标模块ID
|
||||
|
MsgID_t m_uiMsgID; // 消息ID（命令字）
|
||||
|
uint32_t m_uiDataLen; // 数据长度
|
||||
|
uint8_t m_aucData[MSG_CENTER_MAX_DATA_SIZE]; // 消息数据
|
||||
|
} Msg_t; |
||||
|
|
||||
|
// 消息处理回调函数类型
|
||||
|
typedef void (*MsgHandler_t)(const Msg_t *pstMsg); |
||||
|
|
||||
|
// 模块注册信息
|
||||
|
typedef struct { |
||||
|
char m_szName[MSG_CENTER_MAX_NAME_LEN]; // 模块名称
|
||||
|
uint32_t m_uiModuleID; // 模块ID
|
||||
|
MsgHandler_t m_pfHandler; // 消息处理回调
|
||||
|
} MsgModule_t; |
||||
|
|
||||
|
/*==============================================*
|
||||
|
* project-wide global variables * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*==============================================*
|
||||
|
* routines' or functions' implementations * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/**
|
||||
|
* @brief 初始化消息中心 |
||||
|
*/ |
||||
|
void MsgCenter_Init(void); |
||||
|
|
||||
|
/**
|
||||
|
* @brief 注册模块到消息中心 |
||||
|
* @param pszName 模块名称 |
||||
|
* @param pfHandler 消息处理回调 |
||||
|
* @return 模块ID，失败返回0 |
||||
|
*/ |
||||
|
uint32_t MsgCenter_Register(const char *pszName, MsgHandler_t pfHandler); |
||||
|
|
||||
|
/**
|
||||
|
* @brief 通过名称查找模块ID |
||||
|
* @param pszName 模块名称 |
||||
|
* @return 模块ID，未找到返回0 |
||||
|
*/ |
||||
|
uint32_t MsgCenter_FindModule(const char *pszName); |
||||
|
|
||||
|
/**
|
||||
|
* @brief 发送消息（异步，跨线程安全） |
||||
|
* @param pstMsg 消息指针 |
||||
|
* @return 0成功，非0失败 |
||||
|
*/ |
||||
|
int MsgCenter_Send(const Msg_t *pstMsg); |
||||
|
|
||||
|
/**
|
||||
|
* @brief 发送消息给指定模块（便捷函数） |
||||
|
* @param pszDstName 目标模块名称 |
||||
|
* @param uiMsgID 消息ID |
||||
|
* @param pData 数据指针（可为NULL） |
||||
|
* @param uiDataLen 数据长度 |
||||
|
* @return 0成功，非0失败 |
||||
|
*/ |
||||
|
int MsgCenter_SendTo(const char *pszDstName, MsgID_t uiMsgID, const void *pData, uint32_t uiDataLen); |
||||
|
|
||||
|
/**
|
||||
|
* @brief 处理当前模块的消息（在模块线程中调用） |
||||
|
* @param uiModuleID 模块ID |
||||
|
* @return 处理的消息数量 |
||||
|
*/ |
||||
|
int MsgCenter_Process(uint32_t uiModuleID); |
||||
|
|
||||
|
/**
|
||||
|
* @brief 处理当前模块的消息（带超时等待） |
||||
|
* @param uiModuleID 模块ID |
||||
|
* @param uiTimeoutMs 超时时间（毫秒），0表示不等待 |
||||
|
* @return 处理的消息数量，-1表示超时 |
||||
|
*/ |
||||
|
int MsgCenter_ProcessWait(uint32_t uiModuleID, uint32_t uiTimeoutMs); |
||||
|
|
||||
|
#ifdef __cplusplus |
||||
|
#if __cplusplus |
||||
|
} |
||||
|
#endif |
||||
|
#endif /* __cplusplus */ |
||||
|
|
||||
|
#endif /* __MSG_CENTER_H__ */ |
||||
@ -0,0 +1,373 @@ |
|||||
|
/******************************************************************************
|
||||
|
|
||||
|
版权所有 (C), 2018-2099, Radkil |
||||
|
|
||||
|
****************************************************************************** |
||||
|
文 件 名 : msg_center.c |
||||
|
版 本 号 : 初稿 |
||||
|
作 者 : radkil |
||||
|
生成日期 : 2026年7月13日 |
||||
|
最近修改 : |
||||
|
功能描述 : 模块间消息分发中心（跨线程通信） |
||||
|
|
||||
|
修改历史 : |
||||
|
1.日 期 : 2026年7月13日 |
||||
|
作 者 : radkil |
||||
|
修改内容 : 创建文件 |
||||
|
|
||||
|
******************************************************************************/ |
||||
|
|
||||
|
#include "msg_center.h" |
||||
|
#include "rd_thread.h" |
||||
|
#include "ringbuffer.h" |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 外部变量说明 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 外部函数原型说明 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 内部函数原型说明 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 全局变量 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 模块级变量 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
// 模块注册表
|
||||
|
typedef struct { |
||||
|
MsgModule_t stModule; |
||||
|
rd_ringbuf_t *pstRingBuf; // 消息队列（ringbuffer）
|
||||
|
rd_mutex_t stMutex; // 互斥锁（保护ringbuffer）
|
||||
|
rd_sem_t stSem; // 信号量（阻塞等待）
|
||||
|
} MsgModuleEntry_t; |
||||
|
|
||||
|
static MsgModuleEntry_t g_astModules[MSG_CENTER_MAX_MODULES]; |
||||
|
static uint32_t g_uiModuleCount = 0; |
||||
|
static uint32_t g_uiNextModuleID = 1; // 从1开始，0表示无效
|
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 常量定义 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
// ringbuffer大小：每个模块可缓存的消息数量
|
||||
|
#define MSG_RING_BUFFER_SIZE (MSG_CENTER_QUEUE_SIZE * sizeof(Msg_t)) |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 宏定义 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
// 查找模块索引
|
||||
|
#define FIND_MODULE_INDEX(uiModuleID) \ |
||||
|
((uiModuleID) > 0 && (uiModuleID) <= g_uiModuleCount ? \ |
||||
|
(uiModuleID) - 1 : -1) |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MsgCenter_Init |
||||
|
功能描述 : 初始化消息中心 |
||||
|
输入参数 : 无 |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : void |
||||
|
*****************************************************************************/ |
||||
|
void MsgCenter_Init(void) |
||||
|
{ |
||||
|
g_uiModuleCount = 0; |
||||
|
g_uiNextModuleID = 1; |
||||
|
|
||||
|
RD_MEMSET(g_astModules, 0, sizeof(g_astModules)); |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MsgCenter_Register |
||||
|
功能描述 : 注册模块到消息中心 |
||||
|
输入参数 : const char *pszName 模块名称 |
||||
|
MsgHandler_t pfHandler 消息处理回调 |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : uint32_t 模块ID，失败返回0 |
||||
|
*****************************************************************************/ |
||||
|
uint32_t MsgCenter_Register(const char *pszName, MsgHandler_t pfHandler) |
||||
|
{ |
||||
|
if (NULL == pszName || NULL == pfHandler) |
||||
|
{ |
||||
|
return 0; |
||||
|
} |
||||
|
|
||||
|
if (g_uiModuleCount >= MSG_CENTER_MAX_MODULES) |
||||
|
{ |
||||
|
return 0; // 模块数已满
|
||||
|
} |
||||
|
|
||||
|
// 检查名称是否重复
|
||||
|
for (uint32_t i = 0; i < g_uiModuleCount; i++) |
||||
|
{ |
||||
|
if (RD_STRCMP(g_astModules[i].stModule.m_szName, pszName) == 0) |
||||
|
{ |
||||
|
return 0; // 名称重复
|
||||
|
} |
||||
|
} |
||||
|
|
||||
|
// 注册模块
|
||||
|
uint32_t uiIndex = g_uiModuleCount; |
||||
|
uint32_t uiModuleID = g_uiNextModuleID++; |
||||
|
|
||||
|
RD_STRNCPY(g_astModules[uiIndex].stModule.m_szName, pszName, MSG_CENTER_MAX_NAME_LEN - 1); |
||||
|
g_astModules[uiIndex].stModule.m_szName[MSG_CENTER_MAX_NAME_LEN - 1] = '\0'; |
||||
|
g_astModules[uiIndex].stModule.m_uiModuleID = uiModuleID; |
||||
|
g_astModules[uiIndex].stModule.m_pfHandler = pfHandler; |
||||
|
|
||||
|
// 创建 ringbuffer
|
||||
|
g_astModules[uiIndex].pstRingBuf = rd_RingbufferCreate(MSG_RING_BUFFER_SIZE); |
||||
|
if (NULL == g_astModules[uiIndex].pstRingBuf) |
||||
|
{ |
||||
|
return 0; |
||||
|
} |
||||
|
|
||||
|
// 初始化互斥锁
|
||||
|
if (Rd_MutexInit(&g_astModules[uiIndex].stMutex) != RD_SUCCESS) |
||||
|
{ |
||||
|
rd_RingbufferDestroy(g_astModules[uiIndex].pstRingBuf); |
||||
|
return 0; |
||||
|
} |
||||
|
|
||||
|
// 初始化信号量（初始值为0，表示无消息）
|
||||
|
if (Rd_SemInit(&g_astModules[uiIndex].stSem, 0, 0) != RD_SUCCESS) |
||||
|
{ |
||||
|
Rd_MutexDestroy(&g_astModules[uiIndex].stMutex); |
||||
|
rd_RingbufferDestroy(g_astModules[uiIndex].pstRingBuf); |
||||
|
return 0; |
||||
|
} |
||||
|
|
||||
|
g_uiModuleCount++; |
||||
|
|
||||
|
return uiModuleID; |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MsgCenter_FindModule |
||||
|
功能描述 : 通过名称查找模块ID |
||||
|
输入参数 : const char *pszName 模块名称 |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : uint32_t 模块ID，未找到返回0 |
||||
|
*****************************************************************************/ |
||||
|
uint32_t MsgCenter_FindModule(const char *pszName) |
||||
|
{ |
||||
|
if (NULL == pszName) |
||||
|
{ |
||||
|
return 0; |
||||
|
} |
||||
|
|
||||
|
for (uint32_t i = 0; i < g_uiModuleCount; i++) |
||||
|
{ |
||||
|
if (RD_STRCMP(g_astModules[i].stModule.m_szName, pszName) == 0) |
||||
|
{ |
||||
|
return g_astModules[i].stModule.m_uiModuleID; |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
return 0; |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MsgCenter_Send |
||||
|
功能描述 : 发送消息（异步，跨线程安全） |
||||
|
输入参数 : const Msg_t *pstMsg 消息指针 |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : int 0成功，非0失败 |
||||
|
*****************************************************************************/ |
||||
|
int MsgCenter_Send(const Msg_t *pstMsg) |
||||
|
{ |
||||
|
if (NULL == pstMsg) |
||||
|
{ |
||||
|
return RD_NULL; |
||||
|
} |
||||
|
|
||||
|
int iDstIndex = FIND_MODULE_INDEX(pstMsg->m_uiDstModule); |
||||
|
if (iDstIndex < 0 || NULL == g_astModules[iDstIndex].pstRingBuf) |
||||
|
{ |
||||
|
return RD_FAILURE; |
||||
|
} |
||||
|
|
||||
|
// 加锁保护ringbuffer
|
||||
|
Rd_MutexLock(&g_astModules[iDstIndex].stMutex, __func__, NULL); |
||||
|
|
||||
|
// 写入ringbuffer
|
||||
|
int iRet = rd_RingbufferPut(g_astModules[iDstIndex].pstRingBuf, |
||||
|
(const char *)pstMsg, sizeof(Msg_t)); |
||||
|
|
||||
|
Rd_MutexUnlock(&g_astModules[iDstIndex].stMutex, __func__, NULL); |
||||
|
|
||||
|
if (iRet > 0) |
||||
|
{ |
||||
|
// 发送信号量，通知有新消息
|
||||
|
Rd_SemPost(&g_astModules[iDstIndex].stSem, __func__, NULL); |
||||
|
return RD_SUCCESS; |
||||
|
} |
||||
|
|
||||
|
return RD_FAILURE; // 队列满
|
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MsgCenter_SendTo |
||||
|
功能描述 : 发送消息给指定模块（便捷函数） |
||||
|
输入参数 : const char *pszDstName 目标模块名称 |
||||
|
MsgID_t uiMsgID 消息ID |
||||
|
const void *pData 数据指针（可为NULL） |
||||
|
uint32_t uiDataLen 数据长度 |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : int 0成功，非0失败 |
||||
|
*****************************************************************************/ |
||||
|
int MsgCenter_SendTo(const char *pszDstName, MsgID_t uiMsgID, const void *pData, uint32_t uiDataLen) |
||||
|
{ |
||||
|
if (NULL == pszDstName) |
||||
|
{ |
||||
|
return RD_NULL; |
||||
|
} |
||||
|
|
||||
|
uint32_t uiDstModuleID = MsgCenter_FindModule(pszDstName); |
||||
|
if (0 == uiDstModuleID) |
||||
|
{ |
||||
|
return RD_FAILURE; |
||||
|
} |
||||
|
|
||||
|
Msg_t stMsg; |
||||
|
RD_MEMSET(&stMsg, 0, sizeof(Msg_t)); |
||||
|
stMsg.m_uiDstModule = uiDstModuleID; |
||||
|
stMsg.m_uiMsgID = uiMsgID; |
||||
|
|
||||
|
if (NULL != pData && uiDataLen > 0) |
||||
|
{ |
||||
|
if (uiDataLen > MSG_CENTER_MAX_DATA_SIZE) |
||||
|
{ |
||||
|
uiDataLen = MSG_CENTER_MAX_DATA_SIZE; |
||||
|
} |
||||
|
RD_MEMCPY(stMsg.m_aucData, pData, uiDataLen); |
||||
|
stMsg.m_uiDataLen = uiDataLen; |
||||
|
} |
||||
|
|
||||
|
return MsgCenter_Send(&stMsg); |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MsgCenter_ProcessOne |
||||
|
功能描述 : 从ringbuffer读取一条消息 |
||||
|
输入参数 : int iIndex 模块索引 |
||||
|
Msg_t *pstMsg 输出消息 |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : int 1成功，0无数据 |
||||
|
*****************************************************************************/ |
||||
|
static int MsgCenter_ProcessOne(int iIndex, Msg_t *pstMsg) |
||||
|
{ |
||||
|
// 加锁保护ringbuffer
|
||||
|
Rd_MutexLock(&g_astModules[iIndex].stMutex, __func__, NULL); |
||||
|
|
||||
|
int iRet = rd_RingbufferGet(g_astModules[iIndex].pstRingBuf, |
||||
|
(char *)pstMsg, sizeof(Msg_t)); |
||||
|
|
||||
|
Rd_MutexUnlock(&g_astModules[iIndex].stMutex, __func__, NULL); |
||||
|
|
||||
|
return (iRet > 0) ? 1 : 0; |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MsgCenter_Process |
||||
|
功能描述 : 处理当前模块的消息（在模块线程中调用） |
||||
|
输入参数 : uint32_t uiModuleID 模块ID |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : int 处理的消息数量 |
||||
|
*****************************************************************************/ |
||||
|
int MsgCenter_Process(uint32_t uiModuleID) |
||||
|
{ |
||||
|
int iIndex = FIND_MODULE_INDEX(uiModuleID); |
||||
|
if (iIndex < 0 || NULL == g_astModules[iIndex].pstRingBuf) |
||||
|
{ |
||||
|
return 0; |
||||
|
} |
||||
|
|
||||
|
int iCount = 0; |
||||
|
Msg_t stMsg; |
||||
|
|
||||
|
// 非阻塞读取所有待处理消息
|
||||
|
while (MsgCenter_ProcessOne(iIndex, &stMsg)) |
||||
|
{ |
||||
|
if (g_astModules[iIndex].stModule.m_pfHandler != NULL) |
||||
|
{ |
||||
|
g_astModules[iIndex].stModule.m_pfHandler(&stMsg); |
||||
|
} |
||||
|
iCount++; |
||||
|
} |
||||
|
|
||||
|
return iCount; |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MsgCenter_ProcessWait |
||||
|
功能描述 : 处理当前模块的消息（带超时等待） |
||||
|
输入参数 : uint32_t uiModuleID 模块ID |
||||
|
uint32_t uiTimeoutMs 超时时间（毫秒），0表示不等待 |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : int 处理的消息数量，-1表示超时 |
||||
|
*****************************************************************************/ |
||||
|
int MsgCenter_ProcessWait(uint32_t uiModuleID, uint32_t uiTimeoutMs) |
||||
|
{ |
||||
|
int iIndex = FIND_MODULE_INDEX(uiModuleID); |
||||
|
if (iIndex < 0 || NULL == g_astModules[iIndex].pstRingBuf) |
||||
|
{ |
||||
|
return -1; |
||||
|
} |
||||
|
|
||||
|
Msg_t stMsg; |
||||
|
|
||||
|
// 等待信号量（有新消息到达）
|
||||
|
if (uiTimeoutMs > 0) |
||||
|
{ |
||||
|
// 带超时等待
|
||||
|
Rd_SemWait(&g_astModules[iIndex].stSem, __func__, NULL); |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
// 非阻塞，直接检查ringbuffer
|
||||
|
if (!MsgCenter_ProcessOne(iIndex, &stMsg)) |
||||
|
{ |
||||
|
return 0; |
||||
|
} |
||||
|
|
||||
|
// 处理第一条消息
|
||||
|
if (g_astModules[iIndex].stModule.m_pfHandler != NULL) |
||||
|
{ |
||||
|
g_astModules[iIndex].stModule.m_pfHandler(&stMsg); |
||||
|
} |
||||
|
|
||||
|
// 继续处理剩余消息
|
||||
|
int iCount = 1; |
||||
|
while (MsgCenter_ProcessOne(iIndex, &stMsg)) |
||||
|
{ |
||||
|
if (g_astModules[iIndex].stModule.m_pfHandler != NULL) |
||||
|
{ |
||||
|
g_astModules[iIndex].stModule.m_pfHandler(&stMsg); |
||||
|
} |
||||
|
iCount++; |
||||
|
} |
||||
|
|
||||
|
return iCount; |
||||
|
} |
||||
|
|
||||
|
// 信号量被触发，处理消息
|
||||
|
int iCount = 0; |
||||
|
while (MsgCenter_ProcessOne(iIndex, &stMsg)) |
||||
|
{ |
||||
|
if (g_astModules[iIndex].stModule.m_pfHandler != NULL) |
||||
|
{ |
||||
|
g_astModules[iIndex].stModule.m_pfHandler(&stMsg); |
||||
|
} |
||||
|
iCount++; |
||||
|
} |
||||
|
|
||||
|
return iCount; |
||||
|
} |
||||
@ -0,0 +1,16 @@ |
|||||
|
cmake_minimum_required(VERSION 3.10) |
||||
|
|
||||
|
get_filename_component(TARGET_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) |
||||
|
set(COMMON_CMAKE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/../library/CMakeLists.txt") |
||||
|
|
||||
|
if(EXISTS ${COMMON_CMAKE_PATH}) |
||||
|
include(${COMMON_CMAKE_PATH}) |
||||
|
else() |
||||
|
message(FATAL_ERROR "Cannot find common build logic at ${COMMON_CMAKE_PATH}") |
||||
|
endif() |
||||
|
|
||||
|
if(TARGET RBcore) |
||||
|
target_link_libraries(${TARGET_NAME} PUBLIC RBcore) |
||||
|
else() |
||||
|
message(FATAL_ERROR "[${TARGET_NAME}] Dependency 'RBcore' not found.") |
||||
|
endif() |
||||
@ -0,0 +1,183 @@ |
|||||
|
/******************************************************************************
|
||||
|
|
||||
|
版权所有 (C), 2018-2099, Radkil |
||||
|
|
||||
|
****************************************************************************** |
||||
|
文 件 名 : motor_manager.h |
||||
|
版 本 号 : 初稿 |
||||
|
作 者 : radkil |
||||
|
生成日期 : 2026年7月13日 |
||||
|
最近修改 : |
||||
|
功能描述 : 电机管理模块（多实例管理，协议无关） |
||||
|
|
||||
|
修改历史 : |
||||
|
1.日 期 : 2026年7月13日 |
||||
|
作 者 : radkil |
||||
|
修改内容 : 创建文件 |
||||
|
|
||||
|
******************************************************************************/ |
||||
|
|
||||
|
#ifndef __MOTOR_MANAGER_H__ |
||||
|
#define __MOTOR_MANAGER_H__ |
||||
|
|
||||
|
#ifdef __cplusplus |
||||
|
#if __cplusplus |
||||
|
extern "C"{ |
||||
|
#endif |
||||
|
#endif /* __cplusplus */ |
||||
|
|
||||
|
/*==============================================*
|
||||
|
* include header files * |
||||
|
*----------------------------------------------*/ |
||||
|
#include "motor_protocol.h" |
||||
|
|
||||
|
/*==============================================*
|
||||
|
* constants or macros define * |
||||
|
*----------------------------------------------*/ |
||||
|
#define MOTOR_INSTANCE_MAX 8 //支持最大电机实例数
|
||||
|
/*==============================================*
|
||||
|
* data structures * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*==============================================*
|
||||
|
* project-wide global variables * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*==============================================*
|
||||
|
* routines' or functions' implementations * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/**
|
||||
|
* @brief 初始化电机管理模块 |
||||
|
*/ |
||||
|
void MotorMgr_Init(void); |
||||
|
|
||||
|
/**
|
||||
|
* @brief 创建电机实例 |
||||
|
* @param pstConfig 电机配置 |
||||
|
* @param ptCAN 绑定的CAN控制器 |
||||
|
* @param pstProtocol 协议操作函数表 |
||||
|
* @param pstPrivData 协议私有数据（可为NULL） |
||||
|
* @return 电机实例指针，失败返回NULL |
||||
|
*/ |
||||
|
Motor_Instance_t* MotorMgr_Create(const Motor_Config_t *pstConfig, |
||||
|
TComCtrl *ptCAN, |
||||
|
const Motor_Protocol_t *pstProtocol, |
||||
|
void *pstPrivData); |
||||
|
|
||||
|
/**
|
||||
|
* @brief 销毁电机实例 |
||||
|
* @param pstMotor 电机实例 |
||||
|
* @return 0成功，非0失败 |
||||
|
*/ |
||||
|
int MotorMgr_Destroy(Motor_Instance_t *pstMotor); |
||||
|
|
||||
|
/**
|
||||
|
* @brief 通过名称查找电机实例 |
||||
|
* @param pszName 实例名称 |
||||
|
* @return 电机实例指针，未找到返回NULL |
||||
|
*/ |
||||
|
Motor_Instance_t* MotorMgr_FindByName(const char *pszName); |
||||
|
|
||||
|
/**
|
||||
|
* @brief 通过ID查找电机实例 |
||||
|
* @param ucMotorID 电机节点ID |
||||
|
* @return 电机实例指针，未找到返回NULL |
||||
|
*/ |
||||
|
Motor_Instance_t* MotorMgr_FindByID(uint8_t ucMotorID); |
||||
|
|
||||
|
/**
|
||||
|
* @brief 复位所有节点 |
||||
|
*/ |
||||
|
int MotorMgr_ResetAll(Motor_Instance_t *pstMotor); |
||||
|
|
||||
|
/**
|
||||
|
* @brief 激活所有节点 |
||||
|
*/ |
||||
|
int MotorMgr_ActivateAll(Motor_Instance_t *pstMotor); |
||||
|
|
||||
|
/**
|
||||
|
* @brief 设置当前位置为零点 |
||||
|
*/ |
||||
|
int MotorMgr_SetHome(Motor_Instance_t *pstMotor); |
||||
|
|
||||
|
/**
|
||||
|
* @brief 设置看门狗时间 |
||||
|
*/ |
||||
|
int MotorMgr_SetWatchdog(Motor_Instance_t *pstMotor, uint32_t uiTimeMs); |
||||
|
|
||||
|
/**
|
||||
|
* @brief 读取电机位置 |
||||
|
*/ |
||||
|
int MotorMgr_RequestPosition(Motor_Instance_t *pstMotor); |
||||
|
|
||||
|
/**
|
||||
|
* @brief 读取电机速度 |
||||
|
*/ |
||||
|
int MotorMgr_RequestVelocity(Motor_Instance_t *pstMotor); |
||||
|
|
||||
|
/**
|
||||
|
* @brief 读取电机电流/转矩 |
||||
|
*/ |
||||
|
int MotorMgr_RequestTorque(Motor_Instance_t *pstMotor); |
||||
|
|
||||
|
/**
|
||||
|
* @brief 读取故障码 |
||||
|
*/ |
||||
|
int MotorMgr_RequestFault(Motor_Instance_t *pstMotor); |
||||
|
|
||||
|
/**
|
||||
|
* @brief 速度模式初始化 |
||||
|
*/ |
||||
|
int MotorMgr_SpeedModeInit(Motor_Instance_t *pstMotor); |
||||
|
|
||||
|
/**
|
||||
|
* @brief 设置目标速度 |
||||
|
*/ |
||||
|
int MotorMgr_SetTargetSpeed(Motor_Instance_t *pstMotor, int32_t iTargetSpeed); |
||||
|
|
||||
|
/**
|
||||
|
* @brief 位置模式初始化 |
||||
|
*/ |
||||
|
int MotorMgr_PositionModeInit(Motor_Instance_t *pstMotor, int32_t iAccel, int32_t iDecel, int32_t iTargetSpeed); |
||||
|
|
||||
|
/**
|
||||
|
* @brief 设置目标位置（相对运动） |
||||
|
*/ |
||||
|
int MotorMgr_SetTargetPosition(Motor_Instance_t *pstMotor, int32_t iTargetPos); |
||||
|
|
||||
|
/**
|
||||
|
* @brief 设置数字输出 |
||||
|
*/ |
||||
|
int MotorMgr_SetDO(Motor_Instance_t *pstMotor, uint8_t ucDoIndex, uint8_t ucState); |
||||
|
|
||||
|
/**
|
||||
|
* @brief 解析CAN响应数据（遍历所有电机实例尝试解析） |
||||
|
* @param ucMotorID 电机节点ID |
||||
|
* @param pucData 数据指针 |
||||
|
* @param uiLen 数据长度 |
||||
|
*/ |
||||
|
void MotorMgr_ParseResponse(uint8_t ucMotorID, const uint8_t *pucData, uint32_t uiLen); |
||||
|
|
||||
|
/**
|
||||
|
* @brief 速度单位转换 |
||||
|
*/ |
||||
|
int32_t MotorMgr_SpeedConvert(Motor_Instance_t *pstMotor, int32_t iSpeedUserUnit); |
||||
|
|
||||
|
/**
|
||||
|
* @brief 获取电机数据 |
||||
|
*/ |
||||
|
const Motor_Data_t* MotorMgr_GetData(Motor_Instance_t *pstMotor); |
||||
|
|
||||
|
/**
|
||||
|
* @brief 获取电机状态 |
||||
|
*/ |
||||
|
Motor_State_e MotorMgr_GetState(Motor_Instance_t *pstMotor); |
||||
|
|
||||
|
#ifdef __cplusplus |
||||
|
#if __cplusplus |
||||
|
} |
||||
|
#endif |
||||
|
#endif /* __cplusplus */ |
||||
|
|
||||
|
#endif /* __MOTOR_MANAGER_H__ */ |
||||
@ -0,0 +1,160 @@ |
|||||
|
/******************************************************************************
|
||||
|
|
||||
|
版权所有 (C), 2018-2099, Radkil |
||||
|
|
||||
|
****************************************************************************** |
||||
|
文 件 名 : motor_protocol.h |
||||
|
版 本 号 : 初稿 |
||||
|
作 者 : radkil |
||||
|
生成日期 : 2026年7月13日 |
||||
|
最近修改 : |
||||
|
功能描述 : 电机协议抽象层接口定义 |
||||
|
|
||||
|
修改历史 : |
||||
|
1.日 期 : 2026年7月13日 |
||||
|
作 者 : radkil |
||||
|
修改内容 : 创建文件 |
||||
|
|
||||
|
******************************************************************************/ |
||||
|
|
||||
|
#ifndef __MOTOR_PROTOCOL_H__ |
||||
|
#define __MOTOR_PROTOCOL_H__ |
||||
|
|
||||
|
#ifdef __cplusplus |
||||
|
#if __cplusplus |
||||
|
extern "C"{ |
||||
|
#endif |
||||
|
#endif /* __cplusplus */ |
||||
|
|
||||
|
/*==============================================*
|
||||
|
* include header files * |
||||
|
*----------------------------------------------*/ |
||||
|
#include "common.h" |
||||
|
#include "slist.h" |
||||
|
#include "com.h" |
||||
|
|
||||
|
/*==============================================*
|
||||
|
* constants or macros define * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
#define MOTOR_MAX_NAME_LEN 16 |
||||
|
|
||||
|
/*==============================================*
|
||||
|
* data structures * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
// 电机状态枚举
|
||||
|
typedef enum { |
||||
|
MOTOR_STATE_IDLE = 0, // 空闲
|
||||
|
MOTOR_STATE_INIT, // 初始化中
|
||||
|
MOTOR_STATE_READY, // 就绪
|
||||
|
MOTOR_STATE_RUNNING, // 运行中
|
||||
|
MOTOR_STATE_ERROR, // 错误
|
||||
|
} Motor_State_e; |
||||
|
|
||||
|
// 电机运行数据
|
||||
|
typedef struct { |
||||
|
int32_t m_iRealPosition; // 实际位置(脉冲)
|
||||
|
int32_t m_iRealVelocity; // 实际速度(脉冲/秒)
|
||||
|
int32_t m_iRealTorque; // 实际转矩
|
||||
|
uint32_t m_uiFaultCode; // 故障码
|
||||
|
double m_dRealDistance; // 实际距离(米)
|
||||
|
int32_t m_iStartMeasurePos; // 测量起始位置
|
||||
|
int32_t m_iLastPosition; // 上次位置
|
||||
|
int32_t m_iRoundCount; // 圈数计数
|
||||
|
uint8_t m_ucStartMeasureFlag; // 开始测量标志
|
||||
|
} Motor_Data_t; |
||||
|
|
||||
|
// 电机物理配置
|
||||
|
typedef struct { |
||||
|
uint8_t m_ucMotorID; // 电机节点ID
|
||||
|
uint32_t m_uiPulsePerRound; // 每圈脉冲数
|
||||
|
uint32_t m_uiReductionRatio; // 减速比
|
||||
|
float m_fWheelDiameter; // 轮直径(米)
|
||||
|
} Motor_Config_t; |
||||
|
|
||||
|
// 前向声明
|
||||
|
struct Motor_Instance; |
||||
|
|
||||
|
// 协议操作函数表（虚函数表）
|
||||
|
typedef struct { |
||||
|
// 协议名称
|
||||
|
char m_szName[MOTOR_MAX_NAME_LEN]; |
||||
|
|
||||
|
// 初始化电机（协议特定的初始化序列）
|
||||
|
int (*pfInit)(struct Motor_Instance *pstMotor); |
||||
|
|
||||
|
// 复位所有节点
|
||||
|
int (*pfResetAll)(struct Motor_Instance *pstMotor); |
||||
|
|
||||
|
// 激活所有节点
|
||||
|
int (*pfActivateAll)(struct Motor_Instance *pstMotor); |
||||
|
|
||||
|
// 设置当前位置为零点
|
||||
|
int (*pfSetHome)(struct Motor_Instance *pstMotor); |
||||
|
|
||||
|
// 设置看门狗时间
|
||||
|
int (*pfSetWatchdog)(struct Motor_Instance *pstMotor, uint32_t uiTimeMs); |
||||
|
|
||||
|
// 读取电机位置
|
||||
|
int (*pfRequestPosition)(struct Motor_Instance *pstMotor); |
||||
|
|
||||
|
// 读取电机速度
|
||||
|
int (*pfRequestVelocity)(struct Motor_Instance *pstMotor); |
||||
|
|
||||
|
// 读取电机电流/转矩
|
||||
|
int (*pfRequestTorque)(struct Motor_Instance *pstMotor); |
||||
|
|
||||
|
// 读取故障码
|
||||
|
int (*pfRequestFault)(struct Motor_Instance *pstMotor); |
||||
|
|
||||
|
// 速度模式初始化
|
||||
|
int (*pfSpeedModeInit)(struct Motor_Instance *pstMotor); |
||||
|
|
||||
|
// 设置目标速度
|
||||
|
int (*pfSetTargetSpeed)(struct Motor_Instance *pstMotor, int32_t iTargetSpeed); |
||||
|
|
||||
|
// 位置模式初始化
|
||||
|
int (*pfPositionModeInit)(struct Motor_Instance *pstMotor, int32_t iAccel, int32_t iDecel, int32_t iTargetSpeed); |
||||
|
|
||||
|
// 设置目标位置（相对运动）
|
||||
|
int (*pfSetTargetPosition)(struct Motor_Instance *pstMotor, int32_t iTargetPos); |
||||
|
|
||||
|
// 设置数字输出
|
||||
|
int (*pfSetDO)(struct Motor_Instance *pstMotor, uint8_t ucDoIndex, uint8_t ucState); |
||||
|
|
||||
|
// 解析CAN响应数据
|
||||
|
int (*pfParseResponse)(struct Motor_Instance *pstMotor, const uint8_t *pucData, uint32_t uiLen); |
||||
|
|
||||
|
// 速度单位转换：用户单位 -> 脉冲/秒
|
||||
|
int32_t (*pfSpeedConvert)(struct Motor_Instance *pstMotor, int32_t iSpeedUserUnit); |
||||
|
} Motor_Protocol_t; |
||||
|
|
||||
|
// 电机实例结构体
|
||||
|
typedef struct Motor_Instance { |
||||
|
rd_slist_t node; // slist链表节点
|
||||
|
char m_szName[MOTOR_MAX_NAME_LEN]; // 实例名称
|
||||
|
Motor_Config_t m_stConfig; // 配置
|
||||
|
Motor_Data_t m_stData; // 运行数据
|
||||
|
Motor_State_e m_eState; // 状态
|
||||
|
TComCtrl *m_ptCAN; // 绑定的CAN控制器
|
||||
|
uint32_t m_uiLastHeartbeat; // 上次心跳时间
|
||||
|
const Motor_Protocol_t *m_pstProtocol; // 协议操作函数表
|
||||
|
void *m_pstPrivData; // 协议私有数据
|
||||
|
} Motor_Instance_t; |
||||
|
|
||||
|
/*==============================================*
|
||||
|
* project-wide global variables * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*==============================================*
|
||||
|
* routines' or functions' implementations * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
#ifdef __cplusplus |
||||
|
#if __cplusplus |
||||
|
} |
||||
|
#endif |
||||
|
#endif /* __cplusplus */ |
||||
|
|
||||
|
#endif /* __MOTOR_PROTOCOL_H__ */ |
||||
@ -0,0 +1,99 @@ |
|||||
|
/******************************************************************************
|
||||
|
|
||||
|
版权所有 (C), 2018-2099, Radkil |
||||
|
|
||||
|
****************************************************************************** |
||||
|
文 件 名 : msp_motor_leisai.h |
||||
|
版 本 号 : 初稿 |
||||
|
作 者 : radkil |
||||
|
生成日期 : 2026年7月13日 |
||||
|
最近修改 : |
||||
|
功能描述 : 雷赛ISV2电机CAN协议实现 |
||||
|
|
||||
|
修改历史 : |
||||
|
1.日 期 : 2026年7月13日 |
||||
|
作 者 : radkil |
||||
|
修改内容 : 创建文件 |
||||
|
|
||||
|
******************************************************************************/ |
||||
|
|
||||
|
#ifndef __MOTOR_LEISAI_H__ |
||||
|
#define __MOTOR_LEISAI_H__ |
||||
|
|
||||
|
#ifdef __cplusplus |
||||
|
#if __cplusplus |
||||
|
extern "C"{ |
||||
|
#endif |
||||
|
#endif /* __cplusplus */ |
||||
|
|
||||
|
/*==============================================*
|
||||
|
* include header files * |
||||
|
*----------------------------------------------*/ |
||||
|
#include "motor_manager.h" |
||||
|
|
||||
|
/*==============================================*
|
||||
|
* constants or macros define * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
// SDO命令字
|
||||
|
#define LEISAI_SDO_CMD_READ 0x40 |
||||
|
#define LEISAI_SDO_CMD_WRITE_1B 0x2F |
||||
|
#define LEISAI_SDO_CMD_WRITE_2B 0x2B |
||||
|
#define LEISAI_SDO_CMD_WRITE_3B 0x27 |
||||
|
#define LEISAI_SDO_CMD_WRITE_4B 0x23 |
||||
|
|
||||
|
// CANopen 对象字典索引
|
||||
|
#define LEISAI_OD_CONTROL_WORD 0x6040 |
||||
|
#define LEISAI_OD_STATUS_WORD 0x6041 |
||||
|
#define LEISAI_OD_MODE_OF_OPERATION 0x6060 |
||||
|
#define LEISAI_OD_POSITION_ACTUAL 0x6064 |
||||
|
#define LEISAI_OD_VELOCITY_ACTUAL 0x606C |
||||
|
#define LEISAI_OD_TORQUE_ACTUAL 0x6077 |
||||
|
#define LEISAI_OD_TARGET_VELOCITY 0x60FF |
||||
|
#define LEISAI_OD_TARGET_POSITION 0x607A |
||||
|
#define LEISAI_OD_ACCELERATION 0x6083 |
||||
|
#define LEISAI_OD_DECELERATION 0x6084 |
||||
|
#define LEISAI_OD_FAULT_CODE 0x603F |
||||
|
#define LEISAI_OD_HOMING_METHOD 0x6098 |
||||
|
#define LEISAI_OD_WATCHDOG_TIME 0x5004 |
||||
|
|
||||
|
// 操作模式
|
||||
|
#define LEISAI_MODE_PROFILE_POS 0x01 |
||||
|
#define LEISAI_MODE_PROFILE_VEL 0x03 |
||||
|
#define LEISAI_MODE_HOMING 0x06 |
||||
|
|
||||
|
// 控制字命令
|
||||
|
#define LEISAI_CTRL_SHUTDOWN 0x06 |
||||
|
#define LEISAI_CTRL_SWITCH_ON 0x07 |
||||
|
#define LEISAI_CTRL_ENABLE 0x0F |
||||
|
#define LEISAI_CTRL_START_MOVE 0x5F |
||||
|
#define LEISAI_CTRL_RELATIVE_MOVE 0x4F |
||||
|
|
||||
|
/*==============================================*
|
||||
|
* data structures * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*==============================================*
|
||||
|
* project-wide global variables * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
// 雷赛协议操作函数表（全局单例）
|
||||
|
extern const Motor_Protocol_t g_stLeiSaiProtocol; |
||||
|
|
||||
|
/*==============================================*
|
||||
|
* routines' or functions' implementations * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/**
|
||||
|
* @brief 获取雷赛协议操作函数表 |
||||
|
* @return 协议操作函数表指针 |
||||
|
*/ |
||||
|
const Motor_Protocol_t* LeiSai_GetProtocol(void); |
||||
|
|
||||
|
#ifdef __cplusplus |
||||
|
#if __cplusplus |
||||
|
} |
||||
|
#endif |
||||
|
#endif /* __cplusplus */ |
||||
|
|
||||
|
#endif /* __MOTOR_LEISAI_H__ */ |
||||
@ -0,0 +1,334 @@ |
|||||
|
/******************************************************************************
|
||||
|
|
||||
|
版权所有 (C), 2018-2099, Radkil |
||||
|
|
||||
|
****************************************************************************** |
||||
|
文 件 名 : motor_example.c |
||||
|
版 本 号 : 初稿 |
||||
|
作 者 : radkil |
||||
|
生成日期 : 2026年7月13日 |
||||
|
最近修改 : |
||||
|
功能描述 : 电机模块使用示例 |
||||
|
|
||||
|
修改历史 : |
||||
|
1.日 期 : 2026年7月13日 |
||||
|
作 者 : radkil |
||||
|
修改内容 : 创建文件 |
||||
|
|
||||
|
******************************************************************************/ |
||||
|
#include "BHBF.h" |
||||
|
#include "motor_manager.h" |
||||
|
#include "msp_motor_leisai.h" |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 外部变量说明 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 外部函数原型说明 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 内部函数原型说明 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 全局变量 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 模块级变量 * |
||||
|
*----------------------------------------------*/ |
||||
|
static Motor_Instance_t *g_apstMotors[MOTOR_INSTANCE_MAX] = {NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL}; |
||||
|
static uint32_t g_uiMotorModuleID = 0; |
||||
|
/*----------------------------------------------*
|
||||
|
* 常量定义 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 宏定义 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
#if 0 |
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MotorExample_LeiSai |
||||
|
功能描述 : 雷赛电机使用示例 |
||||
|
输入参数 : 无 |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : void |
||||
|
*****************************************************************************/ |
||||
|
void MotorExample_LeiSai(void) |
||||
|
{ |
||||
|
// 1. 初始化电机管理模块
|
||||
|
MotorMgr_Init(); |
||||
|
|
||||
|
// 2. 配置左轮电机
|
||||
|
Motor_Config_t stLeftConfig = { |
||||
|
.m_ucMotorID = 1, |
||||
|
.m_uiPulsePerRound = 10000, |
||||
|
.m_uiReductionRatio = 70, |
||||
|
.m_fWheelDiameter = 0.26f |
||||
|
}; |
||||
|
|
||||
|
// 3. 创建电机实例（使用雷赛协议）
|
||||
|
Motor_Instance_t *pstLeftMotor = MotorMgr_Create(&stLeftConfig, g_ptFDCAN1, LeiSai_GetProtocol(), NULL); |
||||
|
|
||||
|
// 4. 配置右轮电机
|
||||
|
Motor_Config_t stRightConfig = { |
||||
|
.m_ucMotorID = 2, |
||||
|
.m_uiPulsePerRound = 10000, |
||||
|
.m_uiReductionRatio = 70, |
||||
|
.m_fWheelDiameter = 0.26f |
||||
|
}; |
||||
|
Motor_Instance_t *pstRightMotor = MotorMgr_Create(&stRightConfig, g_ptFDCAN1, LeiSai_GetProtocol(), NULL); |
||||
|
|
||||
|
// 5. 复位并激活所有电机
|
||||
|
if (pstLeftMotor != NULL) |
||||
|
{ |
||||
|
MotorMgr_ResetAll(pstLeftMotor); |
||||
|
Rd_Delay(1000); |
||||
|
MotorMgr_ActivateAll(pstLeftMotor); |
||||
|
Rd_Delay(1000); |
||||
|
} |
||||
|
|
||||
|
// 6. 设置当前位置为零点
|
||||
|
if (pstLeftMotor != NULL) |
||||
|
{ |
||||
|
MotorMgr_SetHome(pstLeftMotor); |
||||
|
Rd_Delay(100); |
||||
|
} |
||||
|
if (pstRightMotor != NULL) |
||||
|
{ |
||||
|
MotorMgr_SetHome(pstRightMotor); |
||||
|
Rd_Delay(100); |
||||
|
} |
||||
|
|
||||
|
// 7. 初始化速度模式
|
||||
|
if (pstLeftMotor != NULL) |
||||
|
{ |
||||
|
MotorMgr_SpeedModeInit(pstLeftMotor); |
||||
|
} |
||||
|
if (pstRightMotor != NULL) |
||||
|
{ |
||||
|
MotorMgr_SpeedModeInit(pstRightMotor); |
||||
|
} |
||||
|
|
||||
|
// 8. 设置目标速度
|
||||
|
if (pstLeftMotor != NULL) |
||||
|
{ |
||||
|
MotorMgr_SetTargetSpeed(pstLeftMotor, 100000); |
||||
|
} |
||||
|
if (pstRightMotor != NULL) |
||||
|
{ |
||||
|
MotorMgr_SetTargetSpeed(pstRightMotor, -100000); |
||||
|
} |
||||
|
|
||||
|
// 9. 定期读取电机状态
|
||||
|
while(1) |
||||
|
{ |
||||
|
if (pstLeftMotor != NULL) |
||||
|
{ |
||||
|
MotorMgr_RequestPosition(pstLeftMotor); |
||||
|
MotorMgr_RequestVelocity(pstLeftMotor); |
||||
|
MotorMgr_RequestFault(pstLeftMotor); |
||||
|
|
||||
|
const Motor_Data_t *pstData = MotorMgr_GetData(pstLeftMotor); |
||||
|
if (pstData != NULL) |
||||
|
{ |
||||
|
RD_PRINTF("Left Motor: Pos=%ld, Vel=%ld, Dist=%.3f m\n", |
||||
|
pstData->m_iRealPosition, |
||||
|
pstData->m_iRealVelocity, |
||||
|
pstData->m_dRealDistance); |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
Rd_Delay(100); |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MotorExample_MultiProtocol |
||||
|
功能描述 : 多协议使用示例（展示架构扩展性） |
||||
|
输入参数 : 无 |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : void |
||||
|
|
||||
|
说明：假设未来有其他协议实现，只需： |
||||
|
1. 实现 Motor_Protocol_t 函数表 |
||||
|
2. 调用 MotorMgr_Create 时传入不同的协议 |
||||
|
*****************************************************************************/ |
||||
|
void MotorExample_MultiProtocol(void) |
||||
|
{ |
||||
|
MotorMgr_Init(); |
||||
|
|
||||
|
// 使用雷赛协议的电机
|
||||
|
Motor_Config_t stConfig1 = { .m_ucMotorID = 1 }; |
||||
|
Motor_Instance_t *pstMotor1 = MotorMgr_Create(&stConfig1, g_ptFDCAN1, LeiSai_GetProtocol(), NULL); |
||||
|
|
||||
|
// 假设未来有其他协议
|
||||
|
// Motor_Config_t stConfig2 = { .m_ucMotorID = 2 };
|
||||
|
// Motor_Instance_t *pstMotor2 = MotorMgr_Create(&stConfig2, g_ptFDCAN2, OtherProtocol_GetProtocol(), NULL);
|
||||
|
|
||||
|
// 统一的管理接口
|
||||
|
MotorMgr_SpeedModeInit(pstMotor1); |
||||
|
MotorMgr_SetTargetSpeed(pstMotor1, 50000); |
||||
|
|
||||
|
// 通过名称查找
|
||||
|
Motor_Instance_t *pstFound = MotorMgr_FindByName("Motor_1"); |
||||
|
if (pstFound != NULL) |
||||
|
{ |
||||
|
RD_PRINTF("Found motor: %s\n", pstFound->m_szName); |
||||
|
} |
||||
|
} |
||||
|
#endif |
||||
|
|
||||
|
void MotorTask(void *argument) |
||||
|
{ |
||||
|
// 等待系统稳定
|
||||
|
Rd_Delay(1000); |
||||
|
|
||||
|
// 复位并激活所有电机
|
||||
|
for (int i = 0; i < MOTOR_INSTANCE_MAX; i++) |
||||
|
{ |
||||
|
if (g_apstMotors[i] != NULL) |
||||
|
{ |
||||
|
MotorMgr_ResetAll(g_apstMotors[i]); |
||||
|
Rd_Delay(500); |
||||
|
MotorMgr_ActivateAll(g_apstMotors[i]); |
||||
|
Rd_Delay(500); |
||||
|
MotorMgr_SetHome(g_apstMotors[i]); |
||||
|
Rd_Delay(100); |
||||
|
MotorMgr_SpeedModeInit(g_apstMotors[i]); |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
while(1) |
||||
|
{ |
||||
|
// 处理消息
|
||||
|
MsgCenter_ProcessWait(g_uiMotorModuleID, 2); |
||||
|
|
||||
|
for (int i = 0; i < MOTOR_INSTANCE_MAX; i++) |
||||
|
{ |
||||
|
if (g_apstMotors[i] != NULL) |
||||
|
{ |
||||
|
// 读取位置、速度、故障
|
||||
|
MotorMgr_RequestPosition(g_apstMotors[i]); |
||||
|
MotorMgr_RequestFault(g_apstMotors[i]); |
||||
|
MotorMgr_RequestVelocity(g_apstMotors[i]); |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
Rd_Delay(2); // 2ms周期
|
||||
|
} |
||||
|
} |
||||
|
|
||||
|
/*==============================================*
|
||||
|
* 电机模块消息处理 * |
||||
|
*==============================================*/ |
||||
|
|
||||
|
static void Motor_ModuleHandler(const Msg_t *pstMsg) |
||||
|
{ |
||||
|
if (NULL == pstMsg) |
||||
|
{ |
||||
|
return; |
||||
|
} |
||||
|
|
||||
|
switch (pstMsg->m_uiMsgID) |
||||
|
{ |
||||
|
case MOTOR_CMD_SET_SPEED: |
||||
|
{ |
||||
|
if (pstMsg->m_uiDataLen >= sizeof(Motor_CmdData_t)) |
||||
|
{ |
||||
|
Motor_CmdData_t *pstData = (Motor_CmdData_t *)pstMsg->m_aucData; |
||||
|
if (pstData->m_ucMotorIndex < 2 && g_apstMotors[pstData->m_ucMotorIndex] != NULL) |
||||
|
{ |
||||
|
MotorMgr_SetTargetSpeed(g_apstMotors[pstData->m_ucMotorIndex], pstData->m_iValue); |
||||
|
} |
||||
|
} |
||||
|
break; |
||||
|
} |
||||
|
case MOTOR_CMD_STOP: |
||||
|
{ |
||||
|
for (int i = 0; i < 2; i++) |
||||
|
{ |
||||
|
if (g_apstMotors[i] != NULL) |
||||
|
{ |
||||
|
MotorMgr_SetTargetSpeed(g_apstMotors[i], 0); |
||||
|
} |
||||
|
} |
||||
|
break; |
||||
|
} |
||||
|
case MOTOR_CMD_SET_HOME: |
||||
|
{ |
||||
|
if (pstMsg->m_uiDataLen >= sizeof(Motor_CmdData_t)) |
||||
|
{ |
||||
|
Motor_CmdData_t *pstData = (Motor_CmdData_t *)pstMsg->m_aucData; |
||||
|
if (pstData->m_ucMotorIndex < 2 && g_apstMotors[pstData->m_ucMotorIndex] != NULL) |
||||
|
{ |
||||
|
MotorMgr_SetHome(g_apstMotors[pstData->m_ucMotorIndex]); |
||||
|
} |
||||
|
} |
||||
|
break; |
||||
|
} |
||||
|
case MOTOR_CMD_GET_STATUS: |
||||
|
{ |
||||
|
// 回复状态给请求者
|
||||
|
Motor_StatusData_t stStatus; |
||||
|
for (int i = 0; i < 2; i++) |
||||
|
{ |
||||
|
if (g_apstMotors[i] != NULL) |
||||
|
{ |
||||
|
const Motor_Data_t *pstData = MotorMgr_GetData(g_apstMotors[i]); |
||||
|
if (pstData != NULL) |
||||
|
{ |
||||
|
stStatus.m_ucMotorIndex = i; |
||||
|
stStatus.m_iPosition = pstData->m_iRealPosition; |
||||
|
stStatus.m_iVelocity = pstData->m_iRealVelocity; |
||||
|
stStatus.m_uiFaultCode = pstData->m_uiFaultCode; |
||||
|
|
||||
|
// 回复给请求者
|
||||
|
MsgCenter_SendTo(MODULE_NAME_RBCORE, 0x10, &stStatus, sizeof(stStatus)); |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
break; |
||||
|
} |
||||
|
default: |
||||
|
break; |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
void MotorInit(void) |
||||
|
{ |
||||
|
// 初始化电机管理模块
|
||||
|
MotorMgr_Init(); |
||||
|
|
||||
|
// 配置左轮电机
|
||||
|
Motor_Config_t stLeftMotorConfig = { |
||||
|
.m_ucMotorID = 1, |
||||
|
.m_uiPulsePerRound = 10000, |
||||
|
.m_uiReductionRatio = 70, |
||||
|
.m_fWheelDiameter = 0.26f |
||||
|
}; |
||||
|
g_apstMotors[0] = MotorMgr_Create(&stLeftMotorConfig, g_ptFDCAN1, LeiSai_GetProtocol(), NULL); |
||||
|
|
||||
|
// 配置右轮电机
|
||||
|
Motor_Config_t stRightMotorConfig = { |
||||
|
.m_ucMotorID = 2, |
||||
|
.m_uiPulsePerRound = 10000, |
||||
|
.m_uiReductionRatio = 70, |
||||
|
.m_fWheelDiameter = 0.26f |
||||
|
}; |
||||
|
g_apstMotors[1] = MotorMgr_Create(&stRightMotorConfig, g_ptFDCAN1, LeiSai_GetProtocol(), NULL); |
||||
|
|
||||
|
g_uiMotorModuleID = MsgCenter_Register(MODULE_NAME_MOTOR, Motor_ModuleHandler); |
||||
|
|
||||
|
// 创建电机任务
|
||||
|
const osThreadAttr_t motor_task_attributes = { |
||||
|
.name = "motor_task", |
||||
|
.stack_size = 1024, |
||||
|
.priority = (osPriority_t) osPriorityHigh5, |
||||
|
}; |
||||
|
(void)osThreadNew(MotorTask, NULL, &motor_task_attributes); |
||||
|
} |
||||
|
|
||||
@ -0,0 +1,512 @@ |
|||||
|
/******************************************************************************
|
||||
|
|
||||
|
版权所有 (C), 2018-2099, Radkil |
||||
|
|
||||
|
****************************************************************************** |
||||
|
文 件 名 : motor_manager.c |
||||
|
版 本 号 : 初稿 |
||||
|
作 者 : radkil |
||||
|
生成日期 : 2026年7月13日 |
||||
|
最近修改 : |
||||
|
功能描述 : 电机管理模块（多实例管理，协议无关） |
||||
|
|
||||
|
修改历史 : |
||||
|
1.日 期 : 2026年7月13日 |
||||
|
作 者 : radkil |
||||
|
修改内容 : 创建文件 |
||||
|
|
||||
|
******************************************************************************/ |
||||
|
|
||||
|
#include "motor_manager.h" |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 外部变量说明 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 外部函数原型说明 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 内部函数原型说明 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 全局变量 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 模块级变量 * |
||||
|
*----------------------------------------------*/ |
||||
|
static rd_slist_t g_stMotorList; // 电机链表头
|
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 常量定义 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 宏定义 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
// 宏：检查协议函数是否存在并调用
|
||||
|
#define MOTOR_PROTOCOL_CALL(pstMotor, func, ...) \ |
||||
|
((pstMotor)->m_pstProtocol && (pstMotor)->m_pstProtocol->func) ? \ |
||||
|
(pstMotor)->m_pstProtocol->func(pstMotor, ##__VA_ARGS__) : RD_NULL |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MotorMgr_Init |
||||
|
功能描述 : 初始化电机管理模块 |
||||
|
输入参数 : 无 |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : void |
||||
|
调用函数 : |
||||
|
被调函数 : |
||||
|
|
||||
|
修改历史 : |
||||
|
1.日 期 : 2026年7月13日 |
||||
|
作 者 : radkil |
||||
|
修改内容 : 新生成函数 |
||||
|
|
||||
|
*****************************************************************************/ |
||||
|
void MotorMgr_Init(void) |
||||
|
{ |
||||
|
rd_slist_init(&g_stMotorList); |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MotorMgr_Create |
||||
|
功能描述 : 创建电机实例 |
||||
|
输入参数 : const Motor_Config_t *pstConfig 电机配置 |
||||
|
TComCtrl *ptCAN 绑定的CAN控制器 |
||||
|
const Motor_Protocol_t *pstProtocol 协议操作函数表 |
||||
|
void *pstPrivData 协议私有数据 |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : Motor_Instance_t* 电机实例指针 |
||||
|
调用函数 : |
||||
|
被调函数 : |
||||
|
|
||||
|
修改历史 : |
||||
|
1.日 期 : 2026年7月13日 |
||||
|
作 者 : radkil |
||||
|
修改内容 : 新生成函数 |
||||
|
|
||||
|
*****************************************************************************/ |
||||
|
Motor_Instance_t* MotorMgr_Create(const Motor_Config_t *pstConfig, |
||||
|
TComCtrl *ptCAN, |
||||
|
const Motor_Protocol_t *pstProtocol, |
||||
|
void *pstPrivData) |
||||
|
{ |
||||
|
if (NULL == pstConfig || NULL == ptCAN || NULL == pstProtocol) |
||||
|
{ |
||||
|
return NULL; |
||||
|
} |
||||
|
|
||||
|
// 检查是否已存在相同ID的电机
|
||||
|
rd_slist_t *pos; |
||||
|
rd_slist_t *n; |
||||
|
rd_slist_for_each(pos, n, &g_stMotorList) |
||||
|
{ |
||||
|
Motor_Instance_t *pstCheck = rd_slist_entry(pos, Motor_Instance_t, node); |
||||
|
if (pstCheck->m_stConfig.m_ucMotorID == pstConfig->m_ucMotorID) |
||||
|
{ |
||||
|
return NULL; // ID重复
|
||||
|
} |
||||
|
} |
||||
|
|
||||
|
// 分配内存
|
||||
|
Motor_Instance_t *pstMotor = (Motor_Instance_t *)RD_CALLOC(1, sizeof(Motor_Instance_t)); |
||||
|
if (NULL == pstMotor) |
||||
|
{ |
||||
|
return NULL; |
||||
|
} |
||||
|
|
||||
|
// 初始化链表节点
|
||||
|
pstMotor->node.next = NULL; |
||||
|
|
||||
|
// 复制配置
|
||||
|
pstMotor->m_stConfig.m_ucMotorID = pstConfig->m_ucMotorID; |
||||
|
pstMotor->m_stConfig.m_uiPulsePerRound = pstConfig->m_uiPulsePerRound > 0 ? pstConfig->m_uiPulsePerRound : 10000; |
||||
|
pstMotor->m_stConfig.m_uiReductionRatio = pstConfig->m_uiReductionRatio > 0 ? pstConfig->m_uiReductionRatio : 70; |
||||
|
pstMotor->m_stConfig.m_fWheelDiameter = pstConfig->m_fWheelDiameter > 0 ? pstConfig->m_fWheelDiameter : 0.26f; |
||||
|
|
||||
|
// 初始化运行数据
|
||||
|
pstMotor->m_stData.m_iRealPosition = 0; |
||||
|
pstMotor->m_stData.m_iRealVelocity = 0; |
||||
|
pstMotor->m_stData.m_iRealTorque = 0; |
||||
|
pstMotor->m_stData.m_uiFaultCode = 0; |
||||
|
pstMotor->m_stData.m_dRealDistance = 0.0; |
||||
|
pstMotor->m_stData.m_iStartMeasurePos = 0; |
||||
|
pstMotor->m_stData.m_iLastPosition = 0; |
||||
|
pstMotor->m_stData.m_iRoundCount = 0; |
||||
|
pstMotor->m_stData.m_ucStartMeasureFlag = 1; |
||||
|
|
||||
|
// 绑定CAN控制器和协议
|
||||
|
pstMotor->m_ptCAN = ptCAN; |
||||
|
pstMotor->m_eState = MOTOR_STATE_IDLE; |
||||
|
pstMotor->m_uiLastHeartbeat = 0; |
||||
|
pstMotor->m_pstProtocol = pstProtocol; |
||||
|
pstMotor->m_pstPrivData = pstPrivData; |
||||
|
|
||||
|
// 设置实例名称
|
||||
|
RD_SNPRINTF(pstMotor->m_szName, MOTOR_MAX_NAME_LEN, "Motor_%d", pstConfig->m_ucMotorID); |
||||
|
|
||||
|
// 添加到链表尾部
|
||||
|
rd_slist_append(&g_stMotorList, &pstMotor->node); |
||||
|
|
||||
|
return pstMotor; |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MotorMgr_Destroy |
||||
|
功能描述 : 销毁电机实例 |
||||
|
输入参数 : Motor_Instance_t *pstMotor 电机实例 |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : int 0成功，非0失败 |
||||
|
调用函数 : |
||||
|
被调函数 : |
||||
|
|
||||
|
修改历史 : |
||||
|
1.日 期 : 2026年7月13日 |
||||
|
作 者 : radkil |
||||
|
修改内容 : 新生成函数 |
||||
|
|
||||
|
*****************************************************************************/ |
||||
|
int MotorMgr_Destroy(Motor_Instance_t *pstMotor) |
||||
|
{ |
||||
|
if (NULL == pstMotor) |
||||
|
{ |
||||
|
return RD_NULL; |
||||
|
} |
||||
|
|
||||
|
// 从链表中移除
|
||||
|
rd_slist_remove(&g_stMotorList, &pstMotor->node); |
||||
|
RD_FREE(pstMotor); |
||||
|
|
||||
|
return RD_SUCCESS; |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MotorMgr_FindByName |
||||
|
功能描述 : 通过名称查找电机实例 |
||||
|
输入参数 : const char *pszName 实例名称 |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : Motor_Instance_t* 电机实例指针 |
||||
|
调用函数 : |
||||
|
被调函数 : |
||||
|
|
||||
|
修改历史 : |
||||
|
1.日 期 : 2026年7月13日 |
||||
|
作 者 : radkil |
||||
|
修改内容 : 新生成函数 |
||||
|
|
||||
|
*****************************************************************************/ |
||||
|
Motor_Instance_t* MotorMgr_FindByName(const char *pszName) |
||||
|
{ |
||||
|
if (NULL == pszName) |
||||
|
{ |
||||
|
return NULL; |
||||
|
} |
||||
|
|
||||
|
rd_slist_t *pos; |
||||
|
rd_slist_t *n; |
||||
|
rd_slist_for_each(pos, n, &g_stMotorList) |
||||
|
{ |
||||
|
Motor_Instance_t *pstMotor = rd_slist_entry(pos, Motor_Instance_t, node); |
||||
|
if (RD_STRCMP(pstMotor->m_szName, pszName) == 0) |
||||
|
{ |
||||
|
return pstMotor; |
||||
|
} |
||||
|
} |
||||
|
return NULL; |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MotorMgr_FindByID |
||||
|
功能描述 : 通过ID查找电机实例 |
||||
|
输入参数 : uint8_t ucMotorID 电机节点ID |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : Motor_Instance_t* 电机实例指针 |
||||
|
调用函数 : |
||||
|
被调函数 : |
||||
|
|
||||
|
修改历史 : |
||||
|
1.日 期 : 2026年7月13日 |
||||
|
作 者 : radkil |
||||
|
修改内容 : 新生成函数 |
||||
|
|
||||
|
*****************************************************************************/ |
||||
|
Motor_Instance_t* MotorMgr_FindByID(uint8_t ucMotorID) |
||||
|
{ |
||||
|
rd_slist_t *pos; |
||||
|
rd_slist_t *n; |
||||
|
rd_slist_for_each(pos, n, &g_stMotorList) |
||||
|
{ |
||||
|
Motor_Instance_t *pstMotor = rd_slist_entry(pos, Motor_Instance_t, node); |
||||
|
if (pstMotor->m_stConfig.m_ucMotorID == ucMotorID) |
||||
|
{ |
||||
|
return pstMotor; |
||||
|
} |
||||
|
} |
||||
|
return NULL; |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MotorMgr_ResetAll |
||||
|
功能描述 : 复位所有节点 |
||||
|
输入参数 : Motor_Instance_t *pstMotor 电机实例 |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : int 0成功，非0失败 |
||||
|
*****************************************************************************/ |
||||
|
int MotorMgr_ResetAll(Motor_Instance_t *pstMotor) |
||||
|
{ |
||||
|
return MOTOR_PROTOCOL_CALL(pstMotor, pfResetAll); |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MotorMgr_ActivateAll |
||||
|
功能描述 : 激活所有节点 |
||||
|
输入参数 : Motor_Instance_t *pstMotor 电机实例 |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : int 0成功，非0失败 |
||||
|
*****************************************************************************/ |
||||
|
int MotorMgr_ActivateAll(Motor_Instance_t *pstMotor) |
||||
|
{ |
||||
|
return MOTOR_PROTOCOL_CALL(pstMotor, pfActivateAll); |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MotorMgr_SetHome |
||||
|
功能描述 : 设置当前位置为零点 |
||||
|
输入参数 : Motor_Instance_t *pstMotor 电机实例 |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : int 0成功，非0失败 |
||||
|
*****************************************************************************/ |
||||
|
int MotorMgr_SetHome(Motor_Instance_t *pstMotor) |
||||
|
{ |
||||
|
return MOTOR_PROTOCOL_CALL(pstMotor, pfSetHome); |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MotorMgr_SetWatchdog |
||||
|
功能描述 : 设置看门狗时间 |
||||
|
输入参数 : Motor_Instance_t *pstMotor 电机实例 |
||||
|
uint32_t uiTimeMs 毫秒 |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : int 0成功，非0失败 |
||||
|
*****************************************************************************/ |
||||
|
int MotorMgr_SetWatchdog(Motor_Instance_t *pstMotor, uint32_t uiTimeMs) |
||||
|
{ |
||||
|
return MOTOR_PROTOCOL_CALL(pstMotor, pfSetWatchdog, uiTimeMs); |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MotorMgr_RequestPosition |
||||
|
功能描述 : 读取电机位置 |
||||
|
输入参数 : Motor_Instance_t *pstMotor 电机实例 |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : int 0成功，非0失败 |
||||
|
*****************************************************************************/ |
||||
|
int MotorMgr_RequestPosition(Motor_Instance_t *pstMotor) |
||||
|
{ |
||||
|
return MOTOR_PROTOCOL_CALL(pstMotor, pfRequestPosition); |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MotorMgr_RequestVelocity |
||||
|
功能描述 : 读取电机速度 |
||||
|
输入参数 : Motor_Instance_t *pstMotor 电机实例 |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : int 0成功，非0失败 |
||||
|
*****************************************************************************/ |
||||
|
int MotorMgr_RequestVelocity(Motor_Instance_t *pstMotor) |
||||
|
{ |
||||
|
return MOTOR_PROTOCOL_CALL(pstMotor, pfRequestVelocity); |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MotorMgr_RequestTorque |
||||
|
功能描述 : 读取电机电流/转矩 |
||||
|
输入参数 : Motor_Instance_t *pstMotor 电机实例 |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : int 0成功，非0失败 |
||||
|
*****************************************************************************/ |
||||
|
int MotorMgr_RequestTorque(Motor_Instance_t *pstMotor) |
||||
|
{ |
||||
|
return MOTOR_PROTOCOL_CALL(pstMotor, pfRequestTorque); |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MotorMgr_RequestFault |
||||
|
功能描述 : 读取故障码 |
||||
|
输入参数 : Motor_Instance_t *pstMotor 电机实例 |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : int 0成功，非0失败 |
||||
|
*****************************************************************************/ |
||||
|
int MotorMgr_RequestFault(Motor_Instance_t *pstMotor) |
||||
|
{ |
||||
|
return MOTOR_PROTOCOL_CALL(pstMotor, pfRequestFault); |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MotorMgr_SpeedModeInit |
||||
|
功能描述 : 速度模式初始化 |
||||
|
输入参数 : Motor_Instance_t *pstMotor 电机实例 |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : int 0成功，非0失败 |
||||
|
*****************************************************************************/ |
||||
|
int MotorMgr_SpeedModeInit(Motor_Instance_t *pstMotor) |
||||
|
{ |
||||
|
int iRet = MOTOR_PROTOCOL_CALL(pstMotor, pfSpeedModeInit); |
||||
|
if (iRet == RD_SUCCESS) |
||||
|
{ |
||||
|
pstMotor->m_eState = MOTOR_STATE_READY; |
||||
|
} |
||||
|
return iRet; |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MotorMgr_SetTargetSpeed |
||||
|
功能描述 : 设置目标速度 |
||||
|
输入参数 : Motor_Instance_t *pstMotor 电机实例 |
||||
|
int32_t iTargetSpeed 目标速度(脉冲/秒) |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : int 0成功，非0失败 |
||||
|
*****************************************************************************/ |
||||
|
int MotorMgr_SetTargetSpeed(Motor_Instance_t *pstMotor, int32_t iTargetSpeed) |
||||
|
{ |
||||
|
int iRet = MOTOR_PROTOCOL_CALL(pstMotor, pfSetTargetSpeed, iTargetSpeed); |
||||
|
if (iRet == RD_SUCCESS) |
||||
|
{ |
||||
|
pstMotor->m_eState = MOTOR_STATE_RUNNING; |
||||
|
} |
||||
|
return iRet; |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MotorMgr_PositionModeInit |
||||
|
功能描述 : 位置模式初始化 |
||||
|
输入参数 : Motor_Instance_t *pstMotor 电机实例 |
||||
|
int32_t iAccel 加速度(脉冲/秒²) |
||||
|
int32_t iDecel 减速度(脉冲/秒²) |
||||
|
int32_t iTargetSpeed 目标速度(脉冲/秒) |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : int 0成功，非0失败 |
||||
|
*****************************************************************************/ |
||||
|
int MotorMgr_PositionModeInit(Motor_Instance_t *pstMotor, int32_t iAccel, int32_t iDecel, int32_t iTargetSpeed) |
||||
|
{ |
||||
|
int iRet = MOTOR_PROTOCOL_CALL(pstMotor, pfPositionModeInit, iAccel, iDecel, iTargetSpeed); |
||||
|
if (iRet == RD_SUCCESS) |
||||
|
{ |
||||
|
pstMotor->m_eState = MOTOR_STATE_READY; |
||||
|
} |
||||
|
return iRet; |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MotorMgr_SetTargetPosition |
||||
|
功能描述 : 设置目标位置（相对运动） |
||||
|
输入参数 : Motor_Instance_t *pstMotor 电机实例 |
||||
|
int32_t iTargetPos 目标位置(脉冲) |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : int 0成功，非0失败 |
||||
|
*****************************************************************************/ |
||||
|
int MotorMgr_SetTargetPosition(Motor_Instance_t *pstMotor, int32_t iTargetPos) |
||||
|
{ |
||||
|
int iRet = MOTOR_PROTOCOL_CALL(pstMotor, pfSetTargetPosition, iTargetPos); |
||||
|
if (iRet == RD_SUCCESS) |
||||
|
{ |
||||
|
pstMotor->m_eState = MOTOR_STATE_RUNNING; |
||||
|
} |
||||
|
return iRet; |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MotorMgr_SetDO |
||||
|
功能描述 : 设置数字输出 |
||||
|
输入参数 : Motor_Instance_t *pstMotor 电机实例 |
||||
|
uint8_t ucDoIndex 输出索引(1或2) |
||||
|
uint8_t ucState 状态(0或1) |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : int 0成功，非0失败 |
||||
|
*****************************************************************************/ |
||||
|
int MotorMgr_SetDO(Motor_Instance_t *pstMotor, uint8_t ucDoIndex, uint8_t ucState) |
||||
|
{ |
||||
|
return MOTOR_PROTOCOL_CALL(pstMotor, pfSetDO, ucDoIndex, ucState); |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MotorMgr_ParseResponse |
||||
|
功能描述 : 解析CAN响应数据（遍历所有电机实例尝试解析） |
||||
|
输入参数 : uint8_t ucMotorID 电机节点ID |
||||
|
const uint8_t *pucData 数据指针 |
||||
|
uint32_t uiLen 数据长度 |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : void |
||||
|
*****************************************************************************/ |
||||
|
void MotorMgr_ParseResponse(uint8_t ucMotorID, const uint8_t *pucData, uint32_t uiLen) |
||||
|
{ |
||||
|
if (NULL == pucData || uiLen < 8) |
||||
|
{ |
||||
|
return; |
||||
|
} |
||||
|
|
||||
|
Motor_Instance_t *pstMotor = MotorMgr_FindByID(ucMotorID); |
||||
|
if (NULL == pstMotor) |
||||
|
{ |
||||
|
return; |
||||
|
} |
||||
|
|
||||
|
if (pstMotor->m_pstProtocol && pstMotor->m_pstProtocol->pfParseResponse) |
||||
|
{ |
||||
|
pstMotor->m_pstProtocol->pfParseResponse(pstMotor, pucData, uiLen); |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MotorMgr_SpeedConvert |
||||
|
功能描述 : 速度单位转换 |
||||
|
输入参数 : Motor_Instance_t *pstMotor 电机实例 |
||||
|
int32_t iSpeedUserUnit 用户速度单位 |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : int32_t 脉冲/秒 |
||||
|
*****************************************************************************/ |
||||
|
int32_t MotorMgr_SpeedConvert(Motor_Instance_t *pstMotor, int32_t iSpeedUserUnit) |
||||
|
{ |
||||
|
if (pstMotor->m_pstProtocol && pstMotor->m_pstProtocol->pfSpeedConvert) |
||||
|
{ |
||||
|
return pstMotor->m_pstProtocol->pfSpeedConvert(pstMotor, iSpeedUserUnit); |
||||
|
} |
||||
|
return 0; |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MotorMgr_GetData |
||||
|
功能描述 : 获取电机数据 |
||||
|
输入参数 : Motor_Instance_t *pstMotor 电机实例 |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : const Motor_Data_t* |
||||
|
*****************************************************************************/ |
||||
|
const Motor_Data_t* MotorMgr_GetData(Motor_Instance_t *pstMotor) |
||||
|
{ |
||||
|
if (NULL == pstMotor) |
||||
|
{ |
||||
|
return NULL; |
||||
|
} |
||||
|
return &pstMotor->m_stData; |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : MotorMgr_GetState |
||||
|
功能描述 : 获取电机状态 |
||||
|
输入参数 : Motor_Instance_t *pstMotor 电机实例 |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : Motor_State_e |
||||
|
*****************************************************************************/ |
||||
|
Motor_State_e MotorMgr_GetState(Motor_Instance_t *pstMotor) |
||||
|
{ |
||||
|
if (NULL == pstMotor) |
||||
|
{ |
||||
|
return MOTOR_STATE_IDLE; |
||||
|
} |
||||
|
return pstMotor->m_eState; |
||||
|
} |
||||
@ -0,0 +1,486 @@ |
|||||
|
/******************************************************************************
|
||||
|
|
||||
|
版权所有 (C), 2018-2099, Radkil |
||||
|
|
||||
|
****************************************************************************** |
||||
|
文 件 名 : msp_motor_leisai.c |
||||
|
版 本 号 : 初稿 |
||||
|
作 者 : radkil |
||||
|
生成日期 : 2026年7月13日 |
||||
|
最近修改 : |
||||
|
功能描述 : 雷赛ISV2电机CAN协议实现 |
||||
|
|
||||
|
修改历史 : |
||||
|
1.日 期 : 2026年7月13日 |
||||
|
作 者 : radkil |
||||
|
修改内容 : 创建文件 |
||||
|
|
||||
|
******************************************************************************/ |
||||
|
|
||||
|
#include "msp_motor_leisai.h" |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 外部变量说明 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 外部函数原型说明 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 内部函数原型说明 * |
||||
|
*----------------------------------------------*/ |
||||
|
static int LeiSai_SendSDO(Motor_Instance_t *pstMotor, uint8_t ucCmd, |
||||
|
uint16_t usIndex, uint8_t ucSubIndex, |
||||
|
int32_t iValue); |
||||
|
static void LeiSai_UpdateDistance(Motor_Instance_t *pstMotor); |
||||
|
|
||||
|
static int LeiSai_Init(Motor_Instance_t *pstMotor); |
||||
|
static int LeiSai_ResetAll(Motor_Instance_t *pstMotor); |
||||
|
static int LeiSai_ActivateAll(Motor_Instance_t *pstMotor); |
||||
|
static int LeiSai_SetHome(Motor_Instance_t *pstMotor); |
||||
|
static int LeiSai_SetWatchdog(Motor_Instance_t *pstMotor, uint32_t uiTimeMs); |
||||
|
static int LeiSai_RequestPosition(Motor_Instance_t *pstMotor); |
||||
|
static int LeiSai_RequestVelocity(Motor_Instance_t *pstMotor); |
||||
|
static int LeiSai_RequestTorque(Motor_Instance_t *pstMotor); |
||||
|
static int LeiSai_RequestFault(Motor_Instance_t *pstMotor); |
||||
|
static int LeiSai_SpeedModeInit(Motor_Instance_t *pstMotor); |
||||
|
static int LeiSai_SetTargetSpeed(Motor_Instance_t *pstMotor, int32_t iTargetSpeed); |
||||
|
static int LeiSai_PositionModeInit(Motor_Instance_t *pstMotor, int32_t iAccel, int32_t iDecel, int32_t iTargetSpeed); |
||||
|
static int LeiSai_SetTargetPosition(Motor_Instance_t *pstMotor, int32_t iTargetPos); |
||||
|
static int LeiSai_SetDO(Motor_Instance_t *pstMotor, uint8_t ucDoIndex, uint8_t ucState); |
||||
|
static int LeiSai_ParseResponse(Motor_Instance_t *pstMotor, const uint8_t *pucData, uint32_t uiLen); |
||||
|
static int32_t LeiSai_SpeedConvert(Motor_Instance_t *pstMotor, int32_t iSpeedE01Mmin); |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 全局变量 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
// 雷赛协议操作函数表
|
||||
|
const Motor_Protocol_t g_stLeiSaiProtocol = { |
||||
|
.m_szName = "LeiSai_ISV2", |
||||
|
.pfInit = LeiSai_Init, |
||||
|
.pfResetAll = LeiSai_ResetAll, |
||||
|
.pfActivateAll = LeiSai_ActivateAll, |
||||
|
.pfSetHome = LeiSai_SetHome, |
||||
|
.pfSetWatchdog = LeiSai_SetWatchdog, |
||||
|
.pfRequestPosition = LeiSai_RequestPosition, |
||||
|
.pfRequestVelocity = LeiSai_RequestVelocity, |
||||
|
.pfRequestTorque = LeiSai_RequestTorque, |
||||
|
.pfRequestFault = LeiSai_RequestFault, |
||||
|
.pfSpeedModeInit = LeiSai_SpeedModeInit, |
||||
|
.pfSetTargetSpeed = LeiSai_SetTargetSpeed, |
||||
|
.pfPositionModeInit = LeiSai_PositionModeInit, |
||||
|
.pfSetTargetPosition = LeiSai_SetTargetPosition, |
||||
|
.pfSetDO = LeiSai_SetDO, |
||||
|
.pfParseResponse = LeiSai_ParseResponse, |
||||
|
.pfSpeedConvert = LeiSai_SpeedConvert, |
||||
|
}; |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 模块级变量 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 常量定义 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 宏定义 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : LeiSai_GetProtocol |
||||
|
功能描述 : 获取雷赛协议操作函数表 |
||||
|
输入参数 : 无 |
||||
|
输出参数 : 无 |
||||
|
返 回 值 : const Motor_Protocol_t* |
||||
|
*****************************************************************************/ |
||||
|
const Motor_Protocol_t* LeiSai_GetProtocol(void) |
||||
|
{ |
||||
|
return &g_stLeiSaiProtocol; |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : LeiSai_SendSDO |
||||
|
功能描述 : 发送SDO命令 |
||||
|
*****************************************************************************/ |
||||
|
static int LeiSai_SendSDO(Motor_Instance_t *pstMotor, uint8_t ucCmd, |
||||
|
uint16_t usIndex, uint8_t ucSubIndex, |
||||
|
int32_t iValue) |
||||
|
{ |
||||
|
if (NULL == pstMotor || NULL == pstMotor->m_ptCAN) |
||||
|
{ |
||||
|
return RD_NULL; |
||||
|
} |
||||
|
|
||||
|
uint8_t aucData[8]; |
||||
|
aucData[0] = ucCmd; |
||||
|
aucData[1] = (uint8_t)(usIndex & 0xFF); |
||||
|
aucData[2] = (uint8_t)((usIndex >> 8) & 0xFF); |
||||
|
aucData[3] = ucSubIndex; |
||||
|
aucData[4] = (uint8_t)(iValue & 0xFF); |
||||
|
aucData[5] = (uint8_t)((iValue >> 8) & 0xFF); |
||||
|
aucData[6] = (uint8_t)((iValue >> 16) & 0xFF); |
||||
|
aucData[7] = (uint8_t)((iValue >> 24) & 0xFF); |
||||
|
|
||||
|
uint32_t uiFrameID = 0x600 + pstMotor->m_stConfig.m_ucMotorID; |
||||
|
return rd_ComIDSend(pstMotor->m_ptCAN, uiFrameID, (char *)aucData, 8); |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : LeiSai_UpdateDistance |
||||
|
功能描述 : 更新距离计算 |
||||
|
*****************************************************************************/ |
||||
|
static void LeiSai_UpdateDistance(Motor_Instance_t *pstMotor) |
||||
|
{ |
||||
|
double dCircleLength = 3.14 * pstMotor->m_stConfig.m_fWheelDiameter; |
||||
|
double dDeltaCounts = pstMotor->m_stData.m_iRealPosition - pstMotor->m_stData.m_iStartMeasurePos; |
||||
|
|
||||
|
pstMotor->m_stData.m_dRealDistance = (dDeltaCounts / pstMotor->m_stConfig.m_uiPulsePerRound |
||||
|
/ pstMotor->m_stConfig.m_uiReductionRatio |
||||
|
+ (double)pstMotor->m_stData.m_iRoundCount) |
||||
|
* dCircleLength; |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : LeiSai_Init |
||||
|
功能描述 : 初始化电机 |
||||
|
*****************************************************************************/ |
||||
|
static int LeiSai_Init(Motor_Instance_t *pstMotor) |
||||
|
{ |
||||
|
if (NULL == pstMotor) |
||||
|
{ |
||||
|
return RD_NULL; |
||||
|
} |
||||
|
pstMotor->m_eState = MOTOR_STATE_IDLE; |
||||
|
return RD_SUCCESS; |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : LeiSai_ResetAll |
||||
|
功能描述 : 复位所有节点（NMT命令） |
||||
|
*****************************************************************************/ |
||||
|
static int LeiSai_ResetAll(Motor_Instance_t *pstMotor) |
||||
|
{ |
||||
|
if (NULL == pstMotor || NULL == pstMotor->m_ptCAN) |
||||
|
{ |
||||
|
return RD_NULL; |
||||
|
} |
||||
|
|
||||
|
uint8_t aucData[2] = {0x81, 0x00}; |
||||
|
return rd_ComIDSend(pstMotor->m_ptCAN, 0x000, (char *)aucData, 2); |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : LeiSai_ActivateAll |
||||
|
功能描述 : 激活所有节点（NMT命令） |
||||
|
*****************************************************************************/ |
||||
|
static int LeiSai_ActivateAll(Motor_Instance_t *pstMotor) |
||||
|
{ |
||||
|
if (NULL == pstMotor || NULL == pstMotor->m_ptCAN) |
||||
|
{ |
||||
|
return RD_NULL; |
||||
|
} |
||||
|
|
||||
|
uint8_t aucData1[2] = {0x01, 0x01}; |
||||
|
rd_ComIDSend(pstMotor->m_ptCAN, 0x000, (char *)aucData1, 2); |
||||
|
|
||||
|
uint8_t aucData2[2] = {0x01, 0x02}; |
||||
|
return rd_ComIDSend(pstMotor->m_ptCAN, 0x000, (char *)aucData2, 2); |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : LeiSai_SetHome |
||||
|
功能描述 : 设置当前位置为零点 |
||||
|
*****************************************************************************/ |
||||
|
static int LeiSai_SetHome(Motor_Instance_t *pstMotor) |
||||
|
{ |
||||
|
if (NULL == pstMotor) |
||||
|
{ |
||||
|
return RD_NULL; |
||||
|
} |
||||
|
|
||||
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_1B, LEISAI_OD_MODE_OF_OPERATION, 0x00, LEISAI_MODE_HOMING); |
||||
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_1B, LEISAI_OD_HOMING_METHOD, 0x00, 0x23); |
||||
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_SHUTDOWN); |
||||
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_SWITCH_ON); |
||||
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_ENABLE); |
||||
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_START_MOVE); |
||||
|
|
||||
|
return RD_SUCCESS; |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : LeiSai_SetWatchdog |
||||
|
功能描述 : 设置看门狗时间 |
||||
|
*****************************************************************************/ |
||||
|
static int LeiSai_SetWatchdog(Motor_Instance_t *pstMotor, uint32_t uiTimeMs) |
||||
|
{ |
||||
|
if (NULL == pstMotor) |
||||
|
{ |
||||
|
return RD_NULL; |
||||
|
} |
||||
|
|
||||
|
return LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_WATCHDOG_TIME, 0x06, (int32_t)uiTimeMs); |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : LeiSai_RequestPosition |
||||
|
功能描述 : 读取电机位置 |
||||
|
*****************************************************************************/ |
||||
|
static int LeiSai_RequestPosition(Motor_Instance_t *pstMotor) |
||||
|
{ |
||||
|
if (NULL == pstMotor) |
||||
|
{ |
||||
|
return RD_NULL; |
||||
|
} |
||||
|
|
||||
|
return LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_READ, LEISAI_OD_POSITION_ACTUAL, 0x00, 0); |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : LeiSai_RequestVelocity |
||||
|
功能描述 : 读取电机速度 |
||||
|
*****************************************************************************/ |
||||
|
static int LeiSai_RequestVelocity(Motor_Instance_t *pstMotor) |
||||
|
{ |
||||
|
if (NULL == pstMotor) |
||||
|
{ |
||||
|
return RD_NULL; |
||||
|
} |
||||
|
|
||||
|
return LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_READ, LEISAI_OD_VELOCITY_ACTUAL, 0x00, 0); |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : LeiSai_RequestTorque |
||||
|
功能描述 : 读取电机电流/转矩 |
||||
|
*****************************************************************************/ |
||||
|
static int LeiSai_RequestTorque(Motor_Instance_t *pstMotor) |
||||
|
{ |
||||
|
if (NULL == pstMotor) |
||||
|
{ |
||||
|
return RD_NULL; |
||||
|
} |
||||
|
|
||||
|
return LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_READ, LEISAI_OD_TORQUE_ACTUAL, 0x00, 0); |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : LeiSai_RequestFault |
||||
|
功能描述 : 读取故障码 |
||||
|
*****************************************************************************/ |
||||
|
static int LeiSai_RequestFault(Motor_Instance_t *pstMotor) |
||||
|
{ |
||||
|
if (NULL == pstMotor) |
||||
|
{ |
||||
|
return RD_NULL; |
||||
|
} |
||||
|
|
||||
|
return LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_READ, LEISAI_OD_FAULT_CODE, 0x00, 0); |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : LeiSai_SpeedModeInit |
||||
|
功能描述 : 速度模式初始化 |
||||
|
*****************************************************************************/ |
||||
|
static int LeiSai_SpeedModeInit(Motor_Instance_t *pstMotor) |
||||
|
{ |
||||
|
if (NULL == pstMotor) |
||||
|
{ |
||||
|
return RD_NULL; |
||||
|
} |
||||
|
|
||||
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_SHUTDOWN); |
||||
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_SWITCH_ON); |
||||
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_ENABLE); |
||||
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_1B, LEISAI_OD_MODE_OF_OPERATION, 0x00, LEISAI_MODE_PROFILE_VEL); |
||||
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_4B, LEISAI_OD_TARGET_VELOCITY, 0x00, 0); |
||||
|
|
||||
|
return RD_SUCCESS; |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : LeiSai_SetTargetSpeed |
||||
|
功能描述 : 设置目标速度 |
||||
|
*****************************************************************************/ |
||||
|
static int LeiSai_SetTargetSpeed(Motor_Instance_t *pstMotor, int32_t iTargetSpeed) |
||||
|
{ |
||||
|
if (NULL == pstMotor) |
||||
|
{ |
||||
|
return RD_NULL; |
||||
|
} |
||||
|
|
||||
|
return LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_4B, LEISAI_OD_TARGET_VELOCITY, 0x00, iTargetSpeed); |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : LeiSai_PositionModeInit |
||||
|
功能描述 : 位置模式初始化 |
||||
|
*****************************************************************************/ |
||||
|
static int LeiSai_PositionModeInit(Motor_Instance_t *pstMotor, int32_t iAccel, int32_t iDecel, int32_t iTargetSpeed) |
||||
|
{ |
||||
|
if (NULL == pstMotor) |
||||
|
{ |
||||
|
return RD_NULL; |
||||
|
} |
||||
|
|
||||
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_SHUTDOWN); |
||||
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_SWITCH_ON); |
||||
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_ENABLE); |
||||
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_1B, LEISAI_OD_MODE_OF_OPERATION, 0x00, LEISAI_MODE_PROFILE_POS); |
||||
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_4B, LEISAI_OD_ACCELERATION, 0x00, iAccel); |
||||
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_4B, LEISAI_OD_DECELERATION, 0x00, iDecel); |
||||
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_4B, LEISAI_OD_TARGET_VELOCITY, 0x00, iTargetSpeed); |
||||
|
|
||||
|
return RD_SUCCESS; |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : LeiSai_SetTargetPosition |
||||
|
功能描述 : 设置目标位置（相对运动） |
||||
|
*****************************************************************************/ |
||||
|
static int LeiSai_SetTargetPosition(Motor_Instance_t *pstMotor, int32_t iTargetPos) |
||||
|
{ |
||||
|
if (NULL == pstMotor) |
||||
|
{ |
||||
|
return RD_NULL; |
||||
|
} |
||||
|
|
||||
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_4B, LEISAI_OD_TARGET_POSITION, 0x00, iTargetPos); |
||||
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_RELATIVE_MOVE); |
||||
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_START_MOVE); |
||||
|
|
||||
|
return RD_SUCCESS; |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : LeiSai_SetDO |
||||
|
功能描述 : 设置数字输出 |
||||
|
*****************************************************************************/ |
||||
|
static int LeiSai_SetDO(Motor_Instance_t *pstMotor, uint8_t ucDoIndex, uint8_t ucState) |
||||
|
{ |
||||
|
if (NULL == pstMotor) |
||||
|
{ |
||||
|
return RD_NULL; |
||||
|
} |
||||
|
|
||||
|
uint16_t usIndex = 0; |
||||
|
switch (ucDoIndex) |
||||
|
{ |
||||
|
case 1: |
||||
|
usIndex = 0x2410; |
||||
|
break; |
||||
|
case 2: |
||||
|
usIndex = 0x2411; |
||||
|
break; |
||||
|
default: |
||||
|
return RD_INVALUE; |
||||
|
} |
||||
|
|
||||
|
return LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_4B, usIndex, 0x00, ucState ? 1 : 0); |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : LeiSai_ParseResponse |
||||
|
功能描述 : 解析CAN响应数据 |
||||
|
*****************************************************************************/ |
||||
|
static int LeiSai_ParseResponse(Motor_Instance_t *pstMotor, const uint8_t *pucData, uint32_t uiLen) |
||||
|
{ |
||||
|
if (NULL == pstMotor || NULL == pucData || uiLen < 8) |
||||
|
{ |
||||
|
return RD_NULL; |
||||
|
} |
||||
|
|
||||
|
uint16_t usFunctionCode = (uint16_t)(pucData[1] | (pucData[2] << 8)); |
||||
|
|
||||
|
switch (usFunctionCode) |
||||
|
{ |
||||
|
case LEISAI_OD_POSITION_ACTUAL: |
||||
|
{ |
||||
|
pstMotor->m_stData.m_iRealPosition = (int32_t)(pucData[4] |
||||
|
| (pucData[5] << 8) |
||||
|
| (pucData[6] << 16) |
||||
|
| (pucData[7] << 24)); |
||||
|
|
||||
|
if (pstMotor->m_stData.m_ucStartMeasureFlag == 1) |
||||
|
{ |
||||
|
pstMotor->m_stData.m_ucStartMeasureFlag = 0; |
||||
|
pstMotor->m_stData.m_iRoundCount = 0; |
||||
|
pstMotor->m_stData.m_iStartMeasurePos = pstMotor->m_stData.m_iRealPosition; |
||||
|
pstMotor->m_stData.m_iLastPosition = pstMotor->m_stData.m_iRealPosition; |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
if (abs(pstMotor->m_stData.m_iRealPosition - pstMotor->m_stData.m_iLastPosition) >= 0x3FFFFFFF) |
||||
|
{ |
||||
|
if (pstMotor->m_stData.m_iRealPosition > 0) |
||||
|
{ |
||||
|
pstMotor->m_stData.m_iRoundCount -= 1; |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
pstMotor->m_stData.m_iRoundCount += 1; |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
pstMotor->m_stData.m_iLastPosition = pstMotor->m_stData.m_iRealPosition; |
||||
|
LeiSai_UpdateDistance(pstMotor); |
||||
|
break; |
||||
|
} |
||||
|
case LEISAI_OD_FAULT_CODE: |
||||
|
{ |
||||
|
pstMotor->m_stData.m_uiFaultCode = (uint32_t)(pucData[4] |
||||
|
| (pucData[5] << 8) |
||||
|
| (pucData[6] << 16) |
||||
|
| (pucData[7] << 24)); |
||||
|
if (pstMotor->m_stData.m_uiFaultCode != 0) |
||||
|
{ |
||||
|
pstMotor->m_eState = MOTOR_STATE_ERROR; |
||||
|
} |
||||
|
break; |
||||
|
} |
||||
|
case LEISAI_OD_VELOCITY_ACTUAL: |
||||
|
{ |
||||
|
pstMotor->m_stData.m_iRealVelocity = (int32_t)(pucData[4] |
||||
|
| (pucData[5] << 8) |
||||
|
| (pucData[6] << 16) |
||||
|
| (pucData[7] << 24)); |
||||
|
break; |
||||
|
} |
||||
|
case LEISAI_OD_TORQUE_ACTUAL: |
||||
|
{ |
||||
|
pstMotor->m_stData.m_iRealTorque = (int32_t)(pucData[4] |
||||
|
| (pucData[5] << 8) |
||||
|
| (pucData[6] << 16) |
||||
|
| (pucData[7] << 24)); |
||||
|
break; |
||||
|
} |
||||
|
default: |
||||
|
break; |
||||
|
} |
||||
|
|
||||
|
return RD_SUCCESS; |
||||
|
} |
||||
|
|
||||
|
/*****************************************************************************
|
||||
|
函 数 名 : LeiSai_SpeedConvert |
||||
|
功能描述 : 速度单位转换：0.1m/min -> 脉冲/秒 |
||||
|
*****************************************************************************/ |
||||
|
static int32_t LeiSai_SpeedConvert(Motor_Instance_t *pstMotor, int32_t iSpeedE01Mmin) |
||||
|
{ |
||||
|
if (NULL == pstMotor) |
||||
|
{ |
||||
|
return 0; |
||||
|
} |
||||
|
|
||||
|
double dSpeedMs = iSpeedE01Mmin * 0.1 / 60.0; |
||||
|
double dWheelCircumference = 3.14 * pstMotor->m_stConfig.m_fWheelDiameter; |
||||
|
double dPulsePerSec = dSpeedMs / dWheelCircumference |
||||
|
* pstMotor->m_stConfig.m_uiReductionRatio |
||||
|
* pstMotor->m_stConfig.m_uiPulsePerRound; |
||||
|
|
||||
|
return (int32_t)dPulsePerSec; |
||||
|
} |
||||
@ -0,0 +1,50 @@ |
|||||
|
/******************************************************************************
|
||||
|
|
||||
|
版权所有 (C), 2018-2099, Radkil |
||||
|
|
||||
|
****************************************************************************** |
||||
|
文 件 名 : paint_robot_new.c |
||||
|
版 本 号 : 初稿 |
||||
|
作 者 : radkil |
||||
|
生成日期 : 2026年7月14日 |
||||
|
最近修改 : |
||||
|
功能描述 : 新版喷漆主逻辑 |
||||
|
|
||||
|
修改历史 : |
||||
|
1.日 期 : 2026年7月14日 |
||||
|
作 者 : radkil |
||||
|
修改内容 : 创建文件 |
||||
|
|
||||
|
******************************************************************************/ |
||||
|
#include "paint_robot_new.h" |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 外部变量说明 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 外部函数原型说明 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 内部函数原型说明 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 全局变量 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 模块级变量 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 常量定义 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 宏定义 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
|
||||
|
|
||||
Loading…
Reference in new issue