diff --git a/motor/CMakeLists.txt b/motor/CMakeLists.txt index e275e8d..d904896 100644 --- a/motor/CMakeLists.txt +++ b/motor/CMakeLists.txt @@ -13,4 +13,10 @@ if(TARGET peripheral) target_link_libraries(${TARGET_NAME} PUBLIC peripheral) else() message(FATAL_ERROR "[${TARGET_NAME}] Dependency 'peripheral' not found.") +endif() + +if(TARGET RBcore) + target_link_libraries(${TARGET_NAME} PUBLIC RBcore) +else() + message(FATAL_ERROR "[${TARGET_NAME}] Dependency 'RBcore' not found.") endif() \ No newline at end of file diff --git a/motor/include/motor_manager.h b/motor/include/motor_manager.h index e0a9976..afa4588 100644 --- a/motor/include/motor_manager.h +++ b/motor/include/motor_manager.h @@ -163,6 +163,7 @@ void MotorMgr_ParseResponse(uint8_t ucMotorID, const uint8_t *pucData, uint32_t * @brief 速度单位转换 */ int32_t MotorMgr_SpeedConvert(Motor_Instance_t *pstMotor, int32_t iSpeedUserUnit); +int32_t MotorMgr_SpeedConvertex(Motor_Instance_t *pstMotor, int32_t iSpeedCount); /** * @brief 获取电机状态 diff --git a/motor/include/motor_protocol.h b/motor/include/motor_protocol.h index 436ef45..a60b4d0 100644 --- a/motor/include/motor_protocol.h +++ b/motor/include/motor_protocol.h @@ -115,6 +115,9 @@ typedef struct { // 速度单位转换:用户单位 -> 脉冲/秒 int32_t (*pfSpeedConvert)(struct Motor_Instance *pstMotor, int32_t iSpeedUserUnit); + + // 速度单位转换:脉冲/秒 -> 用户单位 + int32_t (*pfSpeedConvertex)(struct Motor_Instance *pstMotor, int32_t iSpeedCount); } Motor_Protocol_t; // 电机实例结构体 diff --git a/motor/motor_manager.c b/motor/motor_manager.c index f15c453..0312055 100644 --- a/motor/motor_manager.c +++ b/motor/motor_manager.c @@ -468,6 +468,23 @@ int32_t MotorMgr_SpeedConvert(Motor_Instance_t *pstMotor, int32_t iSpeedUserUnit return 0; } +/***************************************************************************** + 函 数 名 : MotorMgr_SpeedConvert + 功能描述 : 速度单位转换 + 输入参数 : Motor_Instance_t *pstMotor 电机实例 + int32_t iSpeedCount 脉冲/秒 + 输出参数 : 无 + 返 回 值 : int32_t 用户速度单位 +*****************************************************************************/ +int32_t MotorMgr_SpeedConvertex(Motor_Instance_t *pstMotor, int32_t iSpeedCount) +{ + if (pstMotor->m_pstProtocol && pstMotor->m_pstProtocol->pfSpeedConvertex) + { + return pstMotor->m_pstProtocol->pfSpeedConvertex(pstMotor, iSpeedCount); + } + return 0; +} + /***************************************************************************** 函 数 名 : MotorMgr_GetState 功能描述 : 获取电机状态 diff --git a/motor/msp_motor_leisai.c b/motor/msp_motor_leisai.c index d0f18d8..6e3bc29 100644 --- a/motor/msp_motor_leisai.c +++ b/motor/msp_motor_leisai.c @@ -19,6 +19,7 @@ #include "msp_motor_leisai.h" #include "rd_time.h" +#include "BHBF.h" /*----------------------------------------------* * 外部变量说明 * @@ -52,6 +53,7 @@ static int LeiSai_SetTargetPosition(Motor_Instance_t *pstMotor, int32_t iTargetP 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); +static int32_t LeiSai_SpeedConvertex(Motor_Instance_t *pstMotor, int32_t iSpeedCount); /*----------------------------------------------* * 全局变量 * @@ -76,6 +78,7 @@ const Motor_Protocol_t g_stLeiSaiProtocol = { .pfSetDO = LeiSai_SetDO, .pfParseResponse = LeiSai_ParseResponse, .pfSpeedConvert = LeiSai_SpeedConvert, + .pfSpeedConvertex = LeiSai_SpeedConvertex, }; /*----------------------------------------------* @@ -393,12 +396,29 @@ static int LeiSai_ParseResponse(Motor_Instance_t *pstMotor, const uint8_t *pucDa { case LEISAI_OD_FAULT_CODE: { - pstMotor->m_eState = MOTOR_STATE_ERROR; + uint32_t uiFaultCode[2] = {0}; + uiFaultCode[1] = (uint32_t)(pucData[4] + | (pucData[5] << 8) + | (pucData[6] << 16) + | (pucData[7] << 24)); + uiFaultCode[0] = pstMotor->m_stConfig.m_ucMotorID; + if (uiFaultCode[1] != 0) + { + pstMotor->m_eState = MOTOR_STATE_ERROR; + } + MsgCenter_SendTo(MODULE_NAME_CUSTOM, MOTOR_CMD_GET_FAULT_CODE, uiFaultCode, sizeof(uiFaultCode)); break; } case LEISAI_OD_VELOCITY_ACTUAL: { - + int32_t iRealVelocity[2] = {0}; + iRealVelocity[0] = (int32_t)(pucData[4] + | (pucData[5] << 8) + | (pucData[6] << 16) + | (pucData[7] << 24)); + iRealVelocity[1] = MotorMgr_SpeedConvertex(pstMotor, iRealVelocity[0]); + iRealVelocity[0] = pstMotor->m_stConfig.m_ucMotorID; + MsgCenter_SendTo(MODULE_NAME_CUSTOM, MOTOR_CMD_GET_SPEED, iRealVelocity, sizeof(iRealVelocity)); break; } default: @@ -427,3 +447,19 @@ static int32_t LeiSai_SpeedConvert(Motor_Instance_t *pstMotor, int32_t iSpeedE01 return (int32_t)dPulsePerSec; } + +/***************************************************************************** + 函 数 名 : LeiSai_SpeedConvertex + 功能描述 : 速度单位转换:脉冲/秒 -> m/min +*****************************************************************************/ +static int32_t LeiSai_SpeedConvertex(Motor_Instance_t *pstMotor, int32_t iSpeedCount) +{ + if (NULL == pstMotor) + { + return 0; + } + + double dPulsePerSec = iSpeedCount*60.0/pstMotor->m_stConfig.m_uiPulsePerRound/pstMotor->m_stConfig.m_uiReductionRatio* 3.14 * pstMotor->m_stConfig.m_fWheelDiameter; + + return (int32_t)dPulsePerSec; +}