|
|
|
@ -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: |
|
|
|
|
