From c6efcef7d9d61d2f068655aa30841b5a4733ff89 Mon Sep 17 00:00:00 2001 From: Lizongdi <1210855344@qq.com> Date: Tue, 15 Sep 2026 11:34:12 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E8=B6=85=E6=97=B6=E5=81=9C?= =?UTF-8?q?=E8=BD=A6=E9=80=BB=E8=BE=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- project/paint_robot_new/paint_robot_new.c | 207 ++++++++++++++++------ 1 file changed, 148 insertions(+), 59 deletions(-) diff --git a/project/paint_robot_new/paint_robot_new.c b/project/paint_robot_new/paint_robot_new.c index dde4e8e..a826664 100644 --- a/project/paint_robot_new/paint_robot_new.c +++ b/project/paint_robot_new/paint_robot_new.c @@ -47,6 +47,10 @@ 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_RB_State = 0; // 机器人状态，1表示处于竖直行走模式下 +static int g_Paint_State = 0; // 喷枪状态，0表示打开，1表示关闭 +static int angle_protect_lock = 0; // 为1表示角度异常，触发停车逻辑Move_Halt_AngleError +static int g_ipaintOffCount = 0; // Move_Halt_AngleError中的计时，即角度便宜超过多少时间触发停枪 /*----------------------------------------------* * 常量定义 * @@ -63,6 +67,48 @@ 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 void Move_Halt_AngleError(void) +{ + static uint32_t uiLastTime = 0; + if (g_stPV.RunMode != 2 && g_stPV.RunMode != 3) + { + return; + } + + if (0 == g_RB_State) + { + return; + } + + if (1 == angle_protect_lock) + { + if (Rd_GetTime() - uiLastTime >= 600 * g_stCV.Paint_Gun_Shutdown_Distance / g_stIV.RobotMoveSpeed) + { + MsgCenter_SendTo(MODULE_NAME_MOTOR, COMMON_CMD_STOP_ALL, NULL, 0); + return; + } + } + else + { + 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(++g_ipaintOffCount > 500) + { + MsgCenter_SendTo(MODULE_NAME_CUSTOM, COMMON_CMD_STOP_ALL, NULL, 0);//先关枪 + angle_protect_lock = 1; + return; + } + } + else + { + //计数器清零 + g_ipaintOffCount = 0; + } +} + static void Custom_ModuleHandler(const Msg_t *pstMsg) { if (NULL == pstMsg) @@ -74,12 +120,22 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) static int iRight_Compensation = 0; switch (pstMsg->m_uiMsgID) { + case CUSTOM_GET_DAEMON_CODE: + { + if (pstMsg->m_uiDataLen >= sizeof(int32_t)) + { + RD_MEMCPY(&g_stIV.SystemError, pstMsg->m_aucData, sizeof(int32_t)); + } + break; + } case CUSTOM_GET_FAULT_CODE: { uint32_t uiFailtCode[2] = {0}; if (pstMsg->m_uiDataLen >= sizeof(uiFailtCode)) { RD_MEMCPY(uiFailtCode, pstMsg->m_aucData, sizeof(uiFailtCode)); + if (uiFailtCode[0] == 1) g_stIV.Left_Motor_Err = uiFailtCode[1]; + else g_stIV.Right_Motor_Err = uiFailtCode[1]; } break; } @@ -210,19 +266,10 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) { MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_POWER_DISABLE, NULL, 0); MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0); + g_RB_State = 0; break; } - // 更新速度旋钮值 - int iSpeedSelection = 3 * (g_stMK32.CH11_RD1 + 1000) / 200; - int iVehicleSpeed = 1; - if (iSpeedSelection > 0) - { - iVehicleSpeed = g_stCV.Speed_m_per_min*iSpeedSelection/30; - } - g_stIV.RobotMoveSpeed = iVehicleSpeed; - MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_GET_VEHICLE_SPEED, (void *)&iVehicleSpeed, sizeof(int)); - // 按键处于默认位置安卓界面可控 if ((fabs(g_stMK32.CH2_LY_V) <= 200) && (fabs(g_stMK32.CH3_LY_H) <= 200) && (fabs(g_stMK32.CH0_RY_H) <= 200) && (fabs(g_stMK32.CH1_RY_V) <= 200) @@ -230,39 +277,25 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) && (g_stMK32.CH6_SC != -1000) ) { g_stIV.IsWorking = 0; + angle_protect_lock = 0; // 遥控器复位认为打开角度锁 + g_ipaintOffCount = 0; } else { g_stIV.IsWorking = 1; } - // 开关喷枪 - if (g_stPV.RunMode == 1) - { - if (g_stMK32.CH6_SC == -1000) - { - int paintstate = 0; - MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &paintstate, sizeof(int)); - } - else - { - int paintstate = 1; - MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &paintstate, sizeof(int)); - } - } - else if(g_stPV.RunMode == 2 || g_stPV.RunMode == 3) + // 更新速度旋钮值 + int iSpeedSelection = 3 * (g_stMK32.CH11_RD1 + 1000) / 200; + int iVehicleSpeed = 1; + if (iSpeedSelection > 0) { - static int CH13_S2_Value = 0; - if (g_stMK32.CH13_S2 != CH13_S2_Value) - { - if(g_stIV.CurrentSpeed != 0) - { - int paintstate = 0; - MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &paintstate, sizeof(int)); - } - } - CH13_S2_Value = g_stMK32.CH13_S2; + iVehicleSpeed = g_stCV.Speed_m_per_min*iSpeedSelection/30; } + g_stIV.RobotMoveSpeed = iVehicleSpeed; + MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_GET_VEHICLE_SPEED, (void *)&iVehicleSpeed, sizeof(int)); + + Move_Halt_AngleError(); int iTargetAngle = 0; // 根据模式判断换道（仅在竖直向左或向右生效） @@ -278,6 +311,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) iTargetAngle = g_stCV.RobotRightAngleValue; MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); } + g_RB_State = 0; break; } else if (g_stMK32.CH4_SA == -1000) @@ -292,6 +326,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) iTargetAngle = g_stCV.RobotLeftAngleValue; MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); } + g_RB_State = 0; break; } else // 重置前进计时器以便下一次换道重新计数 @@ -299,6 +334,40 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3) { MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_RESET_STRAIGHT, NULL, 0); + g_RB_State = 0; + } + } + + if (0 == angle_protect_lock) + { + // 开关喷枪 + if (g_stPV.RunMode == 1) + { + if (g_stMK32.CH6_SC == -1000) + { + g_Paint_State = 0; + MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_Paint_State, sizeof(int)); + } + else + { + g_Paint_State = 1; + MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_Paint_State, sizeof(int)); + } + g_RB_State = 0; + } + 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) + { + g_Paint_State = 0; + MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_Paint_State, sizeof(int)); + } + } + CH13_S2_Value = g_stMK32.CH13_S2; + g_RB_State = 0; } } @@ -311,12 +380,16 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) } else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3) { - BHBF_straight_drive_Cmd stCmd = { - .m_iMode = 1, - .m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration, - .m_iTime = -1 - }; - MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); + if (0 == angle_protect_lock) + { + BHBF_straight_drive_Cmd stCmd = { + .m_iMode = 1, + .m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration, + .m_iTime = -1 + }; + MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); + } + g_RB_State = 1; } break; } @@ -328,12 +401,16 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) } else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3) { - BHBF_straight_drive_Cmd stCmd = { - .m_iMode = -1, - .m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration, - .m_iTime = -1 - }; - MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); + if (0 == angle_protect_lock) + { + BHBF_straight_drive_Cmd stCmd = { + .m_iMode = -1, + .m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration, + .m_iTime = -1 + }; + MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); + } + g_RB_State = 1; } break; } @@ -342,6 +419,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) if (abs(g_stMK32.CH2_LY_V) <= g_stCV.Joy_Sticker_Value_Allowance && abs(g_stMK32.CH3_LY_H) <= g_stCV.Joy_Sticker_Value_Allowance) { MsgCenter_SendTo(MODULE_NAME_MOTOR, COMMON_CMD_STOP_ALL, NULL, 0); + g_RB_State = 0; break; } // 【注意!!!】摇杆行程死区判断结束，摇杆处理都放在这以后，否则行程死区可能不生效 @@ -357,12 +435,16 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) } else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3) { - BHBF_straight_drive_Cmd stCmd = { - .m_iMode = 1, - .m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration, - .m_iTime = -1 - }; - MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); + if (0 == angle_protect_lock) + { + BHBF_straight_drive_Cmd stCmd = { + .m_iMode = 1, + .m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration, + .m_iTime = -1 + }; + MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); + } + g_RB_State = 1; } } else if (abs(angle - (-90)) <= g_stCV.Joy_Sticker_Angle_Allowance) @@ -373,25 +455,32 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) } else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3) { - BHBF_straight_drive_Cmd stCmd = { - .m_iMode = -1, - .m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration, - .m_iTime = -1 - }; - MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); + if (0 == angle_protect_lock) + { + BHBF_straight_drive_Cmd stCmd = { + .m_iMode = -1, + .m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration, + .m_iTime = -1 + }; + MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); + } + g_RB_State = 1; } } else if (abs(angle - 0) <= g_stCV.Joy_Sticker_Angle_Allowance) { MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_TURNRIGHT, NULL, 0); + g_RB_State = 0; } else if (abs(angle - 180) <= g_stCV.Joy_Sticker_Angle_Allowance || abs(angle - (-180)) <= g_stCV.Joy_Sticker_Angle_Allowance) { MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_TURNLEFT, NULL, 0); + g_RB_State = 0; } else { MsgCenter_SendTo(MODULE_NAME_MOTOR, COMMON_CMD_STOP_ALL, NULL, 0); + g_RB_State = 0; } break; }