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

paint_robot_new【调通打标】增加IV和CV

master
Lizongdi 3 days ago
parent
e424bd04ca
commit
b32de2c39b
5 changed files with 84 additions and 24 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. 4
      RBcore/BHBF.c
  2. 56
      RBcore/client_setting.c
  3. 13
      RBcore/drv_interface.c
  4. 2
      RBcore/include/BHBF.h
  5. 33
      project/paint_robot_new/paint_robot_new.c

4
RBcore/BHBF.c
View File

@ -38,6 +38,7 @@
* 全局变量 * * 全局变量 *
*----------------------------------------------*/ *----------------------------------------------*/
static uint32_t g_uiRBcoreModuleID = 0; static uint32_t g_uiRBcoreModuleID = 0;
static CV_struct_define CV = {0};
/*----------------------------------------------* /*----------------------------------------------*
* 模块级变量 * * 模块级变量 *
@ -91,11 +92,12 @@ static void RBcore_ModuleHandler(const Msg_t *pstMsg)
void RBcore_Task(void *argument) void RBcore_Task(void *argument)
{ {
GF_BSP_EEPROM_Init(); GF_BSP_EEPROM_ReadBytes((uint8_t *)&CV, GF_BSP_EEPROM_CV_struct_define_Start_Address, sizeof(CV_struct_define));
while(1) while(1)
{ {
// 处理消息 // 处理消息
MsgCenter_ProcessWait(g_uiRBcoreModuleID, 2); MsgCenter_ProcessWait(g_uiRBcoreModuleID, 2);
MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_GET_CV, (void *)&CV, sizeof(CV));
} }
} }

56
RBcore/client_setting.c
View File

@ -19,7 +19,9 @@
#include "BHBF.h" #include "BHBF.h"
#include "pb.h" #include "pb.h"
#include "pb_decode.h" #include "pb_decode.h"
#include "pb_encode.h"
#include "bsp_PV.pb.h" #include "bsp_PV.pb.h"
#include "bsp_IV.pb.h"
/*----------------------------------------------* /*----------------------------------------------*
* 外部变量说明 * * 外部变量说明 *
@ -40,8 +42,11 @@
/*----------------------------------------------* /*----------------------------------------------*
* 模块级变量 * * 模块级变量 *
*----------------------------------------------*/ *----------------------------------------------*/
static uint32_t g_uiIVModuleID = 0;
static PV_struct_define decoded_PV_variable = { 0 }; static PV_struct_define decoded_PV_variable = { 0 };
static PV_struct_define decoded_PV = { 0 }; static PV_struct_define decoded_PV = { 0 };
static IV_struct_define IV = { 0 };
static char g_IV_buffer[1024] = {0};
/*----------------------------------------------* /*----------------------------------------------*
* 常量定义 * * 常量定义 *
*----------------------------------------------*/ *----------------------------------------------*/
@ -95,12 +100,57 @@ void decode_PV(const char *_pBuffer, uint32_t _iSize)
} }
} }
void Send_IV(void *argument) static void SendIV_ModuleHandler(const Msg_t *pstMsg)
{ {
while(1) if (NULL == pstMsg)
{
return;
}
switch (pstMsg->m_uiMsgID)
{
case CUSTOM_SET_IV:
{ {
if (pstMsg->m_uiDataLen >= sizeof(IV))
{
RD_MEMCPY(&IV, pstMsg->m_aucData, sizeof(IV));
pb_ostream_t IV_o_stream = pb_ostream_from_buffer((pb_byte_t *)&g_IV_buffer[2], sizeof(g_IV_buffer) - 2);
pb_encode(&IV_o_stream, IV_struct_define_fields, &IV);
g_IV_buffer[0] = 0x55;
g_IV_buffer[1] = 0x55;
uint16_t crc = Rd_modbusCRC16((const uint8_t *)&(g_IV_buffer[0]), IV_o_stream.bytes_written + 2);
Rd_Delay(1); g_IV_buffer[IV_o_stream.bytes_written + 2] = (crc>>8) &0xff;
g_IV_buffer[IV_o_stream.bytes_written + 3] = crc & 0xff;;
rd_ComSend(g_ptlpuart, g_IV_buffer, IV_o_stream.bytes_written + 4);
Rd_Delay(1000);
}
break;
}
default:
break;
} }
} }
static void Send_IV(void *argument)
{
while(1)
{
MsgCenter_ProcessWait(g_uiIVModuleID, 2);
}
}
void SendIV_Init(void)
{
g_uiIVModuleID = MsgCenter_Register(MODULE_NAME_SENDIV, SendIV_ModuleHandler);
const osThreadAttr_t Send_PV_attributes = {
.name = MODULE_NAME_SENDIV,
.stack_size = 1024,
.priority = (osPriority_t) osPriorityHigh4,
};
(void)osThreadNew(Send_IV, NULL, &Send_PV_attributes);
}

13
RBcore/drv_interface.c
View File

@ -20,7 +20,7 @@
#include "cmsis_os.h" #include "cmsis_os.h"
#include "FreeRTOS.h" #include "FreeRTOS.h"
#include "bsp_EEPROM.h"
#include "msg_center.h" #include "msg_center.h"
/*----------------------------------------------* /*----------------------------------------------*
* 外部变量说明 * * 外部变量说明 *
@ -249,14 +249,16 @@ extern int check_MK32(char *_pBuffer, uint32_t _iSize);
extern void decode_MK32(const char *buf, uint32_t _iSize); extern void decode_MK32(const char *buf, uint32_t _iSize);
extern int check_PV(char *_pBuffer, uint32_t _iSize); extern int check_PV(char *_pBuffer, uint32_t _iSize);
extern void decode_PV(const char *buf, uint32_t _iSize); extern void decode_PV(const char *buf, uint32_t _iSize);
extern void Send_IV(void *argument);
extern void ground_manger_init(void); extern void ground_manger_init(void);
extern void Custom_Task(void *argument); extern void Custom_Task(void *argument);
void SendIV_Init(void);
void Drv_InterfaceInit(void) void Drv_InterfaceInit(void)
{ {
GF_BSP_EEPROM_Init();
MsgCenter_Init(); MsgCenter_Init();
SendIV_Init();
// ground_manger_init(); // ground_manger_init();
const osThreadAttr_t Custom_attributes = { const osThreadAttr_t Custom_attributes = {
@ -298,11 +300,4 @@ void Drv_InterfaceInit(void)
.priority = (osPriority_t) osPriorityHigh6, .priority = (osPriority_t) osPriorityHigh6,
}; };
(void)osThreadNew(Read_PV, NULL, &Read_PV_attributes); (void)osThreadNew(Read_PV, NULL, &Read_PV_attributes);
//
// const osThreadAttr_t Send_PV_attributes = {
// .name = "Send_PV",
// .stack_size = 1024,
// .priority = (osPriority_t) osPriorityHigh4,
// };
// (void)osThreadNew(Send_IV, NULL, &Send_PV_attributes);
} }

2
RBcore/include/BHBF.h
View File

@ -72,6 +72,7 @@ extern "C"{
#define MODULE_NAME_RBCORE "RBcore" #define MODULE_NAME_RBCORE "RBcore"
#define MODULE_NAME_MOTOR "motor" #define MODULE_NAME_MOTOR "motor"
#define MODULE_NAME_CUSTOM "custom" #define MODULE_NAME_CUSTOM "custom"
#define MODULE_NAME_SENDIV "sendiv"
typedef enum { typedef enum {
LUA_SHOW_INFO, LUA_SHOW_INFO,
@ -92,6 +93,7 @@ typedef enum {
RBCORE_CMD_END = 0x0200, RBCORE_CMD_END = 0x0200,
CUSTOM_GET_PV, // 获取PV CUSTOM_GET_PV, // 获取PV
CUSTOM_GET_CV, // 获取CV
CUSTOM_SET_IV, // 设置IV CUSTOM_SET_IV, // 设置IV
CUSTOM_GET_MK32, // 获取MK32 CUSTOM_GET_MK32, // 获取MK32
CUSTOM_CMD_END = 0x0300, CUSTOM_CMD_END = 0x0300,

33
project/paint_robot_new/paint_robot_new.c
View File

@ -21,6 +21,8 @@
#include "lua_base.h" #include "lua_base.h"
#include "Protobuf/PSource/bsp_PV.pb.h" #include "Protobuf/PSource/bsp_PV.pb.h"
#include "Protobuf/PSource/msp_MK32.pb.h" #include "Protobuf/PSource/msp_MK32.pb.h"
#include "Protobuf/PSource/bsp_CV.pb.h"
#include "Protobuf/PSource/bsp_IV.pb.h"
/*----------------------------------------------* /*----------------------------------------------*
* 外部变量说明 * * 外部变量说明 *
@ -42,8 +44,10 @@
* 模块级变量 * * 模块级变量 *
*----------------------------------------------*/ *----------------------------------------------*/
static uint32_t g_uiCustomModuleID = 0; static uint32_t g_uiCustomModuleID = 0;
SP_MSP_MK32_Button g_stMK32; static SP_MSP_MK32_Button g_stMK32;
PV_struct_define g_stPV; static PV_struct_define g_stPV;
static CV_struct_define g_stCV;
static IV_struct_define g_stIV;
/*----------------------------------------------* /*----------------------------------------------*
* 常量定义 * * 常量定义 *
@ -52,8 +56,6 @@ PV_struct_define g_stPV;
/*----------------------------------------------* /*----------------------------------------------*
* 宏定义 * * 宏定义 *
*----------------------------------------------*/ *----------------------------------------------*/
#define JOY_STICKER_VALUE_ALLOWANCE 600 //摇杆行程死区
#define JOY_STICKER_ANGLE_ALLOWANCE 30 //摇杆角度死区
static void Custom_ModuleHandler(const Msg_t *pstMsg) static void Custom_ModuleHandler(const Msg_t *pstMsg)
{ {
@ -64,6 +66,14 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
switch (pstMsg->m_uiMsgID) switch (pstMsg->m_uiMsgID)
{ {
case CUSTOM_GET_CV:
{
if (pstMsg->m_uiDataLen >= sizeof(g_stCV))
{
RD_MEMCPY(&g_stCV, pstMsg->m_aucData, sizeof(g_stCV));
}
break;
}
case CUSTOM_GET_PV: case CUSTOM_GET_PV:
{ {
if (pstMsg->m_uiDataLen >= sizeof(g_stPV)) if (pstMsg->m_uiDataLen >= sizeof(g_stPV))
@ -79,7 +89,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
RD_MEMCPY(&g_stMK32, pstMsg->m_aucData, sizeof(g_stMK32)); RD_MEMCPY(&g_stMK32, pstMsg->m_aucData, sizeof(g_stMK32));
} }
if (abs(g_stMK32.CH2_LY_V) <= JOY_STICKER_VALUE_ALLOWANCE && abs(g_stMK32.CH3_LY_H) <= 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_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0); MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0);
break; break;
@ -87,19 +97,19 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
int angle = atan2(g_stMK32.CH2_LY_V, g_stMK32.CH3_LY_H) * 180 / M_PI; int angle = atan2(g_stMK32.CH2_LY_V, g_stMK32.CH3_LY_H) * 180 / M_PI;
if (abs(angle - 90) <= JOY_STICKER_ANGLE_ALLOWANCE) if (abs(angle - 90) <= g_stCV.Joy_Sticker_Angle_Allowance)
{ {
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_FORWARD, NULL, 0); MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_FORWARD, NULL, 0);
} }
else if (abs(angle - (-90)) <= JOY_STICKER_ANGLE_ALLOWANCE) else if (abs(angle - (-90)) <= g_stCV.Joy_Sticker_Angle_Allowance)
{ {
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_BACKWARD, NULL, 0); MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_BACKWARD, NULL, 0);
} }
else if (abs(angle - 0) <= 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);
} }
else if (abs(angle - 180) <= JOY_STICKER_ANGLE_ALLOWANCE) else if (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);
} }
@ -129,8 +139,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
lua_print("CH14_LT \t%d\n", g_stMK32.CH14_LT); lua_print("CH14_LT \t%d\n", g_stMK32.CH14_LT);
lua_print("CH15_RT \t%d\n", g_stMK32.CH15_RT); lua_print("CH15_RT \t%d\n", g_stMK32.CH15_RT);
lua_print("PV Info\n"); lua_print("PV Info\n");
lua_print("RunMode = %d\nRobotSpeed = %d\nLaneChangeDistance = %d\nVertical_Calibration = %d\nIV_IsRestart_Notified = %d\nTimeStamp = %ld\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, g_stPV.TimeStamp); g_stPV.RunMode, g_stPV.RobotSpeed, g_stPV.LaneChangeDistance, (int)g_stPV.Vertical_Calibration, g_stPV.IV_IsRestart_Notified, (int)g_stPV.TimeStamp);
break; break;
} }
default: default:
@ -144,6 +154,7 @@ void Custom_Task(void *argument)
while(1) while(1)
{ {
MsgCenter_ProcessWait(g_uiCustomModuleID, 2); MsgCenter_ProcessWait(g_uiCustomModuleID, 2);
MsgCenter_SendTo(MODULE_NAME_SENDIV, CUSTOM_SET_IV, (void *)&g_stIV, sizeof(g_stIV));
} }
} }

Write Preview
Loading…
Cancel
Save