From 3f43369c55284b67b970817d7d942075ab9ef69c Mon Sep 17 00:00:00 2001
From: Lizongdi <1210855344@qq.com>
Date: Thu, 17 Sep 2026 11:21:49 +0800
Subject: [PATCH] =?UTF-8?q?=E3=80=90=E5=BE=85=E4=BC=98=E5=8C=96=E3=80=91?=
 =?UTF-8?q?=E8=A7=A3=E5=86=B3PID=E6=8E=A7=E5=88=B6=E4=B8=8D=E5=8F=8A?=
 =?UTF-8?q?=E6=97=B6=E9=97=AE=E9=A2=98=EF=BC=8C=E7=94=B5=E6=9C=BA=E6=9F=A5?=
 =?UTF-8?q?=E8=AF=A2=E5=91=BD=E4=BB=A4=E9=99=8D=E9=A2=91=EF=BC=8C=E9=99=80?=
 =?UTF-8?q?=E8=9E=BA=E4=BB=AA=E6=94=B9=E4=B8=BA=E5=85=A8=E5=B1=80=E5=8F=98?=
 =?UTF-8?q?=E9=87=8F=EF=BC=88=E8=BF=99=E9=87=8C=E6=9C=89=E5=BE=85=E5=95=86?=
 =?UTF-8?q?=E8=AE=AE=EF=BC=89?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 RBcore/TL720D.c                               |  3 +-
 .../paint_robot_new/paint_robot_new_motors.c  | 32 ++++++++++++-------
 2 files changed, 22 insertions(+), 13 deletions(-)

diff --git a/RBcore/TL720D.c b/RBcore/TL720D.c
index d2b12d3..0c9dfc5 100644
--- a/RBcore/TL720D.c
+++ b/RBcore/TL720D.c
@@ -41,6 +41,7 @@
  * 模块级变量                                   *
  *----------------------------------------------*/
 static MSP_TL720DParameters g_stTL720D = {0};
+volatile int32_t g_RF_Angle_Roll = 0;
 
 /*----------------------------------------------*
  * 常量定义                                     *
@@ -120,12 +121,12 @@ static void decode_TL720D(const char *buf, uint32_t _iSize)
 	g_stTL720D.RF_Gro_Y = getDeci((uint8_t *)&buf[25]);
 	g_stTL720D.RF_Gro_Z = getDeci((uint8_t *)&buf[28]);
 
+	g_RF_Angle_Roll = g_stTL720D.RF_Angle_Roll;
 	uint32_t uiNowTick = Rd_GetTime();
 	if ((int32_t)(uiNowTick - uiLastSendTick) >= 50)
 	{
 		uiLastSendTick = uiNowTick;
 		MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_GET_TL720D_ROLL, (void *)&g_stTL720D.RF_Angle_Roll, sizeof(int32_t));
-		MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_GET_TL720D_ROLL, (void *)&g_stTL720D.RF_Angle_Roll, sizeof(int32_t));
 		MsgCenter_SendTo(MODULE_NAME_DAEMON, DAEMON_SET_TL720D, NULL, 0);
 	}
 }
diff --git a/project/paint_robot_new/paint_robot_new_motors.c b/project/paint_robot_new/paint_robot_new_motors.c
index f4d0a1d..d17cb9b 100644
--- a/project/paint_robot_new/paint_robot_new_motors.c
+++ b/project/paint_robot_new/paint_robot_new_motors.c
@@ -189,6 +189,8 @@ void MotorTask(void *argument)
     	else if (g_isMotorInit > 0)
     	{
     		static int LS_Motor_Config_Count = 0;
+    		static int iStatusQueryCycle = 0;
+
 			if (LS_Motor_Config_Count <= 200)
 			{
 				LS_Motor_Config_Count++;
@@ -197,22 +199,28 @@ void MotorTask(void *argument)
 				MotorMgr_SetWatchdog(g_apstMotors[1], 1000);
 				Rd_Delay(4);
 			}
-			MotorMgr_RequestPosition(g_apstMotors[0]);
-			Rd_Delay(4);
-			MotorMgr_RequestPosition(g_apstMotors[1]);
-			Rd_Delay(4);
-			MotorMgr_RequestFault(g_apstMotors[0]);
-			Rd_Delay(4);
-			MotorMgr_RequestFault(g_apstMotors[1]);
-			Rd_Delay(4);
-			MotorMgr_RequestVelocity(g_apstMotors[0]);
-			Rd_Delay(4);
-			MotorMgr_RequestVelocity(g_apstMotors[1]);
-			Rd_Delay(4);
+			
 			MotorMgr_SetTargetSpeed(g_apstMotors[0], g_aiMotorSpeed[0]);
 			Rd_Delay(4);
 			MotorMgr_SetTargetSpeed(g_apstMotors[1], g_aiMotorSpeed[1]);
 			Rd_Delay(4);
+
+			if (++iStatusQueryCycle >= 10)
+			{
+				iStatusQueryCycle = 0;
+				MotorMgr_RequestPosition(g_apstMotors[0]);
+				Rd_Delay(4);
+				MotorMgr_RequestPosition(g_apstMotors[1]);
+				Rd_Delay(4);
+				MotorMgr_RequestFault(g_apstMotors[0]);
+				Rd_Delay(4);
+				MotorMgr_RequestFault(g_apstMotors[1]);
+				Rd_Delay(4);
+				MotorMgr_RequestVelocity(g_apstMotors[0]);
+				Rd_Delay(4);
+				MotorMgr_RequestVelocity(g_apstMotors[1]);
+				Rd_Delay(4);
+			}
     	}
     }
 }