2025-10-15 下班备份

Project/HC32F460/MDK/HC32F460.uvoptx

@@ -210,6 +210,21 @@
           <WinNumber>1</WinNumber>
           <ItemText>App.ADTrack.RightLow_Org_AdZero,0x0A</ItemText>
         </Ww>
+        <Ww>
+          <count>11</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>App.LeftRunFalg,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>12</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>App.RightRunFalg,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>13</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>App.CentreRunFalg,0x0A</ItemText>
+        </Ww>
       </WatchWindow1>
       <MemoryWindow1>
         <Mm>

Project/HC32F460/source/User/Inc/main.h

@@ -12,14 +12,17 @@
 
 #define 	SLOPE_VPT				1000
 #define		FOLLOW_VPT			20
-#define		SHIELD_VPT			5000
+#define		SHIELD_VPT			2500
 
 #define 	STOP_TIME_MS		500
 #define 	RUN_TIME_MS			0
 
+#define		TRAVEL_TIME_MS	30000
+
+
 #define	BDC_STOP()			(SET_TWIN_BIT0(),SET_TWIN_BIT1())
 #define BDC_UP()			(SET_TWIN_BIT0(),CLR_TWIN_BIT1())
-#define	BDC_DOWN()			(CLR_TWIN_BIT0(),SET_TWIN_BIT1())
+#define	BDC_DOWN()			(SET_TWIN_BIT1(),CLR_TWIN_BIT0())
 	
 /*控制等级*/
 typedef enum {
@@ -33,6 +36,7 @@ typedef enum {
 /*设备阈值*/
 typedef struct{
 	int32_t Slope_VPT;
+	int32_t	Stop_VPT;
 	
 	int32_t LeftUpVPT;//左上设备阈值
 	int32_t LeftLowVPT;//左下设备阈值
@@ -70,8 +74,9 @@ typedef enum {
 	APP_STATUS_IDLE,
 	APP_STATUS_ON,
 	APP_STATUS_OFF,
-	APP_STATUS_UP_2S,
+	APP_STATUS_UP1,
-	APP_STATUS_DOWN_2S,
+	APP_STATUS_DOWN,
+	APP_STATUS_UP2,
 	APP_STATUS_WAIT,
 }AppStatus_m;
 
@@ -93,7 +98,6 @@ typedef struct {
 	uint32_t Read1mSDelayCnt;
 	uint32_t Uart1Delay1mSCnt;
 	uint32_t StopCollDelay1mSCnt;
-	uint32_t RunCollDelay1mSCnt;
 	uint32_t EvenRunDelay1mSCnt;
 	uint8_t FeedDogDlyCnt;
 	
@@ -103,6 +107,7 @@ typedef struct {
 	bool EvenRunning;//平稳运行标志位
 	bool LeftRunFalg;
 	bool RightRunFalg;
+	bool CentreRunFalg;
 
 	bool LeftUpCS1237ReadEndFlag;
 	bool LeftLowCS1237ReadEndFlag;

Project/HC32F460/source/User/Src/bsp.c

@@ -1347,6 +1347,8 @@ void BSP_Init(void)
 	SystemClockConfig();
 	SystemCoreClockUpdate();
 	//Xtal32_ClockConfig();
+	
+
 	GPIO_Config();
 	ExtiCh10_Init();
 	ExtiCh01_Init();
@@ -1364,6 +1366,7 @@ void BSP_Init(void)
 	//Adc1_Config();
 	//Dma_Config();
 	//FeedDog();
+
 	SysTick_Init(1000u);
 	SysTick_Delay(1000);
 	CLK_GetClockFreq(&Freq);

Project/HC32F460/source/User/Src/main.c

@@ -87,6 +87,7 @@ void GradeControl(ControlLevel_m level)
 			App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
 			App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
 			App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
+			App.ADTrack.Stop_VPT = 10000;
 		break;}
 		
 		case LEVEL_2:{
@@ -99,6 +100,7 @@ void GradeControl(ControlLevel_m level)
 			App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
 			App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
 			App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
+			App.ADTrack.Stop_VPT = 20000;
 		break;}
 		
 		case LEVEL_3:{
@@ -111,6 +113,7 @@ void GradeControl(ControlLevel_m level)
 			App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
 			App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
 			App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
+			App.ADTrack.Stop_VPT = 30000/2;
 		break;}
 		
 		case LEVEL_4:{
@@ -123,6 +126,7 @@ void GradeControl(ControlLevel_m level)
 			App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
 			App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
 			App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
+			App.ADTrack.Stop_VPT = 40000/2;	
 		break;}
 		
 		case LEVEL_5:{
@@ -135,6 +139,7 @@ void GradeControl(ControlLevel_m level)
 			App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
 			App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
 			App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
+			App.ADTrack.Stop_VPT = 50000/2;
 		break;}
 	}
 }
@@ -204,12 +209,46 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 		{
 			uint32_t LeftSlopeSum = abs(App.LeftUpCS1237_SlopeArray[0]) + abs(App.LeftLowCS1237_SlopeArray[0]);
 			uint32_t RightSlopeSum = abs(App.RightUpCS1237_SlopeArray[0]) + abs(App.RightLowCS1237_SlopeArray[0]);
-			uint32_t LeftDiffSum = abs(lu_diff[0]) + abs(ll_diff[0]);
+//			uint32_t LeftDiffSum = abs(lu_diff[0]) + abs(ll_diff[0]);
-			uint32_t RightDiffSum = abs(ru_diff[0]) + abs(rl_diff[0]);
+//			uint32_t RightDiffSum = abs(ru_diff[0]) + abs(rl_diff[0]);
+			uint32_t LeftDiffSum = abs(ll_diff[0]);
+			uint32_t RightDiffSum = abs(rl_diff[0]);
 			
 			App.LeftRunFalg = false;
 			App.RightRunFalg = false;
-			if(LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum)
+			App.CentreRunFalg = false;
+			if(((ll_ad[0] < App.ADTrack.LeftLow_Org_AdZero)
+			&& (rl_ad[0] < App.ADTrack.RightLow_Org_AdZero))
+		  || ((ll_ad[0] > App.ADTrack.LeftLow_Org_AdZero)
+			&& (rl_ad[0] > App.ADTrack.RightLow_Org_AdZero)))
+			{
+				App.CentreRunFalg = true;
+				if(abs(Oll_diff1) > abs(Orl_diff1))
+				{
+					App.LeftRunFalg = true;
+				}
+				else if(abs(Oll_diff1) < abs(Orl_diff1))
+				{
+					App.RightRunFalg = true;
+				}
+				if(Oll_diff1 < -App.ADTrack.LeftLowVPT 
+				|| Orl_diff1 < -App.ADTrack.RightLowVPT)
+				{
+					BDC_UP();
+					App.UpFlag = true;
+					App.StopFlag = false;
+					App.DownFlag = false;
+				}
+				else if(Oll_diff1 > App.ADTrack.LeftLowVPT 
+				|| Orl_diff1 > App.ADTrack.RightLowVPT)
+				{
+					BDC_DOWN();
+					App.UpFlag = false;
+					App.StopFlag = false;
+					App.DownFlag = true;
+				}
+			}
+			else if((LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum))
 			{
 				App.LeftRunFalg = true;
 				if(((App.LeftUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) 
@@ -218,21 +257,22 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 				&& (App.LeftLowCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
 				&& (App.LeftLowCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT)
 				&& (App.LeftLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)) 
-				|| ((Olu_diff1 >= App.ADTrack.LeftUpVPT)
+				|| (
+				(Olu_diff1 >= App.ADTrack.LeftUpVPT)
 				&& (Olu_diff2 >= App.ADTrack.LeftUpVPT)
 				&& (Olu_diff3 >= App.ADTrack.LeftUpVPT)
 				&& (Oll_diff1 <= -App.ADTrack.LeftLowVPT)
 				&& (Oll_diff2 <= -App.ADTrack.LeftLowVPT)
 				&& (Oll_diff3 <= -App.ADTrack.LeftLowVPT)))
 				{
-					if((abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT)
+					if((abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT)
-					&& (abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT))
+					&& (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT)
+					)
 					{
 						BDC_UP();
 						App.UpFlag = true;
 						App.StopFlag = false;
 						App.DownFlag = false;
-						App.RunCollDelay1mSCnt = RUN_TIME_MS;
 					}
 				}
 				else if(((App.LeftUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) 
@@ -241,7 +281,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 				&& (App.LeftLowCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT)
 				&& (App.LeftLowCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT)
 				&& (App.LeftLowCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT))
-				|| ((Olu_diff1 <= -App.ADTrack.LeftUpVPT) 
+				|| (
+				(Olu_diff1 <= -App.ADTrack.LeftUpVPT) 
 				&& (Olu_diff1 <= -App.ADTrack.LeftUpVPT)
 				&& (Olu_diff1 <= -App.ADTrack.LeftUpVPT)
 				&& (Oll_diff1 >= App.ADTrack.LeftLowVPT)
@@ -249,13 +290,13 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 				&& (Oll_diff1 >= App.ADTrack.LeftLowVPT)))
 				{
 					if((abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT)
-					&& (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT))
+					&& (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT)
+					)
 					{
 						BDC_DOWN();
 						App.UpFlag = false;
 						App.DownFlag = true;
 						App.StopFlag = false;
-						App.RunCollDelay1mSCnt = RUN_TIME_MS;
 					}
 				}
 			}
@@ -276,13 +317,13 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 				&& (Orl_diff3 <= -App.ADTrack.RightLowVPT)))
 				{
 					if((abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT)
-					&& (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT))
+					&& (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT)
+					)
 					{
 						BDC_UP();
 						App.UpFlag = true;
 						App.StopFlag = false;
 						App.DownFlag = false;
-						App.RunCollDelay1mSCnt = RUN_TIME_MS;
 					}
 				}
 				else if(((App.RightUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) 
@@ -300,13 +341,13 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 				)
 				{
 					if((abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT)
-					&& (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT))
+					&& (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT)
+					)
 					{
 						BDC_DOWN();		
 						App.DownFlag = true;
 						App.StopFlag = false;
 						App.UpFlag = false;
-						App.RunCollDelay1mSCnt = RUN_TIME_MS;
 					}
 				}
 			}
@@ -316,9 +357,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 	{
 		if(App.LeftRunFalg == true)
 		{
-				if(lu_slope < -App.ADTrack.Slope_VPT
+			if(ll_slope > App.ADTrack.Slope_VPT)
-				&& ll_slope > App.ADTrack.Slope_VPT
-				&& App.RunCollDelay1mSCnt <= 0)
 			{
 				BDC_STOP();
 				App.StopFlag = true;
@@ -326,8 +365,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 				App.UpFlag = false;
 				App.StopCollDelay1mSCnt = STOP_TIME_MS;
 			}
-				else if(Olu_diff1 > -(App.ADTrack.LeftUpVPT / 2)
+			else if(Oll_diff1 < App.ADTrack.Stop_VPT)
-				&& Oll_diff1 < (App.ADTrack.LeftLowVPT / 2))
 			{
 				BDC_STOP();
 				App.StopFlag = true;
@@ -338,9 +376,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 		}
 		else if(App.RightRunFalg == true)
 		{
-				if(ru_slope < -App.ADTrack.Slope_VPT
+			if(rl_slope > App.ADTrack.Slope_VPT)
-				&& rl_slope > App.ADTrack.Slope_VPT
-				&& App.RunCollDelay1mSCnt <= 0)
 			{
 				BDC_STOP();
 				App.StopFlag = true;
@@ -348,8 +384,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 				App.UpFlag = false;
 				App.StopCollDelay1mSCnt = STOP_TIME_MS;
 			}
-				else if(Oru_diff1 > -(App.ADTrack.RightUpVPT / 2)
+			else if(Orl_diff1 < App.ADTrack.Stop_VPT)
-				&& Orl_diff1 < (App.ADTrack.RightLowVPT / 2))
 			{
 				BDC_STOP();
 				App.StopFlag = true;
@@ -363,9 +398,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 	{
 		if(App.LeftRunFalg == true)
 		{
-			if(lu_slope > App.ADTrack.Slope_VPT
+			if(ll_slope < -App.ADTrack.Slope_VPT)
-			&& ll_slope < -App.ADTrack.Slope_VPT
-			&& App.RunCollDelay1mSCnt <= 0)
 			{
 				BDC_STOP();
 				App.StopFlag = true;
@@ -373,8 +406,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 				App.UpFlag = false;
 				App.StopCollDelay1mSCnt = STOP_TIME_MS;
 			}
-			else if(Olu_diff1 < (App.ADTrack.LeftUpVPT / 2)
+			else if(Oll_diff1 > -App.ADTrack.Stop_VPT)
-			&& Oll_diff1 > -(App.ADTrack.LeftLowVPT / 2))
 			{
 				BDC_STOP();
 				App.StopFlag = true;
@@ -385,9 +417,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 		}
 		else if(App.RightRunFalg == true)
 		{
-			if(ru_slope > App.ADTrack.Slope_VPT
+			if(rl_slope < -App.ADTrack.Slope_VPT)
-			&& rl_slope < -App.ADTrack.Slope_VPT
-			&& App.RunCollDelay1mSCnt <= 0)
 			{
 				BDC_STOP();
 				App.StopFlag = true;
@@ -395,9 +425,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 				App.UpFlag = false;
 				App.StopCollDelay1mSCnt = STOP_TIME_MS;
 			}
-			else if(Oru_diff1 < (App.ADTrack.RightUpVPT / 2)
+			else if(Orl_diff1 > -App.ADTrack.Stop_VPT)
-			&& Orl_diff1 > -(App.ADTrack.RightLowVPT / 2)
-			)
 			{
 				BDC_STOP();
 				App.StopFlag = true;
@@ -500,8 +528,10 @@ static void AppInit(void)
 	RIGHTUP_CS1237_Init();
 	RIGHTLOW_CS1237_Init();
 	
-	App.Status = APP_STATUS_DOWN_2S;
+	App.Status = APP_STATUS_WAIT;
-	App.TD1mSDelayCnt = 10000;
+	App.Status = APP_STATUS_UP1;
+	App.TD1mSDelayCnt = TRAVEL_TIME_MS;
+	//App.TD1mSDelayCnt = 1000;
 	App.RStatus = READ_STATUS_START;
 	GradeControl(App.Para.Layout.ControlLevel);//等级控制
 	
@@ -620,24 +650,75 @@ static void AppLoopHandler(void)
 			App.Status = APP_STATUS_IDLE;
 		break;}
 		
-		case APP_STATUS_UP_2S:{
+		case APP_STATUS_UP1:{
-			BDC_UP();
+			if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-500))
-			if(App.TD1mSDelayCnt <= 0)
+			{
+				BDC_UP();
+			}
+			else if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-2500) && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-500))
 			{
-				App.TD1mSDelayCnt = 2000;
-				App.Status = APP_STATUS_DOWN_2S;
 				BDC_STOP();
 			}
+			else if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-25000) && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-3000))
+			{
+				BDC_UP();
+			}
+			else if(App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-25000))
+			{
+				BDC_STOP();
+				App.Status = APP_STATUS_DOWN;
+				App.TD1mSDelayCnt = TRAVEL_TIME_MS;
+			}
 		break;}
 		
-		case APP_STATUS_DOWN_2S:{
+		case APP_STATUS_DOWN:{
-			BDC_DOWN();
+			if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-500))
-			if(App.TD1mSDelayCnt <= 0)
+			{
+				BDC_DOWN();
+			}
+			else if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-1500) && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-500))
 			{
-				App.TD1mSDelayCnt = 2000;
-				App.Status = APP_STATUS_WAIT;
 				BDC_STOP();
 			}
+			else if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-2500) && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-1500))
+			{
+				BDC_DOWN();
+			}
+			else if(App.TD1mSDelayCnt >= (TRAVEL_TIME_MS-26100) && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-26000))
+			{
+				BDC_STOP();
+			}
+			else if(App.TD1mSDelayCnt > 0 && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-27100))
+			{
+				BDC_DOWN();
+			}
+			else if(App.TD1mSDelayCnt <= 0)
+			{
+				BDC_STOP();
+				App.Status = APP_STATUS_UP2;
+				App.TD1mSDelayCnt = TRAVEL_TIME_MS;
+			}
+		break;}
+		
+		case APP_STATUS_UP2:{
+			if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-500))
+			{
+				BDC_UP();
+			}
+			else if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-2500) && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-500))
+			{
+				BDC_STOP();
+			}
+			else if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-25000) && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-3000))
+			{
+				BDC_UP();
+			}
+			else if(App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-25000))
+			{
+				BDC_STOP();
+				App.Status = APP_STATUS_WAIT;
+				App.TD1mSDelayCnt = 1000;
+			}
 		break;}
 				
 		case APP_STATUS_WAIT:{
@@ -658,7 +739,9 @@ static void AppLoopHandler(void)
 				App.ADTrack.LeftLow_Org_AdZero,
 				App.ADTrack.RightUp_Org_AdZero,
 				App.ADTrack.RightLow_Org_AdZero);
+				
 				App.Status = APP_STATUS_IDLE;
+				App.TD1mSDelayCnt = 0;
 			}
 		break;}
 	}
@@ -673,8 +756,6 @@ static void App1mSRoutine(void)
 		App.Uart1Delay1mSCnt--;
 	if(App.StopCollDelay1mSCnt > 0)
 		App.StopCollDelay1mSCnt--;
-	if(App.RunCollDelay1mSCnt > 0)
-		App.RunCollDelay1mSCnt--;
 	if(App.EvenRunning == true)
 		App.EvenRunDelay1mSCnt++;