diff --git a/project/paint_robot_new/paint_robot_new.c b/project/paint_robot_new/paint_robot_new.c index a826664..3138ad9 100644 --- a/project/paint_robot_new/paint_robot_new.c +++ b/project/paint_robot_new/paint_robot_new.c @@ -48,9 +48,9 @@ static SP_MSP_MK32_Button g_stMK32; static PV_struct_define g_stPV; static IV_struct_define g_stIV; static int g_RB_State = 0; // 机器人状态,1表示处于竖直行走模式下 -static int g_Paint_State = 0; // 喷枪状态,0表示打开,1表示关闭 +static int g_Paint_State = -1; // 喷枪状态,0表示打开,1表示关闭 static int angle_protect_lock = 0; // 为1表示角度异常,触发停车逻辑Move_Halt_AngleError -static int g_ipaintOffCount = 0; // Move_Halt_AngleError中的计时,即角度便宜超过多少时间触发停枪 +static int g_ipaintOffCount = 0; // Move_Halt_AngleError中的计时,即角度偏移超过多少时间触发停枪 /*----------------------------------------------* * 常量定义 * @@ -82,11 +82,11 @@ static void Move_Halt_AngleError(void) if (1 == angle_protect_lock) { - if (Rd_GetTime() - uiLastTime >= 600 * g_stCV.Paint_Gun_Shutdown_Distance / g_stIV.RobotMoveSpeed) + if (Rd_GetTime() - uiLastTime >= 6000 * g_stCV.Paint_Gun_Shutdown_Distance / g_stIV.RobotMoveSpeed) { MsgCenter_SendTo(MODULE_NAME_MOTOR, COMMON_CMD_STOP_ALL, NULL, 0); - return; } + return; } else { @@ -95,7 +95,7 @@ static void Move_Halt_AngleError(void) if ((abs(g_stIV.CurrentAngle - g_stCV.RobotUpAngleValue) > g_stCV.Robot_Permitted_Angler_Error_Value_E_2D) && 0 == g_Paint_State) { - if(++g_ipaintOffCount > 500) + if(g_ipaintOffCount++ > 500) { MsgCenter_SendTo(MODULE_NAME_CUSTOM, COMMON_CMD_STOP_ALL, NULL, 0);//先关枪 angle_protect_lock = 1; @@ -242,6 +242,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; break; } case CUSTOM_CMD_PAINTGUN: