换道逻辑调通

RBcore/include/BHBF.h

@@ -120,7 +120,8 @@ typedef enum {
 
 	TIMER_CMD_STRAIGHT_DRIVE,   // 机器人直线行驶命令格式为BHBF_straight_drive_Cmd
 	TIMER_CMD_RESET_STRAIGHT,   // 上一条命令支持计时前进，这里重置计时
-	TIMER_CMD_TURN_ANGLE,  	 // 机器人转向目标角度（带PID）
+	TIMER_CMD_TURN_ANGLE,  	    // 机器人转向目标角度（带PID）
+	TIMER_CMD_TURN_UP,          // 机器人转向头朝上
 } BHBF_Cmd_e;
 
 #define THREAD_DEFAULT_STACK 4096

RBcore/msp_Timer.c

@@ -20,6 +20,7 @@
 #include "msg_center.h"
 #include "rd_time.h"
 #include "tim.h"
+#include "lua_base.h"
 
 /*----------------------------------------------*
  * 外部变量说明                                 *
@@ -62,22 +63,30 @@ static void Timer_ModuleHandler(const Msg_t *pstMsg)
     static int g_dletAngle = 0; //PID最终计算补偿值
     static uint32_t uiLastSendTick = 0; //上一次时间戳，用于控制直线行驶距离
     static int bTimerStarted = 0; //直线行驶距离单次触发标志
+	static int bstrgightfinish = 0; //直线行驶完成
     switch (pstMsg->m_uiMsgID)
     {
         case TIMER_CMD_RESET_STRAIGHT:
         {
         	uiLastSendTick = 0;
         	bTimerStarted = 0;
+			bstrgightfinish = 0;
         	break;
         }
         case TIMER_CMD_STRAIGHT_DRIVE:
 		{
-			BHBF_straight_drive_Cmd stCmd = {0};
+			static BHBF_straight_drive_Cmd stCmd = {0};
 			if (pstMsg->m_uiDataLen >= sizeof(stCmd))
 			{
+				if (1 == bstrgightfinish) // 这里换道完成认为第一步旋转已经完成不再根据计时卡状态
+				{
+					MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_STRAIGHT_DRIVE, NULL, 0);
+					break;
+				}
+
 				RD_MEMCPY(&stCmd, pstMsg->m_aucData, sizeof(stCmd));
 
-				if (stCmd.m_iTime > 0 && 0 == bTimerStarted)
+				if (stCmd.m_iTime >= 0 && 0 == bTimerStarted)
 				{
 					uiLastSendTick = Rd_GetTime();
 					bTimerStarted = 1; // 开始计时
@@ -119,17 +128,14 @@ static void Timer_ModuleHandler(const Msg_t *pstMsg)
 					aiMotorSpeed[1] = g_dletAngle ;
 				}
 
-				if (stCmd.m_iTime > 0 && (int32_t)(Rd_GetTime() - uiLastSendTick) >= stCmd.m_iTime)// 到时间停止
+				if (stCmd.m_iTime >= 0 && (int32_t)(Rd_GetTime() - uiLastSendTick) >= stCmd.m_iTime)// 到时间停止
 				{
 					aiMotorSpeed[0] = 0;
 					aiMotorSpeed[1] = 0;
+					bstrgightfinish = 1;
+					MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_STRAIGHT_DRIVE, NULL, 0);
 				}
 				MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
-
-				if (aiMotorSpeed[0] == 0 && aiMotorSpeed[1] == 0)
-				{
-					MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
-				}
 			}
 			break;
 		}
@@ -139,10 +145,18 @@ static void Timer_ModuleHandler(const Msg_t *pstMsg)
 			if (pstMsg->m_uiDataLen >= sizeof(int))
 			{
 				RD_MEMCPY(&iTargetAngle, pstMsg->m_aucData, sizeof(int));
+
+				if (1 == bstrgightfinish) // 这里换道完成认为第一步旋转已经完成不再根据角度卡状态
+				{
+					MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
+					break;
+				}
+
 				if (abs(g_RF_Angle_Roll - iTargetAngle) <= 50)
 				{
 					aiMotorSpeed[0] = 0;
 					aiMotorSpeed[1] = 0;
+					MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
 				}
 				else if (abs(g_RF_Angle_Roll - iTargetAngle) <= 1000)
 				{
@@ -157,14 +171,37 @@ static void Timer_ModuleHandler(const Msg_t *pstMsg)
 					aiMotorSpeed[1] = g_dletAngle ;
 				}
 				MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
-
-				if (aiMotorSpeed[0] == 0 && aiMotorSpeed[1] == 0)
-				{
-					MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
-				}
 			}
 			break;
 		}
+        case TIMER_CMD_TURN_UP:
+		{
+			if (abs(g_RF_Angle_Roll - g_stCV.RobotUpAngleValue) <= 50)
+			{
+				aiMotorSpeed[0] = 0;
+				aiMotorSpeed[1] = 0;
+			}
+			else if (abs(g_RF_Angle_Roll - g_stCV.RobotUpAngleValue) <= 1000)
+			{
+				g_dletAngle = Angle_Tune_PID((double)g_RF_Angle_Roll, g_stCV.RobotUpAngleValue, 1, 0,0.5, 10);
+				aiMotorSpeed[0] = -g_dletAngle ;
+				aiMotorSpeed[1] = g_dletAngle;
+			}
+			else
+			{
+				g_dletAngle = Angle_Tune_PID((double)g_RF_Angle_Roll, g_stCV.RobotUpAngleValue, 2, 0, 0.5, 85);
+				aiMotorSpeed[0] = -g_dletAngle ;
+				aiMotorSpeed[1] = g_dletAngle ;
+			}
+			MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
+			break;
+		}
+        case COMMON_CMD_SHOW_INFO:
+		{
+			lua_print("\ng_dletAngle = %d\nuiLastSendTick = %d\nbTimerStarted = %d\nbstrgightfinish = %d\ng_RF_Angle_Roll = %d\n",
+					g_dletAngle, uiLastSendTick, bTimerStarted, bstrgightfinish, g_RF_Angle_Roll);
+			break;
+		}
         default:
             break;
     }

project/paint_robot_new/paint_robot_new.c

@@ -177,13 +177,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 		}
 		case CUSTOM_CMD_STRAIGHT_DRIVE:
 		{
-			BHBF_straight_drive_Cmd stCmd = {0};
+			MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_UP, NULL, 0);
-			if (pstMsg->m_uiDataLen >= sizeof(stCmd))
-			{
-				RD_MEMCPY(&stCmd, pstMsg->m_aucData, sizeof(stCmd));
-				int iTargetAngle = g_stCV.RobotUpAngleValue;
-				MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
-			}
 			break;
 		}
 		case CUSTOM_CMD_TURN_ANGLE:
@@ -194,7 +188,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 				RD_MEMCPY(&iTargetAngle, pstMsg->m_aucData, sizeof(int));
 				if (iTargetAngle == g_stCV.RobotUpAngleValue)
 				{
-					log_d("change line success");
+
 				}
 				else if (iTargetAngle == g_stCV.RobotLeftAngleValue)
 				{