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 "paint_robot_new.h"
#include "BHBF.h"
#include "pb.h"
#include "pb_decode.h"
#include "../Protobuf/PSource/bsp_PV.pb.h"

/*----------------------------------------------*
 * 外部变量说明                                 *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 外部函数原型说明                             *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 内部函数原型说明                             *
 *----------------------------------------------*/
static MSP_MK32_Button RB_MK32;

/*----------------------------------------------*
 * 全局变量                                     *
 *----------------------------------------------*/
PV_struct_define decoded_PV_variable = { 0 };
PV_struct_define decoded_PV = { 0 };

/*----------------------------------------------*
 * 模块级变量                                   *
 *----------------------------------------------*/

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

/*----------------------------------------------*
 * 宏定义                                       *
 *----------------------------------------------*/
    
int check_MK32(char *_pBuffer, uint32_t _iSize)
{
    if (_iSize < 25) return 0;          // 数据不足25字节，不读[24]不误判，等下一包
    if (_pBuffer[0] != 0x0f) return -1;
    if (_pBuffer[24] != 0x00) return -1;
    return 25;
}

void decode_MK32(const char *buf, uint32_t _iSize)
{
	int32_t CH[16];
	int Start_byte = 0;

	CH[0] = ((buf[Start_byte + 1] | buf[Start_byte + 2] << 8) & 0x07FF);
	CH[1] = ((buf[Start_byte + 2] >> 3 | buf[Start_byte + 3] << 5) & 0x07FF);
	CH[2] = ((buf[Start_byte + 3] >> 6 | buf[Start_byte + 4] << 2
			| buf[Start_byte + 5] << 10) & 0x07FF);
	CH[3] = ((buf[Start_byte + 5] >> 1 | buf[Start_byte + 6] << 7) & 0x07FF);
	CH[4] = ((buf[Start_byte + 6] >> 4 | buf[Start_byte + 7] << 4) & 0x07FF);
	CH[5] = ((buf[Start_byte + 7] >> 7 | buf[Start_byte + 8] << 1
			| buf[Start_byte + 9] << 9) & 0x07FF);
	CH[6] = ((buf[Start_byte + 9] >> 2 | buf[Start_byte + 10] << 6) & 0x07FF);
	CH[7] = ((buf[Start_byte + 10] >> 5 | buf[Start_byte + 11] << 3) & 0x07FF);
	CH[8] = ((buf[Start_byte + 12] | buf[Start_byte + 13] << 8) & 0x07FF);
	CH[9] = ((buf[Start_byte + 13] >> 3 | buf[Start_byte + 14] << 5) & 0x07FF);
	CH[10] = ((buf[Start_byte + 14] >> 6 | buf[Start_byte + 15] << 2
			| buf[Start_byte + 16] << 10) & 0x07FF);
	CH[11] = ((buf[Start_byte + 16] >> 1 | buf[Start_byte + 17] << 7) & 0x07FF);
	CH[12] = ((buf[Start_byte + 17] >> 4 | buf[Start_byte + 18] << 4) & 0x07FF);
	CH[13] = ((buf[Start_byte + 18] >> 7 | buf[Start_byte + 19] << 1
			| buf[Start_byte + 20] << 9) & 0x07FF);
	CH[14] = ((buf[Start_byte + 20] >> 2 | buf[Start_byte + 21] << 6) & 0x07FF);
	CH[15] = ((buf[Start_byte + 21] >> 5 | buf[Start_byte + 22] << 3) & 0x07FF);

	// 按键数值转换：1050为中间值，272-1712
	for (int i = 0; i < 16; i++)
	{
		// 通过指针解引用赋值，完美替代原来的 RB_MK32[i + 1]
		CH[i] = (int32_t)((CH[i] - 992) * 1.388889);
	}

	RD_MEMCPY(&RB_MK32, CH, sizeof(CH));

	// buf[23] 才是在线标志字节，对应老工程 Sbus_Data_Count(&buffer[1]) 中的 buf[22]==buffer[23]
	if (buf[23] == 0)
	{
		RB_MK32.IsOnline = 1;
	}
	else
	{
		RB_MK32.IsOnline = 0;
	}
	RB_MK32.RxIndex++;

	int angle;
	angle = atan2(RB_MK32.CH1_RY_V, RB_MK32.CH0_RY_H) * 180 / M_PI;

	if(RB_MK32.CH5_SB == 0)
	{
		// 前进
		if((angle >= 45) && (angle <= 135))
		{
			MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_FORWARD, NULL, 0);
		}
		// 后退
		else if((angle >= -135) && (angle < -45))
		{
			MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_BACKWARD, NULL, 0);
		}
		else
		{
			MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0);
		}
	}
	else
	{
		// 前进
		if((angle >= 45) && (angle <= 135))
		{
			MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_FORWARD, NULL, 0);
		}
		// 后退
		else if((angle >= -135) && (angle < -45))
		{
			MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_BACKWARD, NULL, 0);
		}
		else
		{
			MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0);
		}
	}
}

int check_PV(char *_pBuffer, uint32_t _iSize)
{
    if (_iSize < 4) return -1;
	if (_pBuffer[0] != 0x55) return -1;
    if (_pBuffer[1] != 0x55) return -1;

    uint16_t crc_check = ((_pBuffer[_iSize - 1] << 8) | _pBuffer[_iSize - 2]);
    uint16_t crc_check1 = Rd_modbusCRC16((uint8_t *)_pBuffer, _iSize - 2);
    if (crc_check == crc_check1)
    {
        return _iSize - 2;
    }
    else
    {
        return -1;
    }
}

void decode_PV(const char *_pBuffer, uint32_t _iSize)
{
	if (_pBuffer[2] == 0x01 && _pBuffer[3] == 0x01) //01 01 设置PV
	{
		pb_istream_t i_pv_stream;

		i_pv_stream = pb_istream_from_buffer((const pb_byte_t *)&_pBuffer[4], _iSize - 6);
		pb_decode(&i_pv_stream, PV_struct_define_fields, &decoded_PV_variable);

		if(decoded_PV_variable.TimeStamp>decoded_PV.TimeStamp)
		{
			decoded_PV=decoded_PV_variable;
		}
	}
	else if (*(_pBuffer + 2) == 0x02 && *(_pBuffer + 3) == 0x01) //设置PV
	{

	}
	else if (*(_pBuffer + 2) == 0x03 && *(_pBuffer + 3) == 0x01) //返回IV
	{

	}
	else
	{

	}
//    Motor_CmdData_t tMotor_CmdData_t = {0};
//    tMotor_CmdData_t.m_ucMotorIndex = 1;
//    tMotor_CmdData_t.m_iValue = 10;
//	MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_SET_SPEED, (void *)&tMotor_CmdData_t, sizeof(Motor_CmdData_t));
}

void Send_PV(void *argument)
{
    while(1)
    {

        Rd_Delay(1);
    }
}