diff --git a/RBcore/TL720D.c b/RBcore/TL720D.c index 0c9dfc5..f866b44 100644 --- a/RBcore/TL720D.c +++ b/RBcore/TL720D.c @@ -41,7 +41,7 @@ * 模块级变量 * *----------------------------------------------*/ static MSP_TL720DParameters g_stTL720D = {0}; -volatile int32_t g_RF_Angle_Roll = 0; +volatile int32_t g_RF_Angle_Roll = 0; // 注意这个全局是为了解决PID实时性问题添加的，不能在其他位置使用 /*----------------------------------------------* * 常量定义 * @@ -122,6 +122,7 @@ static void decode_TL720D(const char *buf, uint32_t _iSize) g_stTL720D.RF_Gro_Z = getDeci((uint8_t *)&buf[28]); g_RF_Angle_Roll = g_stTL720D.RF_Angle_Roll; + // 注意g_RF_Angle_Roll这个全局是为了解决PID实时性问题添加的，不能在其他位置使用 uint32_t uiNowTick = Rd_GetTime(); if ((int32_t)(uiNowTick - uiLastSendTick) >= 50) { diff --git a/RBcore/include/BHBF.h b/RBcore/include/BHBF.h index c809bd8..ab5f3e6 100644 --- a/RBcore/include/BHBF.h +++ b/RBcore/include/BHBF.h @@ -125,6 +125,7 @@ typedef struct { int m_iMode; //正数表示前进，负数表示倒退 int m_iAngle; //目标角度 int m_iTime; //行驶时间(单位毫秒)计时，负数表示一直行走 + int m_iSpeed; //车速 } BHBF_straight_drive_Cmd; /*==============================================* * project-wide global variables * diff --git a/RBcore/msp_Timer.c b/RBcore/msp_Timer.c index 25dd1c7..04efbfe 100644 --- a/RBcore/msp_Timer.c +++ b/RBcore/msp_Timer.c @@ -102,13 +102,13 @@ static void Timer_ModuleHandler(const Msg_t *pstMsg) } if (stCmd.m_iMode > 0) { - aiMotorSpeed[0] = (g_stCV.Lane_Change_Speed_m_per_min * 10) - g_dletAngle ; - aiMotorSpeed[1] = (g_stCV.Lane_Change_Speed_m_per_min * 10) + g_dletAngle ; + aiMotorSpeed[0] = (stCmd.m_iSpeed * 10) - g_dletAngle ; + aiMotorSpeed[1] = (stCmd.m_iSpeed * 10) + g_dletAngle ; } else { - aiMotorSpeed[0] = -(g_stCV.Lane_Change_Speed_m_per_min * 10) - g_dletAngle ; - aiMotorSpeed[1] = -(g_stCV.Lane_Change_Speed_m_per_min * 10) + g_dletAngle ; + aiMotorSpeed[0] = -(stCmd.m_iSpeed * 10) - g_dletAngle ; + aiMotorSpeed[1] = -(stCmd.m_iSpeed * 10) + g_dletAngle ; } } else diff --git a/project/paint_robot_new/paint_robot_new.c b/project/paint_robot_new/paint_robot_new.c index 1c85f2d..2cdcfe7 100644 --- a/project/paint_robot_new/paint_robot_new.c +++ b/project/paint_robot_new/paint_robot_new.c @@ -119,6 +119,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) static int iLeft_Compensation = 0; static int iRight_Compensation = 0; + static int iVehicleSpeed = 1; switch (pstMsg->m_uiMsgID) { case CUSTOM_GET_DAEMON_CODE: @@ -198,7 +199,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) BHBF_straight_drive_Cmd stCmd = { .m_iMode = 1, .m_iAngle = g_stCV.RobotLeftAngleValue + iLeft_Compensation, - .m_iTime = RunTime_DistanceCm_SpeedE_2MPMin() + .m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(), + .m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min }; MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); } @@ -207,7 +209,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) BHBF_straight_drive_Cmd stCmd = { .m_iMode = -1, .m_iAngle = g_stCV.RobotLeftAngleValue - iRight_Compensation, - .m_iTime = RunTime_DistanceCm_SpeedE_2MPMin() + .m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(), + .m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min }; MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); } @@ -219,7 +222,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) BHBF_straight_drive_Cmd stCmd = { .m_iMode = -1, .m_iAngle = g_stCV.RobotRightAngleValue + iLeft_Compensation, - .m_iTime = RunTime_DistanceCm_SpeedE_2MPMin() + .m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(), + .m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min }; MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); } @@ -228,7 +232,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) BHBF_straight_drive_Cmd stCmd = { .m_iMode = 1, .m_iAngle = g_stCV.RobotRightAngleValue - iRight_Compensation, - .m_iTime = RunTime_DistanceCm_SpeedE_2MPMin() + .m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(), + .m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min }; MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); } @@ -307,7 +312,6 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) // 更新速度旋钮值 int iSpeedSelection = 3 * (g_stMK32.CH11_RD1 + 1000) / 200; - int iVehicleSpeed = 1; if (iSpeedSelection > 0) { iVehicleSpeed = g_stCV.Speed_m_per_min*iSpeedSelection/30; @@ -405,7 +409,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) BHBF_straight_drive_Cmd stCmd = { .m_iMode = 1, .m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration, - .m_iTime = -1 + .m_iTime = -1, + .m_iSpeed = iVehicleSpeed }; MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); } @@ -426,7 +431,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) BHBF_straight_drive_Cmd stCmd = { .m_iMode = -1, .m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration, - .m_iTime = -1 + .m_iTime = -1, + .m_iSpeed = iVehicleSpeed }; MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); } @@ -460,7 +466,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) BHBF_straight_drive_Cmd stCmd = { .m_iMode = 1, .m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration, - .m_iTime = -1 + .m_iTime = -1, + .m_iSpeed = iVehicleSpeed }; MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); } @@ -480,7 +487,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) BHBF_straight_drive_Cmd stCmd = { .m_iMode = -1, .m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration, - .m_iTime = -1 + .m_iTime = -1, + .m_iSpeed = iVehicleSpeed }; MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); }