diff --git a/RBcore/client_setting.c b/RBcore/client_setting.c index eacd13e..48b8740 100644 --- a/RBcore/client_setting.c +++ b/RBcore/client_setting.c @@ -96,7 +96,7 @@ void decode_PV(const char *_pBuffer, uint32_t _iSize) } } -void Send_PV(void *argument) +void Send_IV(void *argument) { while(1) { diff --git a/RBcore/drv_interface.c b/RBcore/drv_interface.c index 56b52db..6b6d94b 100644 --- a/RBcore/drv_interface.c +++ b/RBcore/drv_interface.c @@ -250,14 +250,23 @@ extern int check_MK32(char *_pBuffer, uint32_t _iSize); extern void decode_MK32(const char *buf, uint32_t _iSize); extern int check_PV(char *_pBuffer, uint32_t _iSize); extern void decode_PV(const char *buf, uint32_t _iSize); -void Send_PV(void *argument); -void ground_manger_init(void); +extern void Send_IV(void *argument); +extern void ground_manger_init(void); +extern void Custom_Task(void *argument); void Drv_InterfaceInit(void) { MsgCenter_Init(); + // ground_manger_init(); + const osThreadAttr_t Custom_attributes = { + .name = MODULE_NAME_CUSTOM, + .stack_size = 1024, + .priority = (osPriority_t) osPriorityRealtime, + }; + (void)osThreadNew(Custom_Task, NULL, &Custom_attributes); + // 创建电机任务 const osThreadAttr_t motor_task_attributes = { .name = MODULE_NAME_MOTOR, @@ -295,5 +304,5 @@ void Drv_InterfaceInit(void) // .stack_size = 1024, // .priority = (osPriority_t) osPriorityHigh4, // }; -// (void)osThreadNew(Send_PV, NULL, &Send_PV_attributes); +// (void)osThreadNew(Send_IV, NULL, &Send_PV_attributes); } diff --git a/RBcore/include/BHBF.h b/RBcore/include/BHBF.h index ea2fa93..3c173f1 100644 --- a/RBcore/include/BHBF.h +++ b/RBcore/include/BHBF.h @@ -74,7 +74,8 @@ extern "C"{ #define MODULE_NAME_CUSTOM "custom" typedef enum { - MOTOR_CMD_SET_SPEED = 0x0001,// 设置速度 + LUA_SHOW_INFO, + MOTOR_CMD_SET_SPEED, // 设置速度 MOTOR_CMD_SET_POSITION, // 设置位置 MOTOR_CMD_STOP, // 停止 MOTOR_CMD_GET_STATUS, // 获取状态 @@ -92,6 +93,8 @@ typedef enum { CUSTOM_GET_PV, // 获取PV CUSTOM_SET_IV, // 设置IV + CUSTOM_GET_MK32, // 获取MK32 + CUSTOM_CMD_END = 0x0300, } Motor_Cmd_e; // 电机命令数据结构 diff --git a/RBcore/include/msg_center.h b/RBcore/include/msg_center.h index 8b81079..c3518f9 100644 --- a/RBcore/include/msg_center.h +++ b/RBcore/include/msg_center.h @@ -38,7 +38,7 @@ extern "C"{ #define MSG_CENTER_MAX_MODULES 16 // 最大模块数 #define MSG_CENTER_MAX_NAME_LEN 16 // 模块名称最大长度 #define MSG_CENTER_QUEUE_SIZE 64 // 消息队列大小 -#define MSG_CENTER_MAX_DATA_SIZE 64 // 消息数据最大大小 +#define MSG_CENTER_MAX_DATA_SIZE 256 // 消息数据最大大小 /*==============================================* * data structures * diff --git a/project/paint_robot_new/paint_robot_new.c b/project/paint_robot_new/paint_robot_new.c index 1cff1e8..aae4cd6 100644 --- a/project/paint_robot_new/paint_robot_new.c +++ b/project/paint_robot_new/paint_robot_new.c @@ -18,7 +18,9 @@ ******************************************************************************/ #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" /*----------------------------------------------* * 外部变量说明 * @@ -40,6 +42,8 @@ * 模块级变量 * *----------------------------------------------*/ static uint32_t g_uiCustomModuleID = 0; +SP_MSP_MK32_Button g_stMK32; +PV_struct_define g_stPV; /*----------------------------------------------* * 常量定义 * @@ -60,7 +64,42 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) { case CUSTOM_GET_PV: { - //log_i("RBCORE_CMD_STOP_ALL"); + if (pstMsg->m_uiDataLen >= sizeof(g_stPV)) + { + RD_MEMCPY(&g_stPV, pstMsg->m_aucData, sizeof(g_stPV)); + } + break; + } + case CUSTOM_GET_MK32: + { + if (pstMsg->m_uiDataLen >= sizeof(g_stMK32)) + { + RD_MEMCPY(&g_stMK32, pstMsg->m_aucData, sizeof(g_stMK32)); + } + 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("RunMode = %d\nRobotSpeed = %d\nVertical_Calibration = %d\nIV_IsRestart_Notified = %d\nTimeStamp = %d\n", + g_stPV.RunMode, g_stPV.RobotSpeed, g_stPV.Vertical_Calibration, g_stPV.IV_IsRestart_Notified, g_stPV.TimeStamp); break; } default: @@ -70,20 +109,10 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) void Custom_Task(void *argument) { + g_uiCustomModuleID = MsgCenter_Register(MODULE_NAME_CUSTOM, Custom_ModuleHandler); while(1) { MsgCenter_ProcessWait(g_uiCustomModuleID, 2); } } -void Custom_Init(void) -{ - g_uiCustomModuleID = MsgCenter_Register(MODULE_NAME_CUSTOM, Custom_ModuleHandler); - - const osThreadAttr_t Custom_attributes = { - .name = MODULE_NAME_CUSTOM, - .stack_size = 1024, - .priority = (osPriority_t) osPriorityRealtime, - }; - (void)osThreadNew(Custom_Task, NULL, &Custom_attributes); -} diff --git a/project/paint_robot_new/paint_robot_new_MK32.c b/project/paint_robot_new/paint_robot_new_MK32.c index 40aed70..1d47d6f 100644 --- a/project/paint_robot_new/paint_robot_new_MK32.c +++ b/project/paint_robot_new/paint_robot_new_MK32.c @@ -100,6 +100,8 @@ void decode_MK32(const char *buf, uint32_t _iSize) } RB_MK32.RxIndex++; + MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_GET_MK32, (void *)&RB_MK32, sizeof(RB_MK32)); + int angle; angle = atan2(RB_MK32.CH1_RY_V, RB_MK32.CH0_RY_H) * 180 / M_PI; diff --git a/project/paint_robot_new/paint_robot_new_motors.c b/project/paint_robot_new/paint_robot_new_motors.c index 11e209f..a2f6217 100644 --- a/project/paint_robot_new/paint_robot_new_motors.c +++ b/project/paint_robot_new/paint_robot_new_motors.c @@ -62,10 +62,14 @@ static void Motor_ModuleHandler(const Msg_t *pstMsg) { case MOTOR_CMD_SET_SPEED: { - int iSpeed = (int)pstMsg->m_aucData; - log_i("Set Speed %d", iSpeed); -// g_aiMotorSpeed[0] = iSpeed; -// g_aiMotorSpeed[1] = iSpeed; + int iSpeed = 0; + if (pstMsg->m_uiDataLen >= sizeof(int)) + { + RD_MEMCPY(&iSpeed, pstMsg->m_aucData, sizeof(int)); + } + //log_i("Set Speed %d", iSpeed); + g_aiMotorSpeed[0] = iSpeed; + g_aiMotorSpeed[1] = iSpeed; break; } case MOTOR_CMD_STOP: @@ -157,7 +161,7 @@ static void decode_LeiSaiMotor(const char *_pBuffer, uint32_t _iSize) MotorMgr_ParseResponse(ucMotorID, (const uint8_t *)&_pBuffer[4], 8); } -static void MotorInit(void) +void MotorTask(void *argument) { // 初始化FDCAN1,使用雷赛电机的回调 TCANUserData *ptCANUserData = CAN_userdata_init(&hfdcan1, 32, 0); @@ -186,11 +190,6 @@ static void MotorInit(void) g_apstMotors[1] = MotorMgr_Create(&stRightMotorConfig, g_ptCAN1, LeiSai_GetProtocol(), NULL); g_uiMotorModuleID = MsgCenter_Register(MODULE_NAME_MOTOR, Motor_ModuleHandler); -} - -void MotorTask(void *argument) -{ - MotorInit(); MotorMgr_ResetAll(g_apstMotors[0]); Rd_Delay(500);