diff --git a/RBcore/include/BHBF.h b/RBcore/include/BHBF.h index 5c17eb8..6ef0cdf 100644 --- a/RBcore/include/BHBF.h +++ b/RBcore/include/BHBF.h @@ -78,6 +78,7 @@ typedef enum { COMMON_CMD_STOP_ALL, // 机器人停止(建议各个控制硬件的模块都要实现) MOTOR_START = 0x0100, // *电机命令开始* MOTOR_SET_SPEED, // 设置速度 + MOTOR_POWER_ENABLE, // 电机上电 MOTOR_POWER_DISABLE, // 电机失电 RBCORE_START = 0x0200, // *机器人共性命令开始* diff --git a/project/paint_robot_new/paint_robot_new.c b/project/paint_robot_new/paint_robot_new.c index 030457e..b675162 100644 --- a/project/paint_robot_new/paint_robot_new.c +++ b/project/paint_robot_new/paint_robot_new.c @@ -268,6 +268,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) if (g_stMK32.CH4_SA == 0 && g_stMK32.CH5_SB == 0 && g_stMK32.CH6_SC == 0 && g_stMK32.CH7_SD == 0 && g_stMK32.IsOnline == 1) { + MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_POWER_ENABLE, NULL, 0); g_Is_All_Button_Reset = 1; } } diff --git a/project/paint_robot_new/paint_robot_new_motors.c b/project/paint_robot_new/paint_robot_new_motors.c index a48a418..f4d0a1d 100644 --- a/project/paint_robot_new/paint_robot_new_motors.c +++ b/project/paint_robot_new/paint_robot_new_motors.c @@ -43,6 +43,7 @@ static Motor_Instance_t *g_apstMotors[MOTOR_INSTANCE_MAX] = {NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL}; static uint32_t g_uiMotorModuleID = 0; static int g_aiMotorSpeed[MOTOR_INSTANCE_MAX] = {0}; +static int g_isMotorInit = -1; // -1表示还在等待遥控器初始化完毕,0表示可以进行电机初始化,1表示电机初始化完毕,运行电机主循环 /*----------------------------------------------* * 常量定义 * @@ -72,6 +73,11 @@ static void Motor_ModuleHandler(const Msg_t *pstMsg) g_aiMotorSpeed[1] = MotorMgr_SpeedConvert(g_apstMotors[1], aiMotorSpeed[1]); break; } + case MOTOR_POWER_ENABLE: + { + g_isMotorInit = 0; + break; + } case MOTOR_POWER_DISABLE: { HAL_GPIO_WritePin(OUT_0_GPIO_Port, OUT_0_Pin, 1); @@ -125,83 +131,88 @@ static void decode_LeiSaiMotor(const char *_pBuffer, uint32_t _iSize) void MotorTask(void *argument) { - // 此时消息中心还未就绪,直接调用HAL库接口给电机上电 - HAL_GPIO_WritePin(OUT_0_GPIO_Port, OUT_0_Pin, 0); - // 初始化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 = g_stCV.pulse_Per_Circle, - .m_uiReductionRatio = g_stCV.wheel_Reduction_Ratio, - .m_fWheelDiameter = g_stCV.wheel_Diameter_m - }; - g_apstMotors[0] = MotorMgr_Create(&stLeftMotorConfig, g_ptCAN1, LeiSai_GetProtocol(), NULL); - - // 配置右轮电机 - Motor_Config_t stRightMotorConfig = { - .m_ucMotorID = 2, - .m_uiPulsePerRound = g_stCV.pulse_Per_Circle, - .m_uiReductionRatio = g_stCV.wheel_Reduction_Ratio, - .m_fWheelDiameter = g_stCV.wheel_Diameter_m - }; - g_apstMotors[1] = MotorMgr_Create(&stRightMotorConfig, g_ptCAN1, LeiSai_GetProtocol(), NULL); - - g_uiMotorModuleID = MsgCenter_Register(MODULE_NAME_MOTOR, Motor_ModuleHandler); - - 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]); - + g_uiMotorModuleID = MsgCenter_Register(MODULE_NAME_MOTOR, Motor_ModuleHandler); while(1) { - static int LS_Motor_Config_Count = 0; - if (LS_Motor_Config_Count <= 200) - { - LS_Motor_Config_Count++; - MotorMgr_SetWatchdog(g_apstMotors[0], 1000); + // 处理消息 + MsgCenter_ProcessWait(g_uiMotorModuleID, 2); + + if (0 == g_isMotorInit) + { + HAL_GPIO_WritePin(OUT_0_GPIO_Port, OUT_0_Pin, 0); + // 初始化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 = g_stCV.pulse_Per_Circle, + .m_uiReductionRatio = g_stCV.wheel_Reduction_Ratio, + .m_fWheelDiameter = g_stCV.wheel_Diameter_m + }; + g_apstMotors[0] = MotorMgr_Create(&stLeftMotorConfig, g_ptCAN1, LeiSai_GetProtocol(), NULL); + + // 配置右轮电机 + Motor_Config_t stRightMotorConfig = { + .m_ucMotorID = 2, + .m_uiPulsePerRound = g_stCV.pulse_Per_Circle, + .m_uiReductionRatio = g_stCV.wheel_Reduction_Ratio, + .m_fWheelDiameter = g_stCV.wheel_Diameter_m + }; + g_apstMotors[1] = MotorMgr_Create(&stRightMotorConfig, g_ptCAN1, LeiSai_GetProtocol(), NULL); + + 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]); + g_isMotorInit = 1; + } + else if (g_isMotorInit > 0) + { + static int LS_Motor_Config_Count = 0; + if (LS_Motor_Config_Count <= 200) + { + LS_Motor_Config_Count++; + MotorMgr_SetWatchdog(g_apstMotors[0], 1000); + Rd_Delay(4); + MotorMgr_SetWatchdog(g_apstMotors[1], 1000); + Rd_Delay(4); + } + MotorMgr_RequestPosition(g_apstMotors[0]); Rd_Delay(4); - MotorMgr_SetWatchdog(g_apstMotors[1], 1000); + MotorMgr_RequestPosition(g_apstMotors[1]); Rd_Delay(4); - } - // 处理消息 - MsgCenter_ProcessWait(g_uiMotorModuleID, 2); - MotorMgr_RequestPosition(g_apstMotors[0]); - Rd_Delay(4); - MotorMgr_RequestPosition(g_apstMotors[1]); - Rd_Delay(4); - MotorMgr_RequestFault(g_apstMotors[0]); - Rd_Delay(4); - MotorMgr_RequestFault(g_apstMotors[1]); - Rd_Delay(4); - MotorMgr_RequestVelocity(g_apstMotors[0]); - Rd_Delay(4); - MotorMgr_RequestVelocity(g_apstMotors[1]); - Rd_Delay(4); - MotorMgr_SetTargetSpeed(g_apstMotors[0], g_aiMotorSpeed[0]); - Rd_Delay(4); - MotorMgr_SetTargetSpeed(g_apstMotors[1], g_aiMotorSpeed[1]); - Rd_Delay(4); + MotorMgr_RequestFault(g_apstMotors[0]); + Rd_Delay(4); + MotorMgr_RequestFault(g_apstMotors[1]); + Rd_Delay(4); + MotorMgr_RequestVelocity(g_apstMotors[0]); + Rd_Delay(4); + MotorMgr_RequestVelocity(g_apstMotors[1]); + Rd_Delay(4); + MotorMgr_SetTargetSpeed(g_apstMotors[0], g_aiMotorSpeed[0]); + Rd_Delay(4); + MotorMgr_SetTargetSpeed(g_apstMotors[1], g_aiMotorSpeed[1]); + Rd_Delay(4); + } } }