From a6f4d0fb17f78d2eddd84de1abfc094b90c6548e Mon Sep 17 00:00:00 2001
From: "DESKTOP-KQHDCM4\\97555" <975559679@qq.com>
Date: Thu, 16 Oct 2025 18:54:13 +0800
Subject: [PATCH] =?UTF-8?q?=E4=B8=8B=E7=8F=AD=E5=A4=87=E4=BB=BD?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
Project/HC32F460/MDK/HC32F460.uvoptx | 5 +++
Project/HC32F460/source/User/Inc/main.h | 1 +
Project/HC32F460/source/User/Src/main.c | 54 +++++++++++++++----------
3 files changed, 39 insertions(+), 21 deletions(-)
diff --git a/Project/HC32F460/MDK/HC32F460.uvoptx b/Project/HC32F460/MDK/HC32F460.uvoptx
index c738b1b..df0776c 100644
--- a/Project/HC32F460/MDK/HC32F460.uvoptx
+++ b/Project/HC32F460/MDK/HC32F460.uvoptx
@@ -225,6 +225,11 @@
1
App.CentreRunFalg,0x0A
+
+ 14
+ 1
+ App.EvenRunDelay1mSCnt
+
diff --git a/Project/HC32F460/source/User/Inc/main.h b/Project/HC32F460/source/User/Inc/main.h
index 412a337..3e3526b 100644
--- a/Project/HC32F460/source/User/Inc/main.h
+++ b/Project/HC32F460/source/User/Inc/main.h
@@ -98,6 +98,7 @@ typedef struct {
uint32_t Read1mSDelayCnt;
uint32_t Uart1Delay1mSCnt;
uint32_t StopCollDelay1mSCnt;
+ uint32_t RunCollDelay1mSCnt;
uint32_t EvenRunDelay1mSCnt;
uint8_t FeedDogDlyCnt;
diff --git a/Project/HC32F460/source/User/Src/main.c b/Project/HC32F460/source/User/Src/main.c
index 7f279fc..c122142 100644
--- a/Project/HC32F460/source/User/Src/main.c
+++ b/Project/HC32F460/source/User/Src/main.c
@@ -217,10 +217,10 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.LeftRunFalg = false;
App.RightRunFalg = false;
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)))
+ 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))
@@ -231,21 +231,23 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
{
App.RightRunFalg = true;
}
- if(Oll_diff1 < -App.ADTrack.LeftLowVPT
- || Orl_diff1 < -App.ADTrack.RightLowVPT)
+ if(((Oll_diff1 + Orl_diff1) < -(App.ADTrack.LeftLowVPT + App.ADTrack.RightLowVPT)/2)
+ && (((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) < -App.ADTrack.Slope_VPT/4)))
{
BDC_UP();
App.UpFlag = true;
App.StopFlag = false;
App.DownFlag = false;
+ App.RunCollDelay1mSCnt = RUN_TIME_MS;
}
- else if(Oll_diff1 > App.ADTrack.LeftLowVPT
- || Orl_diff1 > App.ADTrack.RightLowVPT)
+ else if(((Oll_diff1 + Orl_diff1) > (App.ADTrack.LeftLowVPT + App.ADTrack.RightLowVPT)/2)
+ && (((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT/4)))
{
BDC_DOWN();
App.UpFlag = false;
App.StopFlag = false;
App.DownFlag = true;
+ App.RunCollDelay1mSCnt = RUN_TIME_MS;
}
}
else if((LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum))
@@ -353,11 +355,12 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
}
}
}
- else if(App.DownFlag == true)
+ else if(App.DownFlag == true && App.RunCollDelay1mSCnt <= 0)
{
if(App.LeftRunFalg == true)
{
- if(ll_slope > App.ADTrack.Slope_VPT)
+ if(((App.CentreRunFalg == false ) && (ll_slope > App.ADTrack.Slope_VPT))
+ || ((App.CentreRunFalg == true ) && (App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) < -App.ADTrack.Slope_VPT/3))
{
BDC_STOP();
App.StopFlag = true;
@@ -365,7 +368,8 @@ 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(Oll_diff1 < App.ADTrack.Stop_VPT)
+ else if(((App.CentreRunFalg == false ) && (Oll_diff1 < App.ADTrack.Stop_VPT))
+ || ((App.CentreRunFalg == true) && ((Oll_diff1 + Orl_diff1) < (App.ADTrack.Stop_VPT))))
{
BDC_STOP();
App.StopFlag = true;
@@ -376,7 +380,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
}
else if(App.RightRunFalg == true)
{
- if(rl_slope > App.ADTrack.Slope_VPT)
+ if(((App.CentreRunFalg == false ) && (ll_slope > App.ADTrack.Slope_VPT))
+ || ((App.CentreRunFalg == true ) && (App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) < -App.ADTrack.Slope_VPT/3))
{
BDC_STOP();
App.StopFlag = true;
@@ -384,7 +389,8 @@ 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(Orl_diff1 < App.ADTrack.Stop_VPT)
+ else if(((App.CentreRunFalg == false ) && (Orl_diff1 < App.ADTrack.Stop_VPT))
+ || ((App.CentreRunFalg == true) && ((Oll_diff1 + Orl_diff1) < (App.ADTrack.Stop_VPT))))
{
BDC_STOP();
App.StopFlag = true;
@@ -394,11 +400,12 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
}
}
}
- else if(App.UpFlag == true)
+ else if(App.UpFlag == true && App.RunCollDelay1mSCnt <= 0)
{
if(App.LeftRunFalg == true)
{
- if(ll_slope < -App.ADTrack.Slope_VPT)
+ if(((App.CentreRunFalg == false ) && (ll_slope < -App.ADTrack.Slope_VPT))
+ || ((App.CentreRunFalg == true ) && (App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT/3))
{
BDC_STOP();
App.StopFlag = true;
@@ -406,7 +413,8 @@ 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(Oll_diff1 > -App.ADTrack.Stop_VPT)
+ else if(((App.CentreRunFalg == false ) && (Oll_diff1 > -App.ADTrack.Stop_VPT))
+ || ((App.CentreRunFalg == true) && ((Oll_diff1 + Orl_diff1)> -(App.ADTrack.Stop_VPT))))
{
BDC_STOP();
App.StopFlag = true;
@@ -417,7 +425,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
}
else if(App.RightRunFalg == true)
{
- if(rl_slope < -App.ADTrack.Slope_VPT)
+ if(((App.CentreRunFalg == false ) && (rl_slope < -App.ADTrack.Slope_VPT))
+ || ((App.CentreRunFalg == true ) && (App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT/3))
{
BDC_STOP();
App.StopFlag = true;
@@ -425,7 +434,8 @@ 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(Orl_diff1 > -App.ADTrack.Stop_VPT)
+ else if(((App.CentreRunFalg == false ) && (Orl_diff1 > -App.ADTrack.Stop_VPT))
+ || ((App.CentreRunFalg == true) && ((Oll_diff1 + Orl_diff1) > -(App.ADTrack.Stop_VPT))))
{
BDC_STOP();
App.StopFlag = true;
@@ -529,9 +539,9 @@ static void AppInit(void)
RIGHTLOW_CS1237_Init();
App.Status = APP_STATUS_WAIT;
- App.Status = APP_STATUS_UP1;
- App.TD1mSDelayCnt = TRAVEL_TIME_MS;
- //App.TD1mSDelayCnt = 1000;
+ App.TD1mSDelayCnt = 1000;
+// App.Status = APP_STATUS_UP1;
+// App.TD1mSDelayCnt = TRAVEL_TIME_MS;
App.RStatus = READ_STATUS_START;
GradeControl(App.Para.Layout.ControlLevel);//µÈ¼¶¿ØÖÆ
@@ -756,6 +766,8 @@ static void App1mSRoutine(void)
App.Uart1Delay1mSCnt--;
if(App.StopCollDelay1mSCnt > 0)
App.StopCollDelay1mSCnt--;
+ if(App.RunCollDelay1mSCnt > 0)
+ App.RunCollDelay1mSCnt--;
if(App.EvenRunning == true)
App.EvenRunDelay1mSCnt++;