From ae7c21cb799c85da14fd80bab0983bd6e7741152 Mon Sep 17 00:00:00 2001 From: Lizongdi <1210855344@qq.com> Date: Wed, 9 Sep 2026 11:13:29 +0800 Subject: [PATCH] =?UTF-8?q?=E3=80=90=E9=81=97=E7=95=99=E9=97=AE=E9=A2=98?= =?UTF-8?q?=E3=80=91=E5=BD=93=E5=89=8D=E7=94=B5=E6=9C=BA=E5=88=9D=E5=A7=8B?= =?UTF-8?q?=E5=8C=96=E5=90=8E=E4=BC=9A=E5=AF=BC=E8=87=B4CAN=E4=B8=AD?= =?UTF-8?q?=E6=96=AD=E4=B8=8D=E8=A7=A6=E5=8F=91=EF=BC=8C=E6=AD=A3=E5=9C=A8?= =?UTF-8?q?=E5=AE=9A=E4=BD=8D=E4=B8=AD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- project/paint_robot_new/paint_robot_new.c | 23 +++++++ .../paint_robot_new/paint_robot_new_motors.c | 63 +++---------------- 2 files changed, 31 insertions(+), 55 deletions(-) diff --git a/project/paint_robot_new/paint_robot_new.c b/project/paint_robot_new/paint_robot_new.c index f229140..f5d29cd 100644 --- a/project/paint_robot_new/paint_robot_new.c +++ b/project/paint_robot_new/paint_robot_new.c @@ -67,6 +67,29 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) switch (pstMsg->m_uiMsgID) { + case MOTOR_CMD_GET_FAULT_CODE: + { + uint32_t uiFailtCode[2] = {0}; + if (pstMsg->m_uiDataLen >= sizeof(uiFailtCode)) + { + RD_MEMCPY(uiFailtCode, pstMsg->m_aucData, sizeof(uiFailtCode)); + } + break; + } + case MOTOR_CMD_GET_SPEED: + { + int32_t iCurrentSpeed[2] = {0}; + if (pstMsg->m_uiDataLen >= sizeof(iCurrentSpeed)) + { + RD_MEMCPY(iCurrentSpeed, pstMsg->m_aucData, sizeof(iCurrentSpeed)); + if (1 == iCurrentSpeed[0]) + { + g_stIV.CurrentSpeed = iCurrentSpeed[1]; + log_i("g_stIV.CurrentSpeed = %d", g_stIV.CurrentSpeed); + } + } + break; + } case CUSTOM_GET_TL720D_ROLL: { if (pstMsg->m_uiDataLen >= sizeof(int32_t)) diff --git a/project/paint_robot_new/paint_robot_new_motors.c b/project/paint_robot_new/paint_robot_new_motors.c index 8cf75f8..3a76e3c 100644 --- a/project/paint_robot_new/paint_robot_new_motors.c +++ b/project/paint_robot_new/paint_robot_new_motors.c @@ -81,34 +81,6 @@ static void Motor_ModuleHandler(const Msg_t *pstMsg) memset(g_aiMotorSpeed, 0, sizeof(g_aiMotorSpeed)); break; } - case MOTOR_CMD_SET_HOME: - { - log_i("Not Support [%d]", pstMsg->m_uiMsgID); - 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; } @@ -118,21 +90,12 @@ static void Motor_ModuleHandler(const Msg_t *pstMsg) // 雷赛响应ID范围：0x581 - 0x5FF (0x580 + MotorID) static int check_LeiSaiMotor(char *_pBuffer, uint32_t _iSize) { - if (NULL == _pBuffer || _iSize < 4) - { - return 0; - } + TCANUserData *ptCANUserData = (TCANUserData *)g_ptCAN1->m_pUserData; + uint32_t iCANId = (uint32_t)ptCANUserData->m_canrx->Identifier; - // 从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) + if (iCANId >= 0x581 && iCANId <= 0x5FF) { - return 8 + 4; // 数据长度8 + CAN ID长度4 + return _iSize; } return 0; @@ -141,22 +104,14 @@ static int check_LeiSaiMotor(char *_pBuffer, uint32_t _iSize) // 雷赛电机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); + TCANUserData *ptCANUserData = (TCANUserData *)g_ptCAN1->m_pUserData; + uint32_t iCANId = (uint32_t)ptCANUserData->m_canrx->Identifier; // 提取电机ID (0x580 + MotorID) - uint8_t ucMotorID = (uint8_t)(uiFrameID - 0x580); + uint8_t ucMotorID = (uint8_t)(iCANId - 0x580); // 调用电机管理模块解析响应 - MotorMgr_ParseResponse(ucMotorID, (const uint8_t *)&_pBuffer[4], 8); + MotorMgr_ParseResponse(ucMotorID, (const uint8_t *)_pBuffer, _iSize); } void MotorTask(void *argument) @@ -224,7 +179,5 @@ void MotorTask(void *argument) MotorMgr_SetTargetSpeed(g_apstMotors[i], g_aiMotorSpeed[i]); } } - - Rd_Delay(2); // 2ms周期 } }