diff --git a/CMakeLists.txt b/CMakeLists.txt index 13b18ef..a4932eb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -117,9 +117,11 @@ add_module(ringbuffer library/ringbuffer OFF OFF) add_module(peripheral peripheral OFF ON) add_module(lua lua OFF ON) add_module(bspMCU bspMCU OFF OFF) -add_module(RBcore project/RBcore OFF OFF) +add_module(motor motor OFF OFF) +add_module(RBcore RBcore OFF OFF) add_module(optional optional OFF OFF) add_module(Spoolend project/Spoolend OFF OFF) +add_module(paint_robot_new project/paint_robot_new OFF OFF) add_module(Spoolold project_old/Spoolold OFF OFF) add_module(paint_robot_old project_old/paint_robot_old OFF OFF) diff --git a/project/RBcore/BHBF_robot.c b/RBcore/BHBF_robot.c similarity index 90% rename from project/RBcore/BHBF_robot.c rename to RBcore/BHBF_robot.c index cdfc005..cd11f70 100644 --- a/project/RBcore/BHBF_robot.c +++ b/RBcore/BHBF_robot.c @@ -38,7 +38,7 @@ /*----------------------------------------------* * 全局变量 * *----------------------------------------------*/ -GV_struct_define GV = {0}; +GV_define GV = {0}; /*----------------------------------------------* * 模块级变量 * @@ -51,17 +51,17 @@ GV_struct_define GV = {0}; /*----------------------------------------------* * 宏定义 * *----------------------------------------------*/ -GV_struct_define *GetGV(void) +GV_define *GetGV(void) { - static GV_struct_define tmpGV = {0}; + static GV_define tmpGV = {0}; RD_MEMSET(&tmpGV, 0, sizeof(tmpGV)); - RD_MEMCPY(&tmpGV, &GV, sizeof(GV_struct_define)); + RD_MEMCPY(&tmpGV, &GV, sizeof(GV_define)); return &tmpGV; } -void SetGV(GV_struct_define *_pGV) +void SetGV(GV_define *_pGV) { - RD_MEMCPY(&GV, _pGV, sizeof(GV_struct_define)); + RD_MEMCPY(&GV, _pGV, sizeof(GV_define)); } void GF_FsmStart(GF_CMD GFCmd) diff --git a/RBcore/CMakeLists.txt b/RBcore/CMakeLists.txt new file mode 100644 index 0000000..2723f10 --- /dev/null +++ b/RBcore/CMakeLists.txt @@ -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 motor) + target_link_libraries(${TARGET_NAME} PUBLIC motor) +else() + message(FATAL_ERROR "[${TARGET_NAME}] Dependency 'motor' not found.") +endif() \ No newline at end of file diff --git a/project/RBcore/drv_interface.c b/RBcore/drv_interface.c similarity index 95% rename from project/RBcore/drv_interface.c rename to RBcore/drv_interface.c index c187faa..86290a3 100644 --- a/project/RBcore/drv_interface.c +++ b/RBcore/drv_interface.c @@ -20,7 +20,6 @@ #include "cmsis_os.h" #include "FreeRTOS.h" -#include "task.h" /*----------------------------------------------* * 外部变量说明 * @@ -255,6 +254,9 @@ void SendAll_Task(void *argument) } } +extern void MotorInit(void); +void MotorTask(void *argument); + void Drv_InterfaceInit(void) { //MK32_Init(); @@ -273,5 +275,14 @@ void Drv_InterfaceInit(void) }; (void)osThreadNew(SendAll_Task, NULL, &send_task_attributes); - //MoveWheel_Init(); + // 初始化电机 + MotorInit(); + + // 创建电机任务 + const osThreadAttr_t motor_task_attributes = { + .name = "motor_task", + .stack_size = 1024, + .priority = (osPriority_t) osPriorityHigh5, + }; + (void)osThreadNew(MotorTask, NULL, &motor_task_attributes); } diff --git a/project/RBcore/include/BHBF_robot.h b/RBcore/include/BHBF_robot.h similarity index 97% rename from project/RBcore/include/BHBF_robot.h rename to RBcore/include/BHBF_robot.h index c9b5ef2..2de252f 100644 --- a/project/RBcore/include/BHBF_robot.h +++ b/RBcore/include/BHBF_robot.h @@ -79,7 +79,7 @@ typedef struct _GV_struct_define int m_iWorkMode; GF_CMD m_eGFCmd; int32_t m_iMoveSpeed[2];//索引1对应左轮，索引2对应右轮 -} GV_struct_define; +} GV_define; /*==============================================* * project-wide global variables * @@ -90,6 +90,8 @@ typedef struct _GV_struct_define /*==============================================* * routines' or functions' implementations * *----------------------------------------------*/ +GV_define *GetGV(void); +void SetGV(GV_define *_pGV); // 待实现接口 void MK32_Init(void); GF_CMD MK32_Task(int mode); diff --git a/project/RBcore/include/drv_interface.h b/RBcore/include/drv_interface.h similarity index 100% rename from project/RBcore/include/drv_interface.h rename to RBcore/include/drv_interface.h diff --git a/RBcore/include/msg_center.h b/RBcore/include/msg_center.h new file mode 100644 index 0000000..a07e573 --- /dev/null +++ b/RBcore/include/msg_center.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__ */ diff --git a/RBcore/msg_center.c b/RBcore/msg_center.c new file mode 100644 index 0000000..64dc21f --- /dev/null +++ b/RBcore/msg_center.c @@ -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; +} diff --git a/RBcore/msg_example.c b/RBcore/msg_example.c new file mode 100644 index 0000000..850cf02 --- /dev/null +++ b/RBcore/msg_example.c @@ -0,0 +1,378 @@ +/****************************************************************************** + + 版权所有 (C), 2018-2099, Radkil + + ****************************************************************************** + 文 件 名 : msg_example.c + 版 本 号 : 初稿 + 作 者 : radkil + 生成日期 : 2026年7月13日 + 最近修改 : + 功能描述 : 消息中心使用示例 + + 修改历史 : + 1.日 期 : 2026年7月13日 + 作 者 : radkil + 修改内容 : 创建文件 + + ******************************************************************************/ + +#include "BHBF_robot.h" +#include "msg_center.h" +#include "motor_manager.h" +#include "msp_motor_leisai.h" +#include "cmsis_os.h" +#include "FreeRTOS.h" + +/*==============================================* + * 模块定义 * + *==============================================*/ + +// 模块名称定义 +#define MODULE_NAME_MOTOR "motor" +#define MODULE_NAME_CHASSIS "chassis" +#define MODULE_NAME_UI "ui" + +/*==============================================* + * 电机模块命令字定义 * + *==============================================*/ + +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; + +/*==============================================* + * 底盘模块命令字定义 * + *==============================================*/ + +typedef enum { + CHASSIS_CMD_MOVE = 0x01, // 移动 + CHASSIS_CMD_STOP = 0x02, // 停止 + CHASSIS_CMD_SET_SPEED = 0x03, // 设置速度 +} Chassis_Cmd_e; + +typedef struct { + int32_t m_iSpeedX; // X方向速度 + int32_t m_iSpeedY; // Y方向速度 + int32_t m_iSpeedW; // 旋转速度 +} Chassis_MoveData_t; + +/*==============================================* + * 模块级变量 * + *==============================================*/ + +static uint32_t g_uiMotorModuleID = 0; +static uint32_t g_uiChassisModuleID = 0; +static uint32_t g_uiUIModuleID = 0; + +static Motor_Instance_t *g_apstMotors[2] = {NULL, NULL}; + +/*==============================================* + * 电机模块消息处理 * + *==============================================*/ + +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("ui", 0x10, &stStatus, sizeof(stStatus)); + } + } + } + break; + } + default: + break; + } +} + +/*==============================================* + * 底盘模块消息处理 * + *==============================================*/ + +static void Chassis_ModuleHandler(const Msg_t *pstMsg) +{ + if (NULL == pstMsg) + { + return; + } + + switch (pstMsg->m_uiMsgID) + { + case CHASSIS_CMD_MOVE: + { + if (pstMsg->m_uiDataLen >= sizeof(Chassis_MoveData_t)) + { + Chassis_MoveData_t *pstData = (Chassis_MoveData_t *)pstMsg->m_aucData; + + // 转换为左右轮速度（差速模型） + int32_t iLeftSpeed = pstData->m_iSpeedX - pstData->m_iSpeedW; + int32_t iRightSpeed = pstData->m_iSpeedX + pstData->m_iSpeedW; + + // 发送给电机模块 + Motor_CmdData_t stMotorCmd; + stMotorCmd.m_ucMotorIndex = 0; + stMotorCmd.m_iValue = iLeftSpeed; + MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_SET_SPEED, &stMotorCmd, sizeof(stMotorCmd)); + + stMotorCmd.m_ucMotorIndex = 1; + stMotorCmd.m_iValue = iRightSpeed; + MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_SET_SPEED, &stMotorCmd, sizeof(stMotorCmd)); + } + break; + } + case CHASSIS_CMD_STOP: + { + MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_STOP, NULL, 0); + break; + } + default: + break; + } +} + +/*==============================================* + * UI模块消息处理 * + *==============================================*/ + +static void UI_ModuleHandler(const Msg_t *pstMsg) +{ + if (NULL == pstMsg) + { + return; + } + + // 处理来自其他模块的回复 + switch (pstMsg->m_uiMsgID) + { + case 0x10: // 电机状态回复 + { + if (pstMsg->m_uiDataLen >= sizeof(Motor_StatusData_t)) + { + Motor_StatusData_t *pstStatus = (Motor_StatusData_t *)pstMsg->m_aucData; + RD_PRINTF("Motor[%d] Status: Pos=%ld, Vel=%ld, Fault=%ld\n", + pstStatus->m_ucMotorIndex, + pstStatus->m_iPosition, + pstStatus->m_iVelocity, + pstStatus->m_uiFaultCode); + } + break; + } + default: + break; + } +} + +/*==============================================* + * 模块任务函数 * + *==============================================*/ + +// 电机模块任务 +void Motor_ModuleTask(void *argument) +{ + // 初始化电机 + MotorMgr_Init(); + + Motor_Config_t stConfig = { + .m_ucMotorID = 1, + .m_uiPulsePerRound = 10000, + .m_uiReductionRatio = 70, + .m_fWheelDiameter = 0.26f + }; + g_apstMotors[0] = MotorMgr_Create(&stConfig, g_ptFDCAN1, LeiSai_GetProtocol(), NULL); + + stConfig.m_ucMotorID = 2; + g_apstMotors[1] = MotorMgr_Create(&stConfig, g_ptFDCAN1, LeiSai_GetProtocol(), NULL); + + // 初始化电机 + for (int i = 0; i < 2; i++) + { + if (g_apstMotors[i] != NULL) + { + MotorMgr_ResetAll(g_apstMotors[i]); + MotorMgr_ActivateAll(g_apstMotors[i]); + MotorMgr_SpeedModeInit(g_apstMotors[i]); + } + } + + while (1) + { + // 处理消息 + MsgCenter_ProcessWait(g_uiMotorModuleID, 2); + + // 周期性读取电机状态 + for (int i = 0; i < 2; i++) + { + if (g_apstMotors[i] != NULL) + { + MotorMgr_RequestPosition(g_apstMotors[i]); + MotorMgr_RequestVelocity(g_apstMotors[i]); + MotorMgr_RequestFault(g_apstMotors[i]); + } + } + } +} + +// 底盘模块任务 +void Chassis_ModuleTask(void *argument) +{ + while (1) + { + // 处理消息 + MsgCenter_ProcessWait(g_uiChassisModuleID, 10); + } +} + +// UI模块任务 +void UI_ModuleTask(void *argument) +{ + while (1) + { + // 处理消息 + MsgCenter_ProcessWait(g_uiUIModuleID, 100); + + // 定期请求电机状态 + MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_GET_STATUS, NULL, 0); + } +} + +/*==============================================* + * 初始化示例 * + *==============================================*/ + +void MsgExample_Init(void) +{ + // 初始化消息中心 + MsgCenter_Init(); + + // 注册模块 + g_uiMotorModuleID = MsgCenter_Register(MODULE_NAME_MOTOR, Motor_ModuleHandler); + g_uiChassisModuleID = MsgCenter_Register(MODULE_NAME_CHASSIS, Chassis_ModuleHandler); + g_uiUIModuleID = MsgCenter_Register(MODULE_NAME_UI, UI_ModuleHandler); + + // 创建模块任务 + const osThreadAttr_t motor_task_attr = { + .name = "motor_task", + .stack_size = 1024, + .priority = (osPriority_t) osPriorityHigh5, + }; + (void)osThreadNew(Motor_ModuleTask, NULL, &motor_task_attr); + + const osThreadAttr_t chassis_task_attr = { + .name = "chassis_task", + .stack_size = 512, + .priority = (osPriority_t) osPriorityHigh4, + }; + (void)osThreadNew(Chassis_ModuleTask, NULL, &chassis_task_attr); + + const osThreadAttr_t ui_task_attr = { + .name = "ui_task", + .stack_size = 512, + .priority = (osPriority_t) osPriorityNormal3, + }; + (void)osThreadNew(UI_ModuleTask, NULL, &ui_task_attr); +} + +/*==============================================* + * 外部调用示例 * + *==============================================*/ + +// 从任何线程调用这些函数即可跨线程控制电机 +void Example_SetMotorSpeed(uint8_t ucMotorIndex, int32_t iSpeed) +{ + Motor_CmdData_t stData; + stData.m_ucMotorIndex = ucMotorIndex; + stData.m_iValue = iSpeed; + MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_SET_SPEED, &stData, sizeof(stData)); +} + +void Example_StopAllMotors(void) +{ + MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_STOP, NULL, 0); +} + +void Example_ChassisMove(int32_t iSpeedX, int32_t iSpeedW) +{ + Chassis_MoveData_t stData; + stData.m_iSpeedX = iSpeedX; + stData.m_iSpeedY = 0; + stData.m_iSpeedW = iSpeedW; + MsgCenter_SendTo(MODULE_NAME_CHASSIS, CHASSIS_CMD_MOVE, &stData, sizeof(stData)); +} diff --git a/motor/CMakeLists.txt b/motor/CMakeLists.txt new file mode 100644 index 0000000..3b9ceec --- /dev/null +++ b/motor/CMakeLists.txt @@ -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() \ No newline at end of file diff --git a/motor/include/motor_manager.h b/motor/include/motor_manager.h new file mode 100644 index 0000000..b78ae8f --- /dev/null +++ b/motor/include/motor_manager.h @@ -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 * + *----------------------------------------------*/ + +/*==============================================* + * 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__ */ diff --git a/motor/include/motor_protocol.h b/motor/include/motor_protocol.h new file mode 100644 index 0000000..6408f7c --- /dev/null +++ b/motor/include/motor_protocol.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__ */ diff --git a/motor/include/msp_motor_leisai.h b/motor/include/msp_motor_leisai.h new file mode 100644 index 0000000..f1c87a1 --- /dev/null +++ b/motor/include/msp_motor_leisai.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__ */ diff --git a/motor/motor_manager.c b/motor/motor_manager.c new file mode 100644 index 0000000..11bbaab --- /dev/null +++ b/motor/motor_manager.c @@ -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; +} diff --git a/motor/msp_motor_leisai.c b/motor/msp_motor_leisai.c new file mode 100644 index 0000000..de37451 --- /dev/null +++ b/motor/msp_motor_leisai.c @@ -0,0 +1,487 @@ +/****************************************************************************** + + 版权所有 (C), 2018-2099, Radkil + + ****************************************************************************** + 文 件 名 : msp_motor_leisai.c + 版 本 号 : 初稿 + 作 者 : radkil + 生成日期 : 2026年7月13日 + 最近修改 : + 功能描述 : 雷赛ISV2电机CAN协议实现 + + 修改历史 : + 1.日 期 : 2026年7月13日 + 作 者 : radkil + 修改内容 : 创建文件 + + ******************************************************************************/ + +#include "msp_motor_leisai.h" +#include + +/*----------------------------------------------* + * 外部变量说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 外部函数原型说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 内部函数原型说明 * + *----------------------------------------------*/ +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; +} diff --git a/project/RBcore/CMakeLists.txt b/project/RBcore/CMakeLists.txt deleted file mode 100644 index 5febc02..0000000 --- a/project/RBcore/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -cmake_minimum_required(VERSION 3.10) - -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(RBcore PUBLIC bspMCU) -else() - message(WARNING "[RBcore] Dependency 'bspMCU' not found.") -endif() \ No newline at end of file diff --git a/project/paint_robot_new/CMakeLists.txt b/project/paint_robot_new/CMakeLists.txt new file mode 100644 index 0000000..115a83e --- /dev/null +++ b/project/paint_robot_new/CMakeLists.txt @@ -0,0 +1,22 @@ +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() + +if(TARGET peripheral) + target_link_libraries(${TARGET_NAME} PUBLIC peripheral) +else() + message(FATAL_ERROR "[${TARGET_NAME}] Dependency 'peripheral' not found.") +endif() \ No newline at end of file diff --git a/project/paint_robot_new/include/paint_robot_new.h b/project/paint_robot_new/include/paint_robot_new.h new file mode 100644 index 0000000..6144c33 --- /dev/null +++ b/project/paint_robot_new/include/paint_robot_new.h @@ -0,0 +1,91 @@ +/****************************************************************************** + + 版权所有 (C), 2018-2099, Radkil + + ****************************************************************************** + 文 件 名 : paint_robot_new.h + 版 本 号 : 初稿 + 作 者 : radkil + 生成日期 : 2026年7月14日 + 最近修改 : + 功能描述 : paint_robot_new.c 的头文件 + + 修改历史 : + 1.日 期 : 2026年7月14日 + 作 者 : radkil + 修改内容 : 创建文件 + +******************************************************************************/ +#include "motor_manager.h" +#include "msp_motor_leisai.h" + +/*----------------------------------------------* + * 外部变量说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 外部函数原型说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 内部函数原型说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 全局变量 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 模块级变量 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 常量定义 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 宏定义 * + *----------------------------------------------*/ + +#ifndef __PAINT_ROBOT_NEW_H__ +#define __PAINT_ROBOT_NEW_H__ + + +#ifdef __cplusplus +#if __cplusplus +extern "C"{ +#endif +#endif /* __cplusplus */ + + +/*==============================================* + * include header files * + *----------------------------------------------*/ + + + + +/*==============================================* + * constants or macros define * + *----------------------------------------------*/ + + +/*==============================================* + * project-wide global variables * + *----------------------------------------------*/ + + + +/*==============================================* + * routines' or functions' implementations * + *----------------------------------------------*/ + + +#ifdef __cplusplus +#if __cplusplus +} +#endif +#endif /* __cplusplus */ + + +#endif /* __PAINT_ROBOT_NEW_H__ */ diff --git a/project/paint_robot_new/motor_example.c b/project/paint_robot_new/motor_example.c new file mode 100644 index 0000000..aa95075 --- /dev/null +++ b/project/paint_robot_new/motor_example.c @@ -0,0 +1,181 @@ +/****************************************************************************** + + 版权所有 (C), 2018-2099, Radkil + + ****************************************************************************** + 文 件 名 : motor_example.c + 版 本 号 : 初稿 + 作 者 : radkil + 生成日期 : 2026年7月13日 + 最近修改 : + 功能描述 : 电机模块使用示例 + + 修改历史 : + 1.日 期 : 2026年7月13日 + 作 者 : radkil + 修改内容 : 创建文件 + + ******************************************************************************/ + +#include "BHBF_robot.h" +#include "motor_manager.h" +#include "msp_motor_leisai.h" + +/*----------------------------------------------* + * 外部变量说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 外部函数原型说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 内部函数原型说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 全局变量 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 模块级变量 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 常量定义 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 宏定义 * + *----------------------------------------------*/ + +/***************************************************************************** + 函 数 名 : 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); + } +} diff --git a/project/paint_robot_new/paint_robot_new.c b/project/paint_robot_new/paint_robot_new.c new file mode 100644 index 0000000..7378998 --- /dev/null +++ b/project/paint_robot_new/paint_robot_new.c @@ -0,0 +1,122 @@ +/****************************************************************************** + + 版权所有 (C), 2018-2099, Radkil + + ****************************************************************************** + 文 件 名 : paint_robot_new.c + 版 本 号 : 初稿 + 作 者 : radkil + 生成日期 : 2026年7月14日 + 最近修改 : + 功能描述 : 新版喷漆主逻辑 + + 修改历史 : + 1.日 期 : 2026年7月14日 + 作 者 : radkil + 修改内容 : 创建文件 + +******************************************************************************/ +#include "paint_robot_new.h" +#include "com.h" +#include "rd_time.h" +/*----------------------------------------------* + * 外部变量说明 * + *----------------------------------------------*/ +extern TComCtrl *g_ptFDCAN1; + +/*----------------------------------------------* + * 外部函数原型说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 内部函数原型说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 全局变量 * + *----------------------------------------------*/ +static Motor_Instance_t *g_pstLeftMotor; // 左轮电机实例 +static Motor_Instance_t *g_pstRightMotor; // 右轮电机实例 + +/*----------------------------------------------* + * 模块级变量 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 常量定义 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 宏定义 * + *----------------------------------------------*/ + +void MotorInit(void) +{ + // 初始化电机管理模块 + MotorMgr_Init(); + + + // 配置左轮电机 + Motor_Config_t stLeftMotorConfig = { + .m_ucMotorID = 1, + .m_uiPulsePerRound = 10000, + .m_uiReductionRatio = 70, + .m_fWheelDiameter = 0.26f + }; + g_pstLeftMotor = 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_pstRightMotor = MotorMgr_Create(&stRightMotorConfig, g_ptFDCAN1, LeiSai_GetProtocol(), NULL); +} + +void MotorTask(void *argument) +{ + // 等待系统稳定 + Rd_Delay(1000); + + // 复位并激活所有电机 + if (g_pstLeftMotor != NULL) + { + MotorMgr_ResetAll(g_pstLeftMotor); + Rd_Delay(500); + MotorMgr_ActivateAll(g_pstLeftMotor); + Rd_Delay(500); + MotorMgr_SetHome(g_pstLeftMotor); + Rd_Delay(100); + MotorMgr_SpeedModeInit(g_pstLeftMotor); + } + + while(1) + { + if (g_pstLeftMotor != NULL) + { + // 读取位置、速度、故障 + MotorMgr_RequestPosition(g_pstLeftMotor); + MotorMgr_RequestFault(g_pstLeftMotor); + MotorMgr_RequestVelocity(g_pstLeftMotor); + + // 设置目标速度 + MotorMgr_SetTargetSpeed(g_pstLeftMotor, 0); + } + + if (g_pstRightMotor != NULL) + { + // 读取位置、速度、故障 + MotorMgr_RequestPosition(g_pstRightMotor); + MotorMgr_RequestFault(g_pstRightMotor); + MotorMgr_RequestVelocity(g_pstRightMotor); + + // 设置目标速度 + MotorMgr_SetTargetSpeed(g_pstRightMotor, 0); + } + + Rd_Delay(2); // 2ms周期 + } +} +