diff --git a/RBcore/BHBF.c b/RBcore/BHBF.c index 4f862d4..1f9edd1 100644 --- a/RBcore/BHBF.c +++ b/RBcore/BHBF.c @@ -54,63 +54,13 @@ static void RBcore_ModuleHandler(const Msg_t *pstMsg) { return; } - - static int iVehicleSpeed = -1; // 表示由RD1转换来的速度值 - int aiMotorSpeed[2] = {0}; //0索引表示左，1索引表示右，多轮时到对应motor的MOTOR_CMD_SET_SPEED命令自行处理 + switch (pstMsg->m_uiMsgID) { case RBCORE_CMD_STOP_ALL: { - MsgCenter_SendTo(MODULE_NAME_MOTOR, COMMON_CMD_STOP_ALL, NULL, 0); - MsgCenter_SendTo(MODULE_NAME_CUSTOM, COMMON_CMD_STOP_ALL, NULL, 0); - break; - } - case RBCORE_CMD_MANUAL_FORWARD: - { - if (iVehicleSpeed >= 0) - { - aiMotorSpeed[0] = iVehicleSpeed * 10; - aiMotorSpeed[1] = iVehicleSpeed * 10; - MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed)); - } - break; - } - case RBCORE_CMD_MANUAL_BACKWARD: - { - if (iVehicleSpeed >= 0) - { - aiMotorSpeed[0] = -iVehicleSpeed * 10; - aiMotorSpeed[1] = -iVehicleSpeed * 10; - MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed)); - } - break; - } - case RBCORE_CMD_MANUAL_TURNLEFT: - { - if (g_stCV.LeftTurnSpeed > 0 && g_stCV.RightTurnSpeed > 0) - { - aiMotorSpeed[0] = -g_stCV.LeftTurnSpeed; - aiMotorSpeed[1] = g_stCV.RightTurnSpeed; - MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed)); - } - break; - } - case RBCORE_CMD_MANUAL_TURNRIGHT: - { - if (g_stCV.LeftTurnSpeed > 0 && g_stCV.RightTurnSpeed > 0) - { - aiMotorSpeed[0] = g_stCV.LeftTurnSpeed; - aiMotorSpeed[1] = -g_stCV.RightTurnSpeed; - MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed)); - } - break; - } - case RBCORE_GET_VEHICLE_SPEED: - { - if (pstMsg->m_uiDataLen >= sizeof(int)) - { - RD_MEMCPY(&iVehicleSpeed, pstMsg->m_aucData, sizeof(int)); - } + MsgCenter_SendTo(MODULE_NAME_MOTOR, CMD_STOP_ALL, NULL, 0); + MsgCenter_SendTo(MODULE_NAME_CUSTOM, CMD_STOP_ALL, NULL, 0); break; } default: diff --git a/RBcore/include/BHBF.h b/RBcore/include/BHBF.h index 4018efd..b84c9a0 100644 --- a/RBcore/include/BHBF.h +++ b/RBcore/include/BHBF.h @@ -75,24 +75,14 @@ extern "C"{ #define MODULE_NAME_TIMER "timer" typedef enum { - COMMON_CMD_SHOW_INFO, // 终端信息展示 - COMMON_CMD_STOP_ALL, // 机器人停止（建议各个控制硬件的模块都要实现） - MOTOR_START = 0x0100, // *电机命令开始* + CMD_SHOW_INFO, // 终端信息展示 + CMD_STOP_ALL, // 机器人停止（建议各个控制硬件的模块都要实现） + GET_VEHICLE_SPEED, // 获取机器人速度（内部速度归一化） MOTOR_SET_SPEED, // 设置速度 MOTOR_POWER_ENABLE, // 电机上电 MOTOR_POWER_DISABLE, // 电机失电 - MOTOR_GET_VEHICLE_SPEED, // 获取机器人速度（内部速度归一化） MOTOR_CMD_STOP_AFTER, // 先关枪后停车 - RBCORE_START = 0x0200, // *机器人共性命令开始* - RBCORE_CMD_STOP_ALL, // 此命令统一调用机器人各个模块的COMMON_CMD_STOP_ALL - RBCORE_CMD_MANUAL_FORWARD, // 机器人手动前进 - RBCORE_CMD_MANUAL_BACKWARD, // 机器人手动后退 - RBCORE_CMD_MANUAL_TURNLEFT, // 机器人手动左转 - RBCORE_CMD_MANUAL_TURNRIGHT, // 机器人手动右转 - RBCORE_GET_VEHICLE_SPEED, // 获取机器人速度（内部速度归一化） - CUSTOM_START = 0x0300, // *机器人特性命令开始* - CUSTOM_GET_PV, // 获取PV CUSTOM_GET_MK32, // 获取MK32 CUSTOM_GET_TL720D_ROLL, // 获取陀螺仪横滚角（IV显示） diff --git a/RBcore/msp_Timer.c b/RBcore/msp_Timer.c index 99e857a..a3d0e00 100644 --- a/RBcore/msp_Timer.c +++ b/RBcore/msp_Timer.c @@ -159,7 +159,7 @@ static void Timer_ModuleHandler(const Msg_t *pstMsg) } break; } - case COMMON_CMD_SHOW_INFO: + case CMD_SHOW_INFO: { lua_print("\ng_dletAngle = %d\nuiLastSendTick = %d\nbTimerStarted = %d\ng_RF_Angle_Roll = %d\n", g_dletAngle, uiLastSendTick, bTimerStarted, g_RF_Angle_Roll); diff --git a/README.md b/README.md index 688edeb..95b5a54 100644 --- a/README.md +++ b/README.md @@ -18,7 +18,7 @@ c1((按下急停)) end a((Read_MK32))-->CUSTOM_GET_MK32-->MOTOR_POWER_ENABLE-->CUSTOM_GET_MOTOR_OK-->c0 c1-->MOTOR_POWER_DISABLE-->m0 -c1-->RBCORE_CMD_STOP_ALL-->COMMON_CMD_STOP_ALL +c1-->RBCORE_CMD_STOP_ALL-->CMD_STOP_ALL ``` diff --git a/project/paint_robot_new/paint_robot_new.c b/project/paint_robot_new/paint_robot_new.c index a1dff3d..fda2a6e 100644 --- a/project/paint_robot_new/paint_robot_new.c +++ b/project/paint_robot_new/paint_robot_new.c @@ -189,7 +189,7 @@ static void Move_Halt_AngleError(void) { if (Rd_GetTime() - uiLastTime >= 600 * g_stCV.Paint_Gun_Shutdown_Distance / g_stIV.RobotMoveSpeed) { - MsgCenter_SendTo(MODULE_NAME_MOTOR, COMMON_CMD_STOP_ALL, NULL, 0); + MsgCenter_SendTo(MODULE_NAME_MOTOR, CMD_STOP_ALL, NULL, 0); } return; } @@ -202,7 +202,7 @@ static void Move_Halt_AngleError(void) { if(Rd_GetTime() - g_stAngleError_Ctl.m_ipaintOffCount > 1000) { - MsgCenter_SendTo(MODULE_NAME_CUSTOM, COMMON_CMD_STOP_ALL, NULL, 0);//先关枪 + MsgCenter_SendTo(MODULE_NAME_CUSTOM, CMD_STOP_ALL, NULL, 0);//先关枪 g_stAngleError_Ctl.m_iAnglelock = 1; return; } @@ -252,8 +252,16 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) if (pstMsg->m_uiDataLen >= sizeof(uiFailtCode)) { RD_MEMCPY(uiFailtCode, pstMsg->m_aucData, sizeof(uiFailtCode)); - if (uiFailtCode[0] == 1) g_stIV.Left_Motor_Err = uiFailtCode[1]; - else g_stIV.Right_Motor_Err = uiFailtCode[1]; + if (uiFailtCode[0] == 1) + { + g_stIV.Left_Motor_Err = uiFailtCode[1]; + MsgCenter_SendTo(MODULE_NAME_DAEMON, DAEMON_SET_LEFT_MOTOR, NULL, 0); + } + else + { + g_stIV.Right_Motor_Err = uiFailtCode[1]; + MsgCenter_SendTo(MODULE_NAME_DAEMON, DAEMON_SET_RIGHT_MOTOR, NULL, 0); + } } break; } @@ -266,6 +274,11 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) if (1 == iCurrentSpeed[0]) { g_stIV.CurrentSpeed = iCurrentSpeed[1]; + MsgCenter_SendTo(MODULE_NAME_DAEMON, DAEMON_SET_LEFT_MOTOR, NULL, 0); + } + else + { + MsgCenter_SendTo(MODULE_NAME_DAEMON, DAEMON_SET_RIGHT_MOTOR, NULL, 0); } } break; @@ -301,7 +314,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) g_ichangLineState = 1; // 走到这说明换道第一次转完了 break; } - case COMMON_CMD_STOP_ALL: + case CMD_STOP_ALL: { HAL_GPIO_WritePin(OUT_1_GPIO_Port, OUT_1_Pin, 1); g_stAngleError_Ctl.m_iPaintState = 1; @@ -382,8 +395,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) iVehicleSpeed = g_stCV.Speed_m_per_min*iSpeedSelection/30; } g_stIV.RobotMoveSpeed = iVehicleSpeed; - MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_GET_VEHICLE_SPEED, (void *)&iVehicleSpeed, sizeof(int)); - MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_GET_VEHICLE_SPEED, (void *)&iVehicleSpeed, sizeof(int)); + MsgCenter_SendTo(MODULE_NAME_MOTOR, GET_VEHICLE_SPEED, (void *)&iVehicleSpeed, sizeof(int)); Move_Halt_AngleError(); @@ -459,12 +471,18 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) CH13_S2_Value = g_stMK32.CH13_S2; } + int aiMotorSpeed[2] = {0}; //0索引表示左，1索引表示右，多轮时到对应motor的MOTOR_CMD_SET_SPEED命令自行处理 // 自动巡航 if (g_stMK32.CH5_SB == -1000) { if (g_stPV.RunMode == 1) { - MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_FORWARD, NULL, 0); + if (iVehicleSpeed >= 0) + { + aiMotorSpeed[0] = iVehicleSpeed * 10; + aiMotorSpeed[1] = iVehicleSpeed * 10; + MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed)); + } } else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3) { @@ -485,7 +503,12 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) { if (g_stPV.RunMode == 1) { - MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_BACKWARD, NULL, 0); + if (iVehicleSpeed >= 0) + { + aiMotorSpeed[0] = -iVehicleSpeed * 10; + aiMotorSpeed[1] = -iVehicleSpeed * 10; + MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed)); + } } else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3) { @@ -512,7 +535,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) } else { - MsgCenter_SendTo(MODULE_NAME_MOTOR, COMMON_CMD_STOP_ALL, NULL, 0); + MsgCenter_SendTo(MODULE_NAME_MOTOR, CMD_STOP_ALL, NULL, 0); } break; } @@ -525,8 +548,13 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) { if (g_stPV.RunMode == 1) { - MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_FORWARD, NULL, 0); - } + if (iVehicleSpeed >= 0) + { + aiMotorSpeed[0] = iVehicleSpeed * 10; + aiMotorSpeed[1] = iVehicleSpeed * 10; + MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed)); + } + } else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3) { if (0 == g_stAngleError_Ctl.m_iAnglelock) @@ -545,7 +573,12 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) { if (g_stPV.RunMode == 1) { - MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_BACKWARD, NULL, 0); + if (iVehicleSpeed >= 0) + { + aiMotorSpeed[0] = -iVehicleSpeed * 10; + aiMotorSpeed[1] = -iVehicleSpeed * 10; + MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed)); + } } else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3) { @@ -563,19 +596,29 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) } else if (abs(angle - 0) <= g_stCV.Joy_Sticker_Angle_Allowance) { - MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_TURNRIGHT, NULL, 0); + if (g_stCV.LeftTurnSpeed > 0 && g_stCV.RightTurnSpeed > 0) + { + aiMotorSpeed[0] = g_stCV.LeftTurnSpeed; + aiMotorSpeed[1] = -g_stCV.RightTurnSpeed; + MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed)); + } } else if (abs(angle - 180) <= g_stCV.Joy_Sticker_Angle_Allowance || abs(angle - (-180)) <= g_stCV.Joy_Sticker_Angle_Allowance) { - MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_TURNLEFT, NULL, 0); + if (g_stCV.LeftTurnSpeed > 0 && g_stCV.RightTurnSpeed > 0) + { + aiMotorSpeed[0] = -g_stCV.LeftTurnSpeed; + aiMotorSpeed[1] = g_stCV.RightTurnSpeed; + MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed)); + } } else { - MsgCenter_SendTo(MODULE_NAME_MOTOR, COMMON_CMD_STOP_ALL, NULL, 0); + MsgCenter_SendTo(MODULE_NAME_MOTOR, CMD_STOP_ALL, NULL, 0); } break; } - case COMMON_CMD_SHOW_INFO: + case CMD_SHOW_INFO: { lua_print("MK32 Info\nRxIndex:%d\tIsOnline:%d\n", g_stMK32.RxIndex, g_stMK32.IsOnline); lua_print("CH0_RY_H\t%d\n", g_stMK32.CH0_RY_H); diff --git a/project/paint_robot_new/paint_robot_new_motors.c b/project/paint_robot_new/paint_robot_new_motors.c index 50c8c5a..f10176c 100644 --- a/project/paint_robot_new/paint_robot_new_motors.c +++ b/project/paint_robot_new/paint_robot_new_motors.c @@ -97,7 +97,7 @@ static void Motor_ModuleHandler(const Msg_t *pstMsg) HAL_GPIO_WritePin(OUT_0_GPIO_Port, OUT_0_Pin, 1); break; } - case MOTOR_GET_VEHICLE_SPEED: + case GET_VEHICLE_SPEED: { if (pstMsg->m_uiDataLen >= sizeof(int)) { @@ -112,7 +112,7 @@ static void Motor_ModuleHandler(const Msg_t *pstMsg) if (1 == g_iLastVehicleMove) { MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_SET_STOP_OFF_PAINT, NULL, 0); - MsgCenter_SendTo(MODULE_NAME_CUSTOM, COMMON_CMD_STOP_ALL, NULL, 0);//先关枪 + MsgCenter_SendTo(MODULE_NAME_CUSTOM, CMD_STOP_ALL, NULL, 0);//先关枪 iIsDelayStop = 1; } @@ -134,14 +134,14 @@ static void Motor_ModuleHandler(const Msg_t *pstMsg) } break; } - case COMMON_CMD_STOP_ALL: + case CMD_STOP_ALL: { memset(g_aiMotorSpeed, 0, sizeof(g_aiMotorSpeed)); uiLastTime = Rd_GetTime(); g_iLastVehicleMove = 0; break; } - case COMMON_CMD_SHOW_INFO: + case CMD_SHOW_INFO: { lua_print("iVehicleSpeed = %d\nuiLastTime = %d\ng_isMotorInit = %d\ng_iLastVehicleMove = %d\n", iVehicleSpeed, uiLastTime, g_isMotorInit, g_iLastVehicleMove); @@ -178,14 +178,6 @@ static void decode_LeiSaiMotor(const char *_pBuffer, uint32_t _iSize) // 调用电机管理模块解析响应 MotorMgr_ParseResponse(ucMotorID, (const uint8_t *)_pBuffer, _iSize); - if (1 == ucMotorID) - { - MsgCenter_SendTo(MODULE_NAME_DAEMON, DAEMON_SET_LEFT_MOTOR, NULL, 0); - } - else if (2 == ucMotorID) - { - MsgCenter_SendTo(MODULE_NAME_DAEMON, DAEMON_SET_RIGHT_MOTOR, NULL, 0); - } } void MotorTask(void *argument)