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.
141 lines
4.5 KiB
141 lines
4.5 KiB
/******************************************************************************
|
|
|
|
版权所有 (C), 2018-2099, Radkil
|
|
|
|
******************************************************************************
|
|
文 件 名 : paint_robot_new.c
|
|
版 本 号 : 初稿
|
|
作 者 : radkil
|
|
生成日期 : 2026年7月14日
|
|
最近修改 :
|
|
功能描述 : 新版喷漆主逻辑
|
|
|
|
修改历史 :
|
|
1.日 期 : 2026年7月14日
|
|
作 者 : radkil
|
|
修改内容 : 创建文件
|
|
|
|
******************************************************************************/
|
|
#include "BHBF.h"
|
|
#include "../Protobuf/PSource/msp_MK32.pb.h"
|
|
|
|
/*----------------------------------------------*
|
|
* 外部变量说明 *
|
|
*----------------------------------------------*/
|
|
|
|
/*----------------------------------------------*
|
|
* 外部函数原型说明 *
|
|
*----------------------------------------------*/
|
|
|
|
/*----------------------------------------------*
|
|
* 内部函数原型说明 *
|
|
*----------------------------------------------*/
|
|
|
|
/*----------------------------------------------*
|
|
* 全局变量 *
|
|
*----------------------------------------------*/
|
|
|
|
/*----------------------------------------------*
|
|
* 模块级变量 *
|
|
*----------------------------------------------*/
|
|
static SP_MSP_MK32_Button RB_MK32;
|
|
|
|
/*----------------------------------------------*
|
|
* 常量定义 *
|
|
*----------------------------------------------*/
|
|
|
|
/*----------------------------------------------*
|
|
* 宏定义 *
|
|
*----------------------------------------------*/
|
|
|
|
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++)
|
|
{
|
|
CH[i] = (int32_t)((CH[i] - 992) * 1.388889);
|
|
}
|
|
|
|
RD_MEMCPY(&(RB_MK32.CH0_RY_H), CH, sizeof(CH));
|
|
|
|
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);
|
|
}
|
|
}
|
|
}
|
|
|
|
|