robot/project/paint_robot_new/paint_robot_new.c

/******************************************************************************

                  版权所有 (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;

/*----------------------------------------------*
 * 常量定义                                     *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 宏定义                                       *
 *----------------------------------------------*/

#define IV_SEND_TIME 500 // IV上报周期（单位毫秒）

static void Custom_ModuleHandler(const Msg_t *pstMsg)
{
    if (NULL == pstMsg)
    {
        return;
    }

    switch (pstMsg->m_uiMsgID)
    {
    	case MOTOR_CMD_GET_FAULT_CODE:
		{
			uint32_t uiFailtCode[2] = {0};
			if (pstMsg->m_uiDataLen >= sizeof(uiFailtCode))
			{
				RD_MEMCPY(uiFailtCode, pstMsg->m_aucData, sizeof(uiFailtCode));
			}
			break;
		}
    	case MOTOR_CMD_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_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));
			}

			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 (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));
			}

			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_RBCORE, CMD_STOP_ALL, NULL, 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)
			{
				MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_FORWARD, NULL, 0);
			}
			else if (abs(angle - (-90)) <= g_stCV.Joy_Sticker_Angle_Allowance)
			{
				MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_BACKWARD, NULL, 0);
			}
			else if (abs(angle - 0) <= g_stCV.Joy_Sticker_Angle_Allowance)
			{
				MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_TURNRIGHT, NULL, 0);
			}
			else if (abs(angle - 180) <= g_stCV.Joy_Sticker_Angle_Allowance)
			{
				MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_TURNLEFT, NULL, 0);
			}
			else
			{
				MsgCenter_SendTo(MODULE_NAME_RBCORE, CMD_STOP_ALL, NULL, 0);
			}
			break;
		}
		case LUA_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;
            MsgCenter_SendTo(MODULE_NAME_SENDIV, CUSTOM_SET_IV, (void *)&g_stIV, sizeof(g_stIV));
        }
    }
}