diff --git a/project/paint_robot_new/paint_robot_new.c b/project/paint_robot_new/paint_robot_new.c index 5159f86..f63db16 100644 --- a/project/paint_robot_new/paint_robot_new.c +++ b/project/paint_robot_new/paint_robot_new.c @@ -47,9 +47,6 @@ static uint32_t g_uiCustomModuleID = 0; static SP_MSP_MK32_Button g_stMK32; static PV_struct_define g_stPV; static IV_struct_define g_stIV; -static int g_Paint_State = -1; // 喷枪状态，0表示打开，1表示关闭 -static int g_angle_protect_lock = 0; // 为1表示角度异常，触发停车逻辑Move_Halt_AngleError -static int g_ipaintOffCount = 0; // Move_Halt_AngleError中的计时，即角度偏移超过多少时间触发停枪 static int g_Is_All_Button_Reset = 0; // 1表示按钮正常工作，0表示需要复位 /*----------------------------------------------* @@ -67,6 +64,14 @@ static int32_t RunTime_DistanceCm_SpeedE_2MPMin(void) return (600 * (g_stPV.LaneChangeDistance + g_stCV.Vertical_ChangeLane_Compensation) / g_stCV.Lane_Change_Speed_m_per_min); } +static struct { + int m_iPaintState; // 喷枪状态：0=打开，1=关闭 + int m_iAnglelock; // 1=角度异常，触发 Move_Halt_AngleError 并屏蔽互斥代码 + int m_ipaintOffCount; // Move_Halt_AngleError 中的计时，当前为 1 秒 +} g_stAngleError_Ctl = { + .m_iPaintState = -1 +}; + static void Move_Halt_AngleError(void) { static uint32_t uiLastTime = 0; @@ -75,7 +80,7 @@ static void Move_Halt_AngleError(void) return; } - if (1 == g_angle_protect_lock) + if (1 == g_stAngleError_Ctl.m_iAnglelock) { if (Rd_GetTime() - uiLastTime >= 600 * g_stCV.Paint_Gun_Shutdown_Distance / g_stIV.RobotMoveSpeed) { @@ -88,18 +93,18 @@ static void Move_Halt_AngleError(void) uiLastTime = Rd_GetTime(); } - if ((abs(g_stIV.CurrentAngle - g_stCV.RobotUpAngleValue) > g_stCV.Robot_Permitted_Angler_Error_Value_E_2D) && 0 == g_Paint_State) + if ((abs(g_stIV.CurrentAngle - g_stCV.RobotUpAngleValue) > g_stCV.Robot_Permitted_Angler_Error_Value_E_2D) && 0 == g_stAngleError_Ctl.m_iPaintState) { - if(Rd_GetTime() - g_ipaintOffCount > 1000) + if(Rd_GetTime() - g_stAngleError_Ctl.m_ipaintOffCount > 1000) { MsgCenter_SendTo(MODULE_NAME_CUSTOM, COMMON_CMD_STOP_ALL, NULL, 0);//先关枪 - g_angle_protect_lock = 1; + g_stAngleError_Ctl.m_iAnglelock = 1; return; } } else { - g_ipaintOffCount = Rd_GetTime(); + g_stAngleError_Ctl.m_ipaintOffCount = Rd_GetTime(); } } @@ -252,7 +257,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) case COMMON_CMD_STOP_ALL: { HAL_GPIO_WritePin(OUT_1_GPIO_Port, OUT_1_Pin, 1); - g_Paint_State = 1; + g_stAngleError_Ctl.m_iPaintState = 1; break; } case CUSTOM_CMD_PAINTGUN: @@ -317,8 +322,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) && (g_stMK32.CH6_SC != -1000) ) { g_stIV.IsWorking = 0; - g_angle_protect_lock = 0; // 遥控器复位认为打开角度锁 - g_ipaintOffCount = 0; + g_stAngleError_Ctl.m_iAnglelock = 0; // 遥控器复位认为打开角度锁 + g_stAngleError_Ctl.m_ipaintOffCount = 0; } else { @@ -375,7 +380,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) } } - if (0 == g_angle_protect_lock) + if (0 == g_stAngleError_Ctl.m_iAnglelock) { // 开关喷枪 if (g_stPV.RunMode == 1) @@ -384,15 +389,15 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) { if (0 == iIsStopOffPaint) { - g_Paint_State = 0; - MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_Paint_State, sizeof(int)); + g_stAngleError_Ctl.m_iPaintState = 0; + MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_stAngleError_Ctl.m_iPaintState, sizeof(int)); } } else { iIsStopOffPaint = 0; - g_Paint_State = 1; - MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_Paint_State, sizeof(int)); + g_stAngleError_Ctl.m_iPaintState = 1; + MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_stAngleError_Ctl.m_iPaintState, sizeof(int)); } } else if(g_stPV.RunMode == 2 || g_stPV.RunMode == 3) @@ -401,12 +406,12 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) { if(g_stIV.CurrentSpeed != 0) { - g_Paint_State = 0; - MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_Paint_State, sizeof(int)); + g_stAngleError_Ctl.m_iPaintState = 0; + MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_stAngleError_Ctl.m_iPaintState, sizeof(int)); } else { - log_w("cannot open paint with 0 move speed !"); + //log_w("cannot open paint with 0 move speed !"); } } } @@ -422,7 +427,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) } else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3) { - if (0 == g_angle_protect_lock) + if (0 == g_stAngleError_Ctl.m_iAnglelock) { BHBF_straight_drive_Cmd stCmd = { .m_iMode = 1, @@ -443,7 +448,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) } else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3) { - if (0 == g_angle_protect_lock) + if (0 == g_stAngleError_Ctl.m_iAnglelock) { BHBF_straight_drive_Cmd stCmd = { .m_iMode = -1, @@ -483,7 +488,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) } else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3) { - if (0 == g_angle_protect_lock) + if (0 == g_stAngleError_Ctl.m_iAnglelock) { BHBF_straight_drive_Cmd stCmd = { .m_iMode = 1, @@ -503,7 +508,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) } else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3) { - if (0 == g_angle_protect_lock) + if (0 == g_stAngleError_Ctl.m_iAnglelock) { BHBF_straight_drive_Cmd stCmd = { .m_iMode = -1, @@ -551,8 +556,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) lua_print("PV Info\n"); lua_print("{%d, %d, %d, %d, %d, %ld}\n", g_stPV.RunMode, g_stPV.RobotSpeed, g_stPV.LaneChangeDistance, (int)g_stPV.Vertical_Calibration, g_stPV.IV_IsRestart_Notified, (long long)g_stPV.TimeStamp); - lua_print("\niPaint = %d\ng_Paint_State = %d\ng_angle_protect_lock = %d\ng_ipaintOffCount = %d\ng_Is_All_Button_Reset = %d\n", - iPaint, g_Paint_State, g_angle_protect_lock, g_ipaintOffCount, g_Is_All_Button_Reset); + lua_print("\niPaint = %d\ng_stAngleError_Ctl.m_iPaintState = %d\ng_stAngleError_Ctl.m_iAnglelock = %d\ng_stAngleError_Ctl.m_ipaintOffCount = %d\ng_Is_All_Button_Reset = %d\n", + iPaint, g_stAngleError_Ctl.m_iPaintState, g_stAngleError_Ctl.m_iAnglelock, g_stAngleError_Ctl.m_ipaintOffCount, g_Is_All_Button_Reset); break; } default: