diff --git a/RBcore/client_setting.c b/RBcore/client_setting.c index 9a236dc..eacd13e 100644 --- a/RBcore/client_setting.c +++ b/RBcore/client_setting.c @@ -79,6 +79,7 @@ void decode_PV(const char *_pBuffer, uint32_t _iSize) if(decoded_PV_variable.TimeStamp>decoded_PV.TimeStamp) { decoded_PV=decoded_PV_variable; + MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_GET_PV, (void *)&decoded_PV, sizeof(decoded_PV)); } } else if (*(_pBuffer + 2) == 0x02 && *(_pBuffer + 3) == 0x01) //设置PV @@ -93,10 +94,6 @@ void decode_PV(const char *_pBuffer, uint32_t _iSize) { } -// 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) diff --git a/RBcore/drv_interface.c b/RBcore/drv_interface.c index 2cf970b..058b1f6 100644 --- a/RBcore/drv_interface.c +++ b/RBcore/drv_interface.c @@ -257,7 +257,7 @@ void Drv_InterfaceInit(void) { MsgCenter_Init(); // ground_manger_init(); -// MotorInit(); + MotorInit(); // RBcore_Init(); TUartUserData *ptSbus = UART_userdata_init(5, -1, 512); diff --git a/RBcore/include/BHBF.h b/RBcore/include/BHBF.h index 98e3a49..ea2fa93 100644 --- a/RBcore/include/BHBF.h +++ b/RBcore/include/BHBF.h @@ -71,9 +71,10 @@ extern "C"{ *----------------------------------------------*/ #define MODULE_NAME_RBCORE "RBcore" #define MODULE_NAME_MOTOR "motor" +#define MODULE_NAME_CUSTOM "custom" typedef enum { - MOTOR_CMD_SET_SPEED = 0x0001, // 设置速度 + MOTOR_CMD_SET_SPEED = 0x0001,// 设置速度 MOTOR_CMD_SET_POSITION, // 设置位置 MOTOR_CMD_STOP, // 停止 MOTOR_CMD_GET_STATUS, // 获取状态 @@ -87,6 +88,10 @@ typedef enum { RBCORE_CMD_MANUAL_BACKWARD, // 机器人手动后退 RBCORE_CMD_MANUAL_TURNLEFT, // 机器人手动左转 RBCORE_CMD_MANUAL_TURNRIGHT, // 机器人手动右转 + RBCORE_CMD_END = 0x0200, + + CUSTOM_GET_PV, // 获取PV + CUSTOM_SET_IV, // 设置IV } Motor_Cmd_e; // 电机命令数据结构 diff --git a/motor/include/msp_motor_leisai.h b/motor/include/msp_motor_leisai.h index f1c87a1..09f5bcc 100644 --- a/motor/include/msp_motor_leisai.h +++ b/motor/include/msp_motor_leisai.h @@ -66,7 +66,7 @@ extern "C"{ #define LEISAI_CTRL_SHUTDOWN 0x06 #define LEISAI_CTRL_SWITCH_ON 0x07 #define LEISAI_CTRL_ENABLE 0x0F -#define LEISAI_CTRL_START_MOVE 0x5F +#define LEISAI_CTRL_START_MOVE 0x1F #define LEISAI_CTRL_RELATIVE_MOVE 0x4F /*==============================================* diff --git a/motor/motor_example.c b/motor/motor_example.c index 87125c2..26fe5eb 100644 --- a/motor/motor_example.c +++ b/motor/motor_example.c @@ -184,37 +184,40 @@ void MotorExample_MultiProtocol(void) void MotorTask(void *argument) { - // 等待系统稳定 - Rd_Delay(1000); - - // 复位并激活所有电机 - for (int i = 0; i < MOTOR_INSTANCE_MAX; i++) - { - if (g_apstMotors[i] != NULL) - { - MotorMgr_ResetAll(g_apstMotors[i]); - Rd_Delay(500); - MotorMgr_ActivateAll(g_apstMotors[i]); - Rd_Delay(500); - MotorMgr_SetHome(g_apstMotors[i]); - Rd_Delay(100); - MotorMgr_SpeedModeInit(g_apstMotors[i]); - } - } + MotorMgr_ResetAll(g_apstMotors[0]); + Rd_Delay(500); + MotorMgr_ResetAll(g_apstMotors[1]); + Rd_Delay(500); + + int iCount = 6; + while(iCount--) + { + MotorMgr_ActivateAll(g_apstMotors[0]); + Rd_Delay(500); + MotorMgr_ActivateAll(g_apstMotors[1]); + Rd_Delay(500); + } + + MotorMgr_SetHome(g_apstMotors[0]); + MotorMgr_SetHome(g_apstMotors[1]); + MotorMgr_SpeedModeInit(g_apstMotors[0]); + MotorMgr_SpeedModeInit(g_apstMotors[1]); while(1) { // 处理消息 - MsgCenter_ProcessWait(g_uiMotorModuleID, 2); + //MsgCenter_ProcessWait(g_uiMotorModuleID, 2); for (int i = 0; i < MOTOR_INSTANCE_MAX; i++) { if (g_apstMotors[i] != NULL) { // 读取位置、速度、故障 - MotorMgr_RequestPosition(g_apstMotors[i]); - MotorMgr_RequestFault(g_apstMotors[i]); - MotorMgr_RequestVelocity(g_apstMotors[i]); +// MotorMgr_RequestPosition(g_apstMotors[i]); +// MotorMgr_RequestFault(g_apstMotors[i]); +// MotorMgr_RequestVelocity(g_apstMotors[i]); + MotorMgr_SetTargetSpeed(g_apstMotors[i], 50000); + Rd_Delay(2); } } @@ -346,7 +349,7 @@ static void decode_LeiSaiMotor(const char *_pBuffer, uint32_t _iSize) void MotorInit(void) { // 初始化FDCAN1，使用雷赛电机的回调 - TCANUserData *ptCANUserData = CAN_userdata_init(&hfdcan1, 8, 0); + TCANUserData *ptCANUserData = CAN_userdata_init(&hfdcan1, 32, 0); g_ptCAN1 = rd_ComCreate(check_LeiSaiMotor, decode_LeiSaiMotor, FDCAN1_Send, CONFIG_UART_BUFFER_SIZE, ptCANUserData); CAN_IT_init(g_ptCAN1); @@ -377,7 +380,7 @@ void MotorInit(void) const osThreadAttr_t motor_task_attributes = { .name = MODULE_NAME_MOTOR, .stack_size = 1024, - .priority = (osPriority_t) osPriorityHigh5, + .priority = (osPriority_t) osPriorityRealtime, }; (void)osThreadNew(MotorTask, NULL, &motor_task_attributes); } diff --git a/motor/msp_motor_leisai.c b/motor/msp_motor_leisai.c index 8c4b296..c3cb905 100644 --- a/motor/msp_motor_leisai.c +++ b/motor/msp_motor_leisai.c @@ -18,6 +18,7 @@ ******************************************************************************/ #include "msp_motor_leisai.h" +#include "rd_time.h" /*----------------------------------------------* * 外部变量说明 * @@ -183,10 +184,7 @@ static int LeiSai_ActivateAll(Motor_Instance_t *pstMotor) return RD_NULL; } - uint8_t aucData1[2] = {0x01, 0x01}; - rd_ComIDSend(pstMotor->m_ptCAN, 0x000, (char *)aucData1, 2); - - uint8_t aucData2[2] = {0x01, 0x02}; + uint8_t aucData2[2] = {0x01, pstMotor->m_stConfig.m_ucMotorID}; return rd_ComIDSend(pstMotor->m_ptCAN, 0x000, (char *)aucData2, 2); } @@ -202,11 +200,17 @@ static int LeiSai_SetHome(Motor_Instance_t *pstMotor) } LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_1B, LEISAI_OD_MODE_OF_OPERATION, 0x00, LEISAI_MODE_HOMING); + Rd_Delay(50); LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_1B, LEISAI_OD_HOMING_METHOD, 0x00, 0x23); + Rd_Delay(50); LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_SHUTDOWN); + Rd_Delay(50); LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_SWITCH_ON); + Rd_Delay(50); LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_ENABLE); + Rd_Delay(50); LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_START_MOVE); + Rd_Delay(50); return RD_SUCCESS; } @@ -293,10 +297,15 @@ static int LeiSai_SpeedModeInit(Motor_Instance_t *pstMotor) } LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_SHUTDOWN); + Rd_Delay(10); LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_SWITCH_ON); + Rd_Delay(10); LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_ENABLE); + Rd_Delay(10); LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_1B, LEISAI_OD_MODE_OF_OPERATION, 0x00, LEISAI_MODE_PROFILE_VEL); + Rd_Delay(10); LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_4B, LEISAI_OD_TARGET_VELOCITY, 0x00, 0); + Rd_Delay(10); return RD_SUCCESS; } diff --git a/project/paint_robot_new/paint_robot_new.c b/project/paint_robot_new/paint_robot_new.c index 2b3d0ac..0e4a0d4 100644 --- a/project/paint_robot_new/paint_robot_new.c +++ b/project/paint_robot_new/paint_robot_new.c @@ -17,7 +17,9 @@ ******************************************************************************/ #include "BHBF.h" +#include "msg_center.h" #include "../Protobuf/PSource/msp_MK32.pb.h" +#include "../Protobuf/PSource/bsp_PV.pb.h" /*----------------------------------------------* * 外部变量说明 * @@ -39,6 +41,7 @@ * 模块级变量 * *----------------------------------------------*/ static SP_MSP_MK32_Button RB_MK32; +static uint32_t g_uiCustomModuleID = 0; /*----------------------------------------------* * 常量定义 * @@ -139,3 +142,45 @@ void decode_MK32(const char *buf, uint32_t _iSize) } } +static void Custom_ModuleHandler(const Msg_t *pstMsg) +{ + if (NULL == pstMsg) + { + return; + } + + switch (pstMsg->m_uiMsgID) + { + case CUSTOM_GET_PV: + { + //log_i("RBCORE_CMD_STOP_ALL"); + Motor_CmdData_t tMotor_CmdData_t = {0}; + tMotor_CmdData_t.m_ucMotorIndex = 1; + tMotor_CmdData_t.m_iValue = (int32_t)pstMsg->m_aucData; + MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_SET_SPEED, (void *)&tMotor_CmdData_t, sizeof(Motor_CmdData_t)); + break; + } + default: + break; + } +} + +void Custom_Task(void *argument) +{ + 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); +}