diff --git a/project/paint_robot_new/paint_robot_new.c b/project/paint_robot_new/paint_robot_new.c index 7896d82..5159f86 100644 --- a/project/paint_robot_new/paint_robot_new.c +++ b/project/paint_robot_new/paint_robot_new.c @@ -116,6 +116,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) static int iIsStopOffPaint = 0; static int iPaint = 1; static int iMotorOK = 0; + static int CH13_S2_Value = 0; switch (pstMsg->m_uiMsgID) { case CUSTOM_RESET_PAINT: @@ -277,6 +278,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) { + CH13_S2_Value = g_stMK32.CH13_S2; //防止第一次值不正确 MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_POWER_ENABLE, NULL, 0); g_Is_All_Button_Reset = 1; } @@ -292,6 +294,11 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) && (g_stMK32.CH9_SF == -1000 || g_stMK32.CH9_SF == 1000 || g_stMK32.CH9_SF == 0)) || g_stIV.Left_Motor_Err != 0 || g_stIV.Right_Motor_Err != 0) { + if (g_stIV.Left_Motor_Err != 0 || g_stIV.Right_Motor_Err != 0) + { + log_a("Motor Error ! error code: left [%d] right [%d]", g_stIV.Left_Motor_Err, g_stIV.Right_Motor_Err); + } + MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_POWER_DISABLE, NULL, 0); MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0); g_Is_All_Button_Reset = 0; @@ -390,7 +397,6 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) } else if(g_stPV.RunMode == 2 || g_stPV.RunMode == 3) { - static int CH13_S2_Value = 0; if (g_stMK32.CH13_S2 != CH13_S2_Value) { if(g_stIV.CurrentSpeed != 0) @@ -403,8 +409,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) log_w("cannot open paint with 0 move speed !"); } } - CH13_S2_Value = g_stMK32.CH13_S2; } + CH13_S2_Value = g_stMK32.CH13_S2; } // 自动巡航