You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
550 lines
17 KiB
550 lines
17 KiB
/******************************************************************************
|
|
|
|
版权所有 (C), 2018-2099, Radkil
|
|
|
|
******************************************************************************
|
|
文 件 名 : paint_robot_new.c
|
|
版 本 号 : 初稿
|
|
作 者 : radkil
|
|
生成日期 : 2026年7月14日
|
|
最近修改 :
|
|
功能描述 : 新版喷漆主逻辑
|
|
|
|
修改历史 :
|
|
1.日 期 : 2026年7月14日
|
|
作 者 : radkil
|
|
修改内容 : 创建文件
|
|
|
|
******************************************************************************/
|
|
#include "BHBF.h"
|
|
#include "msg_center.h"
|
|
#include "lua_base.h"
|
|
#include "Protobuf/PSource/bsp_PV.pb.h"
|
|
#include "Protobuf/PSource/msp_MK32.pb.h"
|
|
#include "Protobuf/PSource/bsp_CV.pb.h"
|
|
#include "Protobuf/PSource/bsp_IV.pb.h"
|
|
|
|
/*----------------------------------------------*
|
|
* 外部变量说明 *
|
|
*----------------------------------------------*/
|
|
|
|
/*----------------------------------------------*
|
|
* 外部函数原型说明 *
|
|
*----------------------------------------------*/
|
|
|
|
/*----------------------------------------------*
|
|
* 内部函数原型说明 *
|
|
*----------------------------------------------*/
|
|
|
|
/*----------------------------------------------*
|
|
* 全局变量 *
|
|
*----------------------------------------------*/
|
|
|
|
/*----------------------------------------------*
|
|
* 模块级变量 *
|
|
*----------------------------------------------*/
|
|
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 = -1; // 喷枪状态，0表示打开，1表示关闭
|
|
static int angle_protect_lock = 0; // 为1表示角度异常，触发停车逻辑Move_Halt_AngleError
|
|
static int g_ipaintOffCount = 0; // Move_Halt_AngleError中的计时，即角度偏移超过多少时间触发停枪
|
|
static int Is_All_Button_Reset = 0; // 1表示按钮正常工作，0表示需要复位
|
|
|
|
/*----------------------------------------------*
|
|
* 常量定义 *
|
|
*----------------------------------------------*/
|
|
|
|
/*----------------------------------------------*
|
|
* 宏定义 *
|
|
*----------------------------------------------*/
|
|
|
|
#define IV_SEND_TIME 500 // IV上报周期（单位毫秒）
|
|
|
|
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(Rd_GetTime() - g_ipaintOffCount > 1000)
|
|
{
|
|
MsgCenter_SendTo(MODULE_NAME_CUSTOM, COMMON_CMD_STOP_ALL, NULL, 0);//先关枪
|
|
angle_protect_lock = 1;
|
|
return;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
//计数器清零
|
|
g_ipaintOffCount = Rd_GetTime();
|
|
}
|
|
}
|
|
|
|
static void Custom_ModuleHandler(const Msg_t *pstMsg)
|
|
{
|
|
if (NULL == pstMsg)
|
|
{
|
|
return;
|
|
}
|
|
|
|
static int iLeft_Compensation = 0;
|
|
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;
|
|
}
|
|
case CUSTOM_GET_SPEED:
|
|
{
|
|
int32_t iCurrentSpeed[2] = {0};
|
|
if (pstMsg->m_uiDataLen >= sizeof(iCurrentSpeed))
|
|
{
|
|
RD_MEMCPY(iCurrentSpeed, pstMsg->m_aucData, sizeof(iCurrentSpeed));
|
|
if (1 == iCurrentSpeed[0])
|
|
{
|
|
g_stIV.CurrentSpeed = iCurrentSpeed[1];
|
|
log_d("speed = %d", g_stIV.CurrentSpeed);
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
case CUSTOM_GET_TL720D_ROLL:
|
|
{
|
|
if (pstMsg->m_uiDataLen >= sizeof(int32_t))
|
|
{
|
|
RD_MEMCPY(&g_stIV.CurrentAngle, pstMsg->m_aucData, sizeof(int32_t));
|
|
}
|
|
break;
|
|
}
|
|
case CUSTOM_GET_PV:
|
|
{
|
|
if (pstMsg->m_uiDataLen >= sizeof(g_stPV))
|
|
{
|
|
RD_MEMCPY(&g_stPV, pstMsg->m_aucData, sizeof(g_stPV));
|
|
}
|
|
break;
|
|
}
|
|
case CUSTOM_CMD_STRAIGHT_DRIVE:
|
|
{
|
|
BHBF_straight_drive_Cmd stCmd = {0};
|
|
if (pstMsg->m_uiDataLen >= sizeof(stCmd))
|
|
{
|
|
RD_MEMCPY(&stCmd, pstMsg->m_aucData, sizeof(stCmd));
|
|
int iTargetAngle = g_stCV.RobotUpAngleValue;
|
|
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
|
|
}
|
|
break;
|
|
}
|
|
case CUSTOM_CMD_TURN_ANGLE:
|
|
{
|
|
int iTargetAngle = 0;
|
|
if (pstMsg->m_uiDataLen >= sizeof(int))
|
|
{
|
|
RD_MEMCPY(&iTargetAngle, pstMsg->m_aucData, sizeof(int));
|
|
if (iTargetAngle == g_stCV.RobotUpAngleValue)
|
|
{
|
|
log_d("change line success");
|
|
}
|
|
else if (iTargetAngle == g_stCV.RobotLeftAngleValue)
|
|
{
|
|
if (g_stPV.RunMode == 2)//竖直从右往左作业 下端 向左换道 最终头朝上
|
|
{
|
|
BHBF_straight_drive_Cmd stCmd = {
|
|
.m_iMode = 1,
|
|
.m_iAngle = g_stCV.RobotLeftAngleValue + iLeft_Compensation,
|
|
.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin()
|
|
};
|
|
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
|
|
}
|
|
else if (g_stPV.RunMode == 3)//竖直从左往右作业 上端 向右换道 最终头朝上
|
|
{
|
|
BHBF_straight_drive_Cmd stCmd = {
|
|
.m_iMode = -1,
|
|
.m_iAngle = g_stCV.RobotLeftAngleValue - iRight_Compensation,
|
|
.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin()
|
|
};
|
|
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
|
|
}
|
|
}
|
|
else if (iTargetAngle == g_stCV.RobotRightAngleValue)
|
|
{
|
|
if (g_stPV.RunMode == 2)//竖直从右往左作业 上端 向左换道 最终头朝上
|
|
{
|
|
BHBF_straight_drive_Cmd stCmd = {
|
|
.m_iMode = -1,
|
|
.m_iAngle = g_stCV.RobotRightAngleValue + iLeft_Compensation,
|
|
.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin()
|
|
};
|
|
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
|
|
}
|
|
else if (g_stPV.RunMode == 3)//竖直从左往右作业 下端 向右换道 最终头朝上
|
|
{
|
|
BHBF_straight_drive_Cmd stCmd = {
|
|
.m_iMode = 1,
|
|
.m_iAngle = g_stCV.RobotRightAngleValue - iRight_Compensation,
|
|
.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin()
|
|
};
|
|
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
|
|
}
|
|
}
|
|
else if (iTargetAngle == g_stCV.RobotDownAngleValue)
|
|
{
|
|
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
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:
|
|
{
|
|
int paintstate = -1;
|
|
if (pstMsg->m_uiDataLen >= sizeof(paintstate))
|
|
{
|
|
RD_MEMCPY(&paintstate, pstMsg->m_aucData, sizeof(paintstate));
|
|
HAL_GPIO_WritePin(OUT_1_GPIO_Port, OUT_1_Pin, paintstate);
|
|
}
|
|
break;
|
|
}
|
|
case CUSTOM_GET_MK32:
|
|
{
|
|
if (pstMsg->m_uiDataLen >= sizeof(g_stMK32))
|
|
{
|
|
RD_MEMCPY(&g_stMK32, pstMsg->m_aucData, sizeof(g_stMK32));
|
|
}
|
|
|
|
if (Is_All_Button_Reset == 0)
|
|
{
|
|
if (g_stMK32.CH4_SA == 0 && g_stMK32.CH5_SB == 0 && g_stMK32.CH6_SC == 0
|
|
&& g_stMK32.CH7_SD == 0 && g_stMK32.IsOnline == 1)
|
|
{
|
|
Is_All_Button_Reset = 1;
|
|
}
|
|
}
|
|
else if (Is_All_Button_Reset == 1)
|
|
{
|
|
MsgCenter_SendTo(MODULE_NAME_DAEMON, DAEMON_SET_BUTTON_RESET, NULL, 0);
|
|
}
|
|
|
|
// 急停或者遥控器失联
|
|
if ((g_stMK32.CH8_SE == -1000 && g_stMK32.CH9_SF == -1000)
|
|
|| g_stMK32.IsOnline == 0 || g_stIV.Left_Motor_Err != 0 || g_stIV.Right_Motor_Err != 0)
|
|
{
|
|
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;
|
|
Is_All_Button_Reset = 0;
|
|
break;
|
|
}
|
|
|
|
// 按键处于默认位置安卓界面可控
|
|
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)
|
|
&& (g_stMK32.CH4_SA ==0) && (g_stMK32.CH5_SB == 0)
|
|
&& (g_stMK32.CH6_SC != -1000) )
|
|
{
|
|
g_stIV.IsWorking = 0;
|
|
angle_protect_lock = 0; // 遥控器复位认为打开角度锁
|
|
g_ipaintOffCount = 0;
|
|
}
|
|
else
|
|
{
|
|
g_stIV.IsWorking = 1;
|
|
}
|
|
|
|
// 更新速度旋钮值
|
|
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));
|
|
|
|
Move_Halt_AngleError();
|
|
|
|
int iTargetAngle = 0;
|
|
// 根据模式判断换道（仅在竖直向左或向右生效）
|
|
if (g_stMK32.CH4_SA == 1000)
|
|
{
|
|
if (g_stPV.RunMode == 2)//竖直从右往左作业 下端 向左换道 最终头朝上
|
|
{
|
|
iTargetAngle = g_stCV.RobotLeftAngleValue;
|
|
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
|
|
}
|
|
else if (g_stPV.RunMode == 3)//竖直从左往右作业 下端 向右换道 最终头朝上
|
|
{
|
|
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)
|
|
{
|
|
if (g_stPV.RunMode == 2)//竖直从右往左作业 上端 向左换道 最终头朝上
|
|
{
|
|
iTargetAngle = g_stCV.RobotRightAngleValue;
|
|
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
|
|
}
|
|
else if (g_stPV.RunMode == 3)//竖直从左往右作业 上端 向右换道 最终头朝上
|
|
{
|
|
iTargetAngle = g_stCV.RobotLeftAngleValue;
|
|
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
|
|
}
|
|
g_RB_State = 0;
|
|
break;
|
|
}
|
|
else // 重置前进计时器以便下一次换道重新计数
|
|
{
|
|
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;
|
|
}
|
|
}
|
|
|
|
// 自动巡航
|
|
if (g_stMK32.CH5_SB == -1000)
|
|
{
|
|
if (g_stPV.RunMode == 1)
|
|
{
|
|
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_FORWARD, NULL, 0);
|
|
}
|
|
else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
|
|
{
|
|
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;
|
|
}
|
|
else if(g_stMK32.CH5_SB == 1000)
|
|
{
|
|
if (g_stPV.RunMode == 1)
|
|
{
|
|
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_BACKWARD, NULL, 0);
|
|
}
|
|
else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
|
|
{
|
|
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;
|
|
}
|
|
|
|
// 【注意!!!】摇杆行程死区判断开始，除摇杆以外按键在上边都处理完，不然走不下去
|
|
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;
|
|
}
|
|
// 【注意!!!】摇杆行程死区判断结束，摇杆处理都放在这以后，否则行程死区可能不生效
|
|
|
|
// 摇杆角度死区判断
|
|
int angle = atan2(g_stMK32.CH2_LY_V, g_stMK32.CH3_LY_H) * 180 / M_PI;
|
|
|
|
if (abs(angle - 90) <= g_stCV.Joy_Sticker_Angle_Allowance)
|
|
{
|
|
if (g_stPV.RunMode == 1)
|
|
{
|
|
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_FORWARD, NULL, 0);
|
|
}
|
|
else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
|
|
{
|
|
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)
|
|
{
|
|
if (g_stPV.RunMode == 1)
|
|
{
|
|
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_BACKWARD, NULL, 0);
|
|
}
|
|
else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
|
|
{
|
|
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;
|
|
}
|
|
case COMMON_CMD_SHOW_INFO:
|
|
{
|
|
lua_print("MK32 Info\nRxIndex:%d\tIsOnline:%d\n", g_stMK32.RxIndex, g_stMK32.IsOnline);
|
|
lua_print("CH0_RY_H\t%d\n", g_stMK32.CH0_RY_H);
|
|
lua_print("CH1_RY_V\t%d\n", g_stMK32.CH1_RY_V);
|
|
lua_print("CH2_LY_V\t%d\n", g_stMK32.CH2_LY_V);
|
|
lua_print("CH3_LY_H\t%d\n", g_stMK32.CH3_LY_H);
|
|
lua_print("CH4_SA \t%d\n", g_stMK32.CH4_SA);
|
|
lua_print("CH5_SB \t%d\n", g_stMK32.CH5_SB);
|
|
lua_print("CH6_SC \t%d\n", g_stMK32.CH6_SC);
|
|
lua_print("CH7_SD \t%d\n", g_stMK32.CH7_SD);
|
|
lua_print("CH8_SE \t%d\n", g_stMK32.CH8_SE);
|
|
lua_print("CH9_SF \t%d\n", g_stMK32.CH9_SF);
|
|
lua_print("CH10_LD1\t%d\n", g_stMK32.CH10_LD1);
|
|
lua_print("CH11_RD1\t%d\n", g_stMK32.CH11_RD1);
|
|
lua_print("CH12_S1 \t%d\n", g_stMK32.CH12_S1);
|
|
lua_print("CH13_S2 \t%d\n", g_stMK32.CH13_S2);
|
|
lua_print("CH14_LT \t%d\n", g_stMK32.CH14_LT);
|
|
lua_print("CH15_RT \t%d\n", g_stMK32.CH15_RT);
|
|
lua_print("PV Info\n");
|
|
lua_print("{%d, %d, %d, %d, %d, %d}\n",
|
|
g_stPV.RunMode, g_stPV.RobotSpeed, g_stPV.LaneChangeDistance, (int)g_stPV.Vertical_Calibration, g_stPV.IV_IsRestart_Notified, (int)g_stPV.TimeStamp);
|
|
break;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
void Custom_Task(void *argument)
|
|
{
|
|
g_uiCustomModuleID = MsgCenter_Register(MODULE_NAME_CUSTOM, Custom_ModuleHandler);
|
|
uint32_t uiLastSendTick = Rd_GetTime();
|
|
while(1)
|
|
{
|
|
MsgCenter_ProcessWait(g_uiCustomModuleID, 2);
|
|
|
|
uint32_t uiNowTick = Rd_GetTime();
|
|
if ((int32_t)(uiNowTick - uiLastSendTick) >= IV_SEND_TIME)
|
|
{
|
|
uiLastSendTick = uiNowTick;
|
|
g_stIV.SBUS_State = g_stMK32.IsOnline;
|
|
MsgCenter_SendTo(MODULE_NAME_SENDIV, SENDIV_SET_IV, (void *)&g_stIV, sizeof(g_stIV));
|
|
}
|
|
}
|
|
}
|
|
|
|
|