diff --git a/RBcore/drv_interface.c b/RBcore/drv_interface.c index 058b1f6..56b52db 100644 --- a/RBcore/drv_interface.c +++ b/RBcore/drv_interface.c @@ -245,7 +245,7 @@ void Read_PV(void *argument) } } -extern void MotorInit(void); +extern void MotorTask(void *argument); extern int check_MK32(char *_pBuffer, uint32_t _iSize); extern void decode_MK32(const char *buf, uint32_t _iSize); extern int check_PV(char *_pBuffer, uint32_t _iSize); @@ -257,7 +257,15 @@ void Drv_InterfaceInit(void) { MsgCenter_Init(); // ground_manger_init(); - MotorInit(); + + // 创建电机任务 + const osThreadAttr_t motor_task_attributes = { + .name = MODULE_NAME_MOTOR, + .stack_size = 1024, + .priority = (osPriority_t) osPriorityRealtime, + }; + (void)osThreadNew(MotorTask, NULL, &motor_task_attributes); + // RBcore_Init(); TUartUserData *ptSbus = UART_userdata_init(5, -1, 512); diff --git a/motor/CMakeLists.txt b/motor/CMakeLists.txt index 9f262e6..e275e8d 100644 --- a/motor/CMakeLists.txt +++ b/motor/CMakeLists.txt @@ -9,8 +9,8 @@ else() message(FATAL_ERROR "Cannot find common build logic at ${COMMON_CMAKE_PATH}") endif() -if(TARGET RBcore) - target_link_libraries(${TARGET_NAME} PUBLIC RBcore) +if(TARGET peripheral) + target_link_libraries(${TARGET_NAME} PUBLIC peripheral) else() - message(FATAL_ERROR "[${TARGET_NAME}] Dependency 'RBcore' not found.") + message(FATAL_ERROR "[${TARGET_NAME}] Dependency 'peripheral' not found.") endif() \ No newline at end of file diff --git a/motor/motor_example.c b/motor/motor_example.c index 26fe5eb..cb323b7 100644 --- a/motor/motor_example.c +++ b/motor/motor_example.c @@ -16,9 +16,6 @@ 修改内容 : 创建文件 ******************************************************************************/ -#include "BHBF.h" -#include "motor_manager.h" -#include "msp_motor_leisai.h" /*----------------------------------------------* * 外部变量说明 * @@ -39,8 +36,7 @@ /*----------------------------------------------* * 模块级变量 * *----------------------------------------------*/ -static Motor_Instance_t *g_apstMotors[MOTOR_INSTANCE_MAX] = {NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL}; -static uint32_t g_uiMotorModuleID = 0; + /*----------------------------------------------* * 常量定义 * *----------------------------------------------*/ @@ -181,207 +177,3 @@ void MotorExample_MultiProtocol(void) } } #endif - -void MotorTask(void *argument) -{ - MotorMgr_ResetAll(g_apstMotors[0]); - Rd_Delay(500); - MotorMgr_ResetAll(g_apstMotors[1]); - Rd_Delay(500); - - int iCount = 6; - while(iCount--) - { - MotorMgr_ActivateAll(g_apstMotors[0]); - Rd_Delay(500); - MotorMgr_ActivateAll(g_apstMotors[1]); - Rd_Delay(500); - } - - MotorMgr_SetHome(g_apstMotors[0]); - MotorMgr_SetHome(g_apstMotors[1]); - MotorMgr_SpeedModeInit(g_apstMotors[0]); - MotorMgr_SpeedModeInit(g_apstMotors[1]); - - 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]); - MotorMgr_SetTargetSpeed(g_apstMotors[i], 50000); - Rd_Delay(2); - } - } - - 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; - } -} - -// 雷赛电机CAN响应检查函数 -// 雷赛响应ID范围:0x581 - 0x5FF (0x580 + MotorID) -static int check_LeiSaiMotor(char *_pBuffer, uint32_t _iSize) -{ - if (NULL == _pBuffer || _iSize < 4) - { - return 0; - } - - // 从buffer中提取CAN ID(前4字节) - uint32_t uiFrameID = (uint32_t)(_pBuffer[0] & 0xFF) - | ((_pBuffer[1] & 0xFF) << 8) - | ((_pBuffer[2] & 0xFF) << 16) - | ((_pBuffer[3] & 0xFF) << 24); - - // 雷赛响应ID范围:0x581 - 0x5FF - if (uiFrameID >= 0x581 && uiFrameID <= 0x5FF) - { - return 8 + 4; // 数据长度8 + CAN ID长度4 - } - - return 0; -} - -// 雷赛电机CAN响应解码函数 -static void decode_LeiSaiMotor(const char *_pBuffer, uint32_t _iSize) -{ - if (NULL == _pBuffer || _iSize < 12) // 4字节CAN ID + 8字节数据 - { - return; - } - - // 从buffer中提取CAN ID - uint32_t uiFrameID = (uint32_t)(_pBuffer[0] & 0xFF) - | ((_pBuffer[1] & 0xFF) << 8) - | ((_pBuffer[2] & 0xFF) << 16) - | ((_pBuffer[3] & 0xFF) << 24); - - // 提取电机ID (0x580 + MotorID) - uint8_t ucMotorID = (uint8_t)(uiFrameID - 0x580); - - // 调用电机管理模块解析响应 - MotorMgr_ParseResponse(ucMotorID, (const uint8_t *)&_pBuffer[4], 8); -} - -void MotorInit(void) -{ - // 初始化FDCAN1,使用雷赛电机的回调 - TCANUserData *ptCANUserData = CAN_userdata_init(&hfdcan1, 32, 0); - g_ptCAN1 = rd_ComCreate(check_LeiSaiMotor, decode_LeiSaiMotor, FDCAN1_Send, CONFIG_UART_BUFFER_SIZE, ptCANUserData); - CAN_IT_init(g_ptCAN1); - - // 初始化电机管理模块 - 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_ptCAN1, 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_ptCAN1, LeiSai_GetProtocol(), NULL); - - g_uiMotorModuleID = MsgCenter_Register(MODULE_NAME_MOTOR, Motor_ModuleHandler); - - // 创建电机任务 - const osThreadAttr_t motor_task_attributes = { - .name = MODULE_NAME_MOTOR, - .stack_size = 1024, - .priority = (osPriority_t) osPriorityRealtime, - }; - (void)osThreadNew(MotorTask, NULL, &motor_task_attributes); -} - diff --git a/project/paint_robot_new/CMakeLists.txt b/project/paint_robot_new/CMakeLists.txt index ca8cfea..b8d1666 100644 --- a/project/paint_robot_new/CMakeLists.txt +++ b/project/paint_robot_new/CMakeLists.txt @@ -13,4 +13,10 @@ if(TARGET RBcore) target_link_libraries(${TARGET_NAME} PUBLIC RBcore) else() message(FATAL_ERROR "[${TARGET_NAME}] Dependency 'RBcore' not found.") +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/paint_robot_new/include/paint_robot_new.h b/project/paint_robot_new/include/paint_robot_new.h deleted file mode 100644 index 49cedc0..0000000 --- a/project/paint_robot_new/include/paint_robot_new.h +++ /dev/null @@ -1,89 +0,0 @@ -/****************************************************************************** - - 版权所有 (C), 2018-2099, Radkil - - ****************************************************************************** - 文 件 名 : paint_robot_new.h - 版 本 号 : 初稿 - 作 者 : radkil - 生成日期 : 2026年7月14日 - 最近修改 : - 功能描述 : paint_robot_new.c 的头文件 - - 修改历史 : - 1.日 期 : 2026年7月14日 - 作 者 : radkil - 修改内容 : 创建文件 - -******************************************************************************/ - -/*----------------------------------------------* - * 外部变量说明 * - *----------------------------------------------*/ - -/*----------------------------------------------* - * 外部函数原型说明 * - *----------------------------------------------*/ - -/*----------------------------------------------* - * 内部函数原型说明 * - *----------------------------------------------*/ - -/*----------------------------------------------* - * 全局变量 * - *----------------------------------------------*/ - -/*----------------------------------------------* - * 模块级变量 * - *----------------------------------------------*/ - -/*----------------------------------------------* - * 常量定义 * - *----------------------------------------------*/ - -/*----------------------------------------------* - * 宏定义 * - *----------------------------------------------*/ - -#ifndef __PAINT_ROBOT_NEW_H__ -#define __PAINT_ROBOT_NEW_H__ - - -#ifdef __cplusplus -#if __cplusplus -extern "C"{ -#endif -#endif /* __cplusplus */ - - -/*==============================================* - * include header files * - *----------------------------------------------*/ -#include - - - -/*==============================================* - * 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/paint_robot_new.c b/project/paint_robot_new/paint_robot_new.c index 0e4a0d4..c603520 100644 --- a/project/paint_robot_new/paint_robot_new.c +++ b/project/paint_robot_new/paint_robot_new.c @@ -18,8 +18,7 @@ ******************************************************************************/ #include "BHBF.h" #include "msg_center.h" -#include "../Protobuf/PSource/msp_MK32.pb.h" -#include "../Protobuf/PSource/bsp_PV.pb.h" +#include "Protobuf/PSource/bsp_PV.pb.h" /*----------------------------------------------* * 外部变量说明 * @@ -40,7 +39,6 @@ /*----------------------------------------------* * 模块级变量 * *----------------------------------------------*/ -static SP_MSP_MK32_Button RB_MK32; static uint32_t g_uiCustomModuleID = 0; /*----------------------------------------------* @@ -51,97 +49,6 @@ static uint32_t g_uiCustomModuleID = 0; * 宏定义 * *----------------------------------------------*/ -int check_MK32(char *_pBuffer, uint32_t _iSize) -{ - if (_iSize < 25) return 0; // 数据不足25字节,不读[24]不误判,等下一包 - if (_pBuffer[0] != 0x0f) return -1; - if (_pBuffer[24] != 0x00) return -1; - return 25; -} - -void decode_MK32(const char *buf, uint32_t _iSize) -{ - int32_t CH[16]; - int Start_byte = 0; - - CH[0] = ((buf[Start_byte + 1] | buf[Start_byte + 2] << 8) & 0x07FF); - CH[1] = ((buf[Start_byte + 2] >> 3 | buf[Start_byte + 3] << 5) & 0x07FF); - CH[2] = ((buf[Start_byte + 3] >> 6 | buf[Start_byte + 4] << 2 - | buf[Start_byte + 5] << 10) & 0x07FF); - CH[3] = ((buf[Start_byte + 5] >> 1 | buf[Start_byte + 6] << 7) & 0x07FF); - CH[4] = ((buf[Start_byte + 6] >> 4 | buf[Start_byte + 7] << 4) & 0x07FF); - CH[5] = ((buf[Start_byte + 7] >> 7 | buf[Start_byte + 8] << 1 - | buf[Start_byte + 9] << 9) & 0x07FF); - CH[6] = ((buf[Start_byte + 9] >> 2 | buf[Start_byte + 10] << 6) & 0x07FF); - CH[7] = ((buf[Start_byte + 10] >> 5 | buf[Start_byte + 11] << 3) & 0x07FF); - CH[8] = ((buf[Start_byte + 12] | buf[Start_byte + 13] << 8) & 0x07FF); - CH[9] = ((buf[Start_byte + 13] >> 3 | buf[Start_byte + 14] << 5) & 0x07FF); - CH[10] = ((buf[Start_byte + 14] >> 6 | buf[Start_byte + 15] << 2 - | buf[Start_byte + 16] << 10) & 0x07FF); - CH[11] = ((buf[Start_byte + 16] >> 1 | buf[Start_byte + 17] << 7) & 0x07FF); - CH[12] = ((buf[Start_byte + 17] >> 4 | buf[Start_byte + 18] << 4) & 0x07FF); - CH[13] = ((buf[Start_byte + 18] >> 7 | buf[Start_byte + 19] << 1 - | buf[Start_byte + 20] << 9) & 0x07FF); - CH[14] = ((buf[Start_byte + 20] >> 2 | buf[Start_byte + 21] << 6) & 0x07FF); - CH[15] = ((buf[Start_byte + 21] >> 5 | buf[Start_byte + 22] << 3) & 0x07FF); - - // 按键数值转换:1050为中间值,272-1712 - for (int i = 0; i < 16; i++) - { - CH[i] = (int32_t)((CH[i] - 992) * 1.388889); - } - - RD_MEMCPY(&(RB_MK32.CH0_RY_H), CH, sizeof(CH)); - - if (buf[23] == 0) - { - RB_MK32.IsOnline = 1; - } - else - { - RB_MK32.IsOnline = 0; - } - RB_MK32.RxIndex++; - - int angle; - angle = atan2(RB_MK32.CH1_RY_V, RB_MK32.CH0_RY_H) * 180 / M_PI; - - if(RB_MK32.CH5_SB == 0) - { - // 前进 - if((angle >= 45) && (angle <= 135)) - { - MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_FORWARD, NULL, 0); - } - // 后退 - else if((angle >= -135) && (angle < -45)) - { - MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_BACKWARD, NULL, 0); - } - else - { - MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0); - } - } - else - { - // 前进 - if((angle >= 45) && (angle <= 135)) - { - MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_FORWARD, NULL, 0); - } - // 后退 - else if((angle >= -135) && (angle < -45)) - { - MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_BACKWARD, NULL, 0); - } - else - { - MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0); - } - } -} - static void Custom_ModuleHandler(const Msg_t *pstMsg) { if (NULL == pstMsg) diff --git a/project/paint_robot_new/paint_robot_new_MK32.c b/project/paint_robot_new/paint_robot_new_MK32.c new file mode 100644 index 0000000..40aed70 --- /dev/null +++ b/project/paint_robot_new/paint_robot_new_MK32.c @@ -0,0 +1,140 @@ +/****************************************************************************** + + 版权所有 (C), 2018-2099, Radkil + + ****************************************************************************** + 文 件 名 : paint_robot_new_MK32.c + 版 本 号 : 初稿 + 作 者 : radkil + 生成日期 : 2026年8月21日 + 最近修改 : + 功能描述 : 新喷涂机器人遥控器逻辑 + + 修改历史 : + 1.日 期 : 2026年8月21日 + 作 者 : radkil + 修改内容 : 创建文件 + +******************************************************************************/ +#include "BHBF.h" +#include "Protobuf/PSource/msp_MK32.pb.h" + +/*----------------------------------------------* + * 外部变量说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 外部函数原型说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 内部函数原型说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 全局变量 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 模块级变量 * + *----------------------------------------------*/ +static SP_MSP_MK32_Button RB_MK32; + +/*----------------------------------------------* + * 常量定义 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 宏定义 * + *----------------------------------------------*/ + +int check_MK32(char *_pBuffer, uint32_t _iSize) +{ + if (_iSize < 25) return 0; // 数据不足25字节,不读[24]不误判,等下一包 + if (_pBuffer[0] != 0x0f) return -1; + if (_pBuffer[24] != 0x00) return -1; + return 25; +} + +void decode_MK32(const char *buf, uint32_t _iSize) +{ + int32_t CH[16]; + int Start_byte = 0; + + CH[0] = ((buf[Start_byte + 1] | buf[Start_byte + 2] << 8) & 0x07FF); + CH[1] = ((buf[Start_byte + 2] >> 3 | buf[Start_byte + 3] << 5) & 0x07FF); + CH[2] = ((buf[Start_byte + 3] >> 6 | buf[Start_byte + 4] << 2 + | buf[Start_byte + 5] << 10) & 0x07FF); + CH[3] = ((buf[Start_byte + 5] >> 1 | buf[Start_byte + 6] << 7) & 0x07FF); + CH[4] = ((buf[Start_byte + 6] >> 4 | buf[Start_byte + 7] << 4) & 0x07FF); + CH[5] = ((buf[Start_byte + 7] >> 7 | buf[Start_byte + 8] << 1 + | buf[Start_byte + 9] << 9) & 0x07FF); + CH[6] = ((buf[Start_byte + 9] >> 2 | buf[Start_byte + 10] << 6) & 0x07FF); + CH[7] = ((buf[Start_byte + 10] >> 5 | buf[Start_byte + 11] << 3) & 0x07FF); + CH[8] = ((buf[Start_byte + 12] | buf[Start_byte + 13] << 8) & 0x07FF); + CH[9] = ((buf[Start_byte + 13] >> 3 | buf[Start_byte + 14] << 5) & 0x07FF); + CH[10] = ((buf[Start_byte + 14] >> 6 | buf[Start_byte + 15] << 2 + | buf[Start_byte + 16] << 10) & 0x07FF); + CH[11] = ((buf[Start_byte + 16] >> 1 | buf[Start_byte + 17] << 7) & 0x07FF); + CH[12] = ((buf[Start_byte + 17] >> 4 | buf[Start_byte + 18] << 4) & 0x07FF); + CH[13] = ((buf[Start_byte + 18] >> 7 | buf[Start_byte + 19] << 1 + | buf[Start_byte + 20] << 9) & 0x07FF); + CH[14] = ((buf[Start_byte + 20] >> 2 | buf[Start_byte + 21] << 6) & 0x07FF); + CH[15] = ((buf[Start_byte + 21] >> 5 | buf[Start_byte + 22] << 3) & 0x07FF); + + // 按键数值转换:1050为中间值,272-1712 + for (int i = 0; i < 16; i++) + { + CH[i] = (int32_t)((CH[i] - 992) * 1.388889); + } + + RD_MEMCPY(&(RB_MK32.CH0_RY_H), CH, sizeof(CH)); + + if (buf[23] == 0) + { + RB_MK32.IsOnline = 1; + } + else + { + RB_MK32.IsOnline = 0; + } + RB_MK32.RxIndex++; + + int angle; + angle = atan2(RB_MK32.CH1_RY_V, RB_MK32.CH0_RY_H) * 180 / M_PI; + + if(RB_MK32.CH5_SB == 0) + { + // 前进 + if((angle >= 45) && (angle <= 135)) + { + MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_FORWARD, NULL, 0); + } + // 后退 + else if((angle >= -135) && (angle < -45)) + { + MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_BACKWARD, NULL, 0); + } + else + { + MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0); + } + } + else + { + // 前进 + if((angle >= 45) && (angle <= 135)) + { + MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_FORWARD, NULL, 0); + } + // 后退 + else if((angle >= -135) && (angle < -45)) + { + MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_BACKWARD, NULL, 0); + } + else + { + MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0); + } + } +} diff --git a/project/paint_robot_new/paint_robot_new_motors.c b/project/paint_robot_new/paint_robot_new_motors.c new file mode 100644 index 0000000..a117fb4 --- /dev/null +++ b/project/paint_robot_new/paint_robot_new_motors.c @@ -0,0 +1,243 @@ +/****************************************************************************** + + 版权所有 (C), 2018-2099, Radkil + + ****************************************************************************** + 文 件 名 : paint_robot_new_motors.c + 版 本 号 : 初稿 + 作 者 : radkil + 生成日期 : 2026年8月21日 + 最近修改 : + 功能描述 : 喷涂机器人电机逻辑 + + 修改历史 : + 1.日 期 : 2026年8月21日 + 作 者 : 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; + +/*----------------------------------------------* + * 常量定义 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 宏定义 * + *----------------------------------------------*/ + +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; + } +} + +// 雷赛电机CAN响应检查函数 +// 雷赛响应ID范围:0x581 - 0x5FF (0x580 + MotorID) +static int check_LeiSaiMotor(char *_pBuffer, uint32_t _iSize) +{ + if (NULL == _pBuffer || _iSize < 4) + { + return 0; + } + + // 从buffer中提取CAN ID(前4字节) + uint32_t uiFrameID = (uint32_t)(_pBuffer[0] & 0xFF) + | ((_pBuffer[1] & 0xFF) << 8) + | ((_pBuffer[2] & 0xFF) << 16) + | ((_pBuffer[3] & 0xFF) << 24); + + // 雷赛响应ID范围:0x581 - 0x5FF + if (uiFrameID >= 0x581 && uiFrameID <= 0x5FF) + { + return 8 + 4; // 数据长度8 + CAN ID长度4 + } + + return 0; +} + +// 雷赛电机CAN响应解码函数 +static void decode_LeiSaiMotor(const char *_pBuffer, uint32_t _iSize) +{ + if (NULL == _pBuffer || _iSize < 12) // 4字节CAN ID + 8字节数据 + { + return; + } + + // 从buffer中提取CAN ID + uint32_t uiFrameID = (uint32_t)(_pBuffer[0] & 0xFF) + | ((_pBuffer[1] & 0xFF) << 8) + | ((_pBuffer[2] & 0xFF) << 16) + | ((_pBuffer[3] & 0xFF) << 24); + + // 提取电机ID (0x580 + MotorID) + uint8_t ucMotorID = (uint8_t)(uiFrameID - 0x580); + + // 调用电机管理模块解析响应 + MotorMgr_ParseResponse(ucMotorID, (const uint8_t *)&_pBuffer[4], 8); +} + +static void MotorInit(void) +{ + // 初始化FDCAN1,使用雷赛电机的回调 + TCANUserData *ptCANUserData = CAN_userdata_init(&hfdcan1, 32, 0); + g_ptCAN1 = rd_ComCreate(check_LeiSaiMotor, decode_LeiSaiMotor, FDCAN1_Send, CONFIG_UART_BUFFER_SIZE, ptCANUserData); + CAN_IT_init(g_ptCAN1); + + // 初始化电机管理模块 + 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_ptCAN1, 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_ptCAN1, LeiSai_GetProtocol(), NULL); + + g_uiMotorModuleID = MsgCenter_Register(MODULE_NAME_MOTOR, Motor_ModuleHandler); +} + +void MotorTask(void *argument) +{ + MotorInit(); + + MotorMgr_ResetAll(g_apstMotors[0]); + Rd_Delay(500); + MotorMgr_ResetAll(g_apstMotors[1]); + Rd_Delay(500); + + int iCount = 6; + while(iCount--) + { + MotorMgr_ActivateAll(g_apstMotors[0]); + Rd_Delay(500); + MotorMgr_ActivateAll(g_apstMotors[1]); + Rd_Delay(500); + } + + MotorMgr_SetHome(g_apstMotors[0]); + MotorMgr_SetHome(g_apstMotors[1]); + MotorMgr_SpeedModeInit(g_apstMotors[0]); + MotorMgr_SpeedModeInit(g_apstMotors[1]); + + 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]); + MotorMgr_SetTargetSpeed(g_apstMotors[i], 50000); + } + } + + Rd_Delay(2); // 2ms周期 + } +}