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

增加超时停车逻辑

paint_robot_new-v1.5
Lizongdi 2 weeks ago
parent
64ea45148c
commit
c6efcef7d9
1 changed files with 148 additions and 59 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 207
      project/paint_robot_new/paint_robot_new.c

207
project/paint_robot_new/paint_robot_new.c
View File

@ -47,6 +47,10 @@ static uint32_t g_uiCustomModuleID = 0;
static SP_MSP_MK32_Button g_stMK32; static SP_MSP_MK32_Button g_stMK32;
static PV_struct_define g_stPV; static PV_struct_define g_stPV;
static IV_struct_define g_stIV; 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); 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) static void Custom_ModuleHandler(const Msg_t *pstMsg)
{ {
if (NULL == pstMsg) if (NULL == pstMsg)
@ -74,12 +120,22 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
static int iRight_Compensation = 0; static int iRight_Compensation = 0;
switch (pstMsg->m_uiMsgID) 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: case CUSTOM_GET_FAULT_CODE:
{ {
uint32_t uiFailtCode[2] = {0}; uint32_t uiFailtCode[2] = {0};
if (pstMsg->m_uiDataLen >= sizeof(uiFailtCode)) if (pstMsg->m_uiDataLen >= sizeof(uiFailtCode))
{ {
RD_MEMCPY(uiFailtCode, pstMsg->m_aucData, 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; 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_MOTOR, MOTOR_POWER_DISABLE, NULL, 0);
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0); MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0);
g_RB_State = 0;
break; 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) 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) && (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_stMK32.CH6_SC != -1000) )
{ {
g_stIV.IsWorking = 0; g_stIV.IsWorking = 0;
angle_protect_lock = 0; // 遥控器复位认为打开角度锁
g_ipaintOffCount = 0;
} }
else else
{ {
g_stIV.IsWorking = 1; g_stIV.IsWorking = 1;
} }
// 开关喷枪 // 更新速度旋钮值
if (g_stPV.RunMode == 1) int iSpeedSelection = 3 * (g_stMK32.CH11_RD1 + 1000) / 200;
{ int iVehicleSpeed = 1;
if (g_stMK32.CH6_SC == -1000) if (iSpeedSelection > 0)
{
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)
{ {
static int CH13_S2_Value = 0; iVehicleSpeed = g_stCV.Speed_m_per_min*iSpeedSelection/30;
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;
} }
g_stIV.RobotMoveSpeed = iVehicleSpeed;
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_GET_VEHICLE_SPEED, (void *)&iVehicleSpeed, sizeof(int));
Move_Halt_AngleError();
int iTargetAngle = 0; int iTargetAngle = 0;
// 根据模式判断换道（仅在竖直向左或向右生效） // 根据模式判断换道（仅在竖直向左或向右生效）
@ -278,6 +311,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
iTargetAngle = g_stCV.RobotRightAngleValue; iTargetAngle = g_stCV.RobotRightAngleValue;
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
} }
g_RB_State = 0;
break; break;
} }
else if (g_stMK32.CH4_SA == -1000) else if (g_stMK32.CH4_SA == -1000)
@ -292,6 +326,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
iTargetAngle = g_stCV.RobotLeftAngleValue; iTargetAngle = g_stCV.RobotLeftAngleValue;
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
} }
g_RB_State = 0;
break; break;
} }
else // 重置前进计时器以便下一次换道重新计数 else // 重置前进计时器以便下一次换道重新计数
@ -299,6 +334,40 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3) if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
{ {
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_RESET_STRAIGHT, NULL, 0); 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) else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
{ {
BHBF_straight_drive_Cmd stCmd = { if (0 == angle_protect_lock)
.m_iMode = 1, {
.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration, BHBF_straight_drive_Cmd stCmd = {
.m_iTime = -1 .m_iMode = 1,
}; .m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); .m_iTime = -1
};
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
}
g_RB_State = 1;
} }
break; break;
} }
@ -328,12 +401,16 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
} }
else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3) else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
{ {
BHBF_straight_drive_Cmd stCmd = { if (0 == angle_protect_lock)
.m_iMode = -1, {
.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration, BHBF_straight_drive_Cmd stCmd = {
.m_iTime = -1 .m_iMode = -1,
}; .m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); .m_iTime = -1
};
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
}
g_RB_State = 1;
} }
break; 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) 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); MsgCenter_SendTo(MODULE_NAME_MOTOR, COMMON_CMD_STOP_ALL, NULL, 0);
g_RB_State = 0;
break; break;
} }
// 【注意!!!】摇杆行程死区判断结束，摇杆处理都放在这以后，否则行程死区可能不生效 // 【注意!!!】摇杆行程死区判断结束，摇杆处理都放在这以后，否则行程死区可能不生效
@ -357,12 +435,16 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
} }
else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3) else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
{ {
BHBF_straight_drive_Cmd stCmd = { if (0 == angle_protect_lock)
.m_iMode = 1, {
.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration, BHBF_straight_drive_Cmd stCmd = {
.m_iTime = -1 .m_iMode = 1,
}; .m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); .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) 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) else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
{ {
BHBF_straight_drive_Cmd stCmd = { if (0 == angle_protect_lock)
.m_iMode = -1, {
.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration, BHBF_straight_drive_Cmd stCmd = {
.m_iTime = -1 .m_iMode = -1,
}; .m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); .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) else if (abs(angle - 0) <= g_stCV.Joy_Sticker_Angle_Allowance)
{ {
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_TURNRIGHT, NULL, 0); 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) 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); MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_TURNLEFT, NULL, 0);
g_RB_State = 0;
} }
else else
{ {
MsgCenter_SendTo(MODULE_NAME_MOTOR, COMMON_CMD_STOP_ALL, NULL, 0); MsgCenter_SendTo(MODULE_NAME_MOTOR, COMMON_CMD_STOP_ALL, NULL, 0);
g_RB_State = 0;
} }
break; break;
} }

Write Preview
Loading…
Cancel
Save