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