LiZongdi
/
robot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

三个全局归一为一个结构体

paint_robot_new-v1.5
Lizongdi 1 week ago
parent
5e17cc0f50
commit
7b28cf5095
1 changed files with 30 additions and 25 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 55
      project/paint_robot_new/paint_robot_new.c

55
project/paint_robot_new/paint_robot_new.c
View File

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

Write Preview
Loading…
Cancel
Save