From 3b1cce2275c6153da38febb84f618c9aea0bfe03 Mon Sep 17 00:00:00 2001
From: "DESKTOP-KQHDCM4\\97555" <975559679@qq.com>
Date: Wed, 15 Oct 2025 18:46:13 +0800
Subject: [PATCH] =?UTF-8?q?2025-10-15=20=20=E4=B8=8B=E7=8F=AD=E5=A4=87?=
=?UTF-8?q?=E4=BB=BD?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
Project/HC32F460/MDK/HC32F460.uvoptx | 15 ++
Project/HC32F460/source/User/Inc/main.h | 15 +-
Project/HC32F460/source/User/Src/bsp.c | 3 +
Project/HC32F460/source/User/Src/main.c | 253 ++++++++++++++++--------
4 files changed, 195 insertions(+), 91 deletions(-)
diff --git a/Project/HC32F460/MDK/HC32F460.uvoptx b/Project/HC32F460/MDK/HC32F460.uvoptx
index 1aefe63..c738b1b 100644
--- a/Project/HC32F460/MDK/HC32F460.uvoptx
+++ b/Project/HC32F460/MDK/HC32F460.uvoptx
@@ -210,6 +210,21 @@
1
App.ADTrack.RightLow_Org_AdZero,0x0A
+
+ 11
+ 1
+ App.LeftRunFalg,0x0A
+
+
+ 12
+ 1
+ App.RightRunFalg,0x0A
+
+
+ 13
+ 1
+ App.CentreRunFalg,0x0A
+
diff --git a/Project/HC32F460/source/User/Inc/main.h b/Project/HC32F460/source/User/Inc/main.h
index 0cc3472..412a337 100644
--- a/Project/HC32F460/source/User/Inc/main.h
+++ b/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_DOWN_2S,
+ APP_STATUS_UP1,
+ 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;
diff --git a/Project/HC32F460/source/User/Src/bsp.c b/Project/HC32F460/source/User/Src/bsp.c
index 9ded62f..881234b 100644
--- a/Project/HC32F460/source/User/Src/bsp.c
+++ b/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);
diff --git a/Project/HC32F460/source/User/Src/main.c b/Project/HC32F460/source/User/Src/main.c
index 3b7200a..7f279fc 100644
--- a/Project/HC32F460/source/User/Src/main.c
+++ b/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 RightDiffSum = abs(ru_diff[0]) + abs(rl_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 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)
- && (abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT))
+ if((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,72 +341,23 @@ 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;
}
}
}
}
}
else if(App.DownFlag == true)
- {
- if(App.LeftRunFalg == true)
- {
- if(lu_slope < -App.ADTrack.Slope_VPT
- && ll_slope > App.ADTrack.Slope_VPT
- && App.RunCollDelay1mSCnt <= 0)
- {
- BDC_STOP();
- App.StopFlag = true;
- App.DownFlag = false;
- App.UpFlag = false;
- App.StopCollDelay1mSCnt = STOP_TIME_MS;
- }
- else if(Olu_diff1 > -(App.ADTrack.LeftUpVPT / 2)
- && Oll_diff1 < (App.ADTrack.LeftLowVPT / 2))
- {
- BDC_STOP();
- App.StopFlag = true;
- App.DownFlag = false;
- App.UpFlag = false;
- App.StopCollDelay1mSCnt = STOP_TIME_MS;
- }
- }
- else if(App.RightRunFalg == true)
- {
- if(ru_slope < -App.ADTrack.Slope_VPT
- && rl_slope > App.ADTrack.Slope_VPT
- && App.RunCollDelay1mSCnt <= 0)
- {
- BDC_STOP();
- App.StopFlag = true;
- App.DownFlag = false;
- App.UpFlag = false;
- App.StopCollDelay1mSCnt = STOP_TIME_MS;
- }
- else if(Oru_diff1 > -(App.ADTrack.RightUpVPT / 2)
- && Orl_diff1 < (App.ADTrack.RightLowVPT / 2))
- {
- BDC_STOP();
- App.StopFlag = true;
- App.DownFlag = false;
- App.UpFlag = false;
- App.StopCollDelay1mSCnt = STOP_TIME_MS;
- }
- }
- }
- else if(App.UpFlag == true)
{
if(App.LeftRunFalg == true)
{
- if(lu_slope > App.ADTrack.Slope_VPT
- && ll_slope < -App.ADTrack.Slope_VPT
- && App.RunCollDelay1mSCnt <= 0)
+ if(ll_slope > App.ADTrack.Slope_VPT)
{
BDC_STOP();
App.StopFlag = true;
@@ -373,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)
- && Oll_diff1 > -(App.ADTrack.LeftLowVPT / 2))
+ else if(Oll_diff1 < App.ADTrack.Stop_VPT)
{
BDC_STOP();
App.StopFlag = true;
@@ -385,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
- && rl_slope < -App.ADTrack.Slope_VPT
- && App.RunCollDelay1mSCnt <= 0)
+ if(rl_slope > App.ADTrack.Slope_VPT)
{
BDC_STOP();
App.StopFlag = true;
@@ -395,9 +384,48 @@ 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)
- && Orl_diff1 > -(App.ADTrack.RightLowVPT / 2)
- )
+ else if(Orl_diff1 < App.ADTrack.Stop_VPT)
+ {
+ BDC_STOP();
+ App.StopFlag = true;
+ App.DownFlag = false;
+ App.UpFlag = false;
+ App.StopCollDelay1mSCnt = STOP_TIME_MS;
+ }
+ }
+ }
+ else if(App.UpFlag == true)
+ {
+ if(App.LeftRunFalg == true)
+ {
+ if(ll_slope < -App.ADTrack.Slope_VPT)
+ {
+ BDC_STOP();
+ App.StopFlag = true;
+ App.DownFlag = false;
+ App.UpFlag = false;
+ App.StopCollDelay1mSCnt = STOP_TIME_MS;
+ }
+ else if(Oll_diff1 > -App.ADTrack.Stop_VPT)
+ {
+ BDC_STOP();
+ App.StopFlag = true;
+ App.DownFlag = false;
+ App.UpFlag = false;
+ App.StopCollDelay1mSCnt = STOP_TIME_MS;
+ }
+ }
+ else if(App.RightRunFalg == true)
+ {
+ if(rl_slope < -App.ADTrack.Slope_VPT)
+ {
+ BDC_STOP();
+ App.StopFlag = true;
+ App.DownFlag = false;
+ App.UpFlag = false;
+ App.StopCollDelay1mSCnt = STOP_TIME_MS;
+ }
+ else if(Orl_diff1 > -App.ADTrack.Stop_VPT)
{
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.TD1mSDelayCnt = 10000;
+ App.Status = APP_STATUS_WAIT;
+ 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:{
- BDC_UP();
- if(App.TD1mSDelayCnt <= 0)
+ case APP_STATUS_UP1:{
+ if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-500))
+ {
+ 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:{
- BDC_DOWN();
- if(App.TD1mSDelayCnt <= 0)
+ case APP_STATUS_DOWN:{
+ if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-500))
+ {
+ 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++;