paint_robot_new v1.0【调通打标】电机相对稳定运行，之后出现初始化问题再看

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)

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);

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;
     
 // 电机命令数据结构

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
 
 /*==============================================*

motor/motor_example.c

@@ -184,37 +184,40 @@ void MotorExample_MultiProtocol(void)
 
 void MotorTask(void *argument)
 {
-    // 等待系统稳定
+	MotorMgr_ResetAll(g_apstMotors[0]);
-    Rd_Delay(1000);
+	Rd_Delay(500);
-
+	MotorMgr_ResetAll(g_apstMotors[1]);
-    // 复位并激活所有电机
+	Rd_Delay(500);
-    for (int i = 0; i < MOTOR_INSTANCE_MAX; i++)
+
-    {
+	int iCount = 6;
-        if (g_apstMotors[i] != NULL)
+	while(iCount--)
-        {
+	{
-            MotorMgr_ResetAll(g_apstMotors[i]);
+		MotorMgr_ActivateAll(g_apstMotors[0]);
-            Rd_Delay(500);
+		Rd_Delay(500);
-            MotorMgr_ActivateAll(g_apstMotors[i]);
+		MotorMgr_ActivateAll(g_apstMotors[1]);
-            Rd_Delay(500);
+		Rd_Delay(500);
-            MotorMgr_SetHome(g_apstMotors[i]);
+	}
-            Rd_Delay(100);
+
-            MotorMgr_SpeedModeInit(g_apstMotors[i]);
+	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_RequestPosition(g_apstMotors[i]);
-                MotorMgr_RequestFault(g_apstMotors[i]);
+//                MotorMgr_RequestFault(g_apstMotors[i]);
-                MotorMgr_RequestVelocity(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);
 }

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};
+    uint8_t aucData2[2] = {0x01, pstMotor->m_stConfig.m_ucMotorID};
-    rd_ComIDSend(pstMotor->m_ptCAN, 0x000, (char *)aucData1, 2);
-
-    uint8_t aucData2[2] = {0x01, 0x02};
     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;
 }

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);
+}