From 7dfff142c2ba22cf123e56da949a34c1b57a30ac Mon Sep 17 00:00:00 2001 From: "DESKTOP-KQHDCM4\\97555" <975559679@qq.com> Date: Thu, 30 Oct 2025 16:28:23 +0800 Subject: [PATCH] =?UTF-8?q?=E7=AE=97=E6=B3=95=E4=BC=98=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Project/HC32F460/source/User/Inc/main.h | 2 ++ Project/HC32F460/source/User/Src/main.c | 31 +++++++++++++++---------- 2 files changed, 21 insertions(+), 12 deletions(-) diff --git a/Project/HC32F460/source/User/Inc/main.h b/Project/HC32F460/source/User/Inc/main.h index 21f9d8f..0a236c8 100644 --- a/Project/HC32F460/source/User/Inc/main.h +++ b/Project/HC32F460/source/User/Inc/main.h @@ -19,6 +19,7 @@ #define RUN_TIME_MS 0 #define EME_START_MS 1000 #define EME_STOP_MS 2000 +#define SLOPE_START_MS 3000 #define TRAVEL_TIME_MS 30000 @@ -107,6 +108,7 @@ typedef struct { uint32_t StopCollDelay1mSCnt; uint32_t RunCollDelay1mSCnt; uint32_t EmergencyDelay1mSCnt; + uint32_t SlopeCollDelay1mSCnt; uint8_t FeedDogDlyCnt; bool UpFlag;//ÉÏÉý±ê־λ diff --git a/Project/HC32F460/source/User/Src/main.c b/Project/HC32F460/source/User/Src/main.c index 0eceebe..de7597d 100644 --- a/Project/HC32F460/source/User/Src/main.c +++ b/Project/HC32F460/source/User/Src/main.c @@ -245,6 +245,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.UpFlag = false; App.EmeFlag = false; App.StopCollDelay1mSCnt = STOP_TIME_MS; + App.SlopeCollDelay1mSCnt = SLOPE_START_MS; } } else if(App.StopFlag == true && App.StopCollDelay1mSCnt <= 0) @@ -320,7 +321,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (App.LeftUpCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT) && (App.LeftLowCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) && (App.LeftLowCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) - && (App.LeftLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)) + && (App.LeftLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT) + && (App.SlopeCollDelay1mSCnt <= 0)) || ( (Olu_diff1 >= App.ADTrack.LeftUpVPT*5/6) && (Olu_diff2 >= App.ADTrack.LeftUpVPT*5/6) @@ -347,7 +349,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (App.LeftUpCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT) && (App.LeftLowCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) && (App.LeftLowCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) - && (App.LeftLowCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)) + && (App.LeftLowCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT) + && ((App.SlopeCollDelay1mSCnt <= 0))) || ( (Olu_diff1 <= -App.ADTrack.LeftUpVPT) && (Olu_diff1 <= -App.ADTrack.LeftUpVPT) @@ -378,7 +381,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (App.RightUpCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT) && (App.RightLowCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) && (App.RightLowCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) - && (App.RightLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)) + && (App.RightLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT) + && (App.SlopeCollDelay1mSCnt <= 0)) || ((Oru_diff1 >= App.ADTrack.RightUpVPT*5/6) && (Oru_diff2 >= App.ADTrack.RightUpVPT*5/6) && (Oru_diff3 >= App.ADTrack.RightUpVPT*5/6) @@ -404,7 +408,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (App.RightUpCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT) && (App.RightLowCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) && (App.RightLowCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) - && (App.RightLowCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)) + && (App.RightLowCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT) + && (App.SlopeCollDelay1mSCnt <= 0)) || ((Oru_diff1 <= -App.ADTrack.RightUpVPT) && (Oru_diff2 <= -App.ADTrack.RightUpVPT) && (Oru_diff3 <= -App.ADTrack.RightUpVPT) @@ -441,7 +446,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.UpFlag = false; App.EmeFlag = false; App.StopCollDelay1mSCnt = STOP_TIME_MS; - App.EmergencyDelay1mSCnt = EME_START_MS; + App.SlopeCollDelay1mSCnt = SLOPE_START_MS; } else if(((App.CentreRunFalg == false ) && (Oll_diff1 < App.ADTrack.Stop_VPT)) || ((App.CentreRunFalg == true) && ((Oll_diff1 + Orl_diff1) < (App.ADTrack.Stop_VPT)))) @@ -452,7 +457,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.UpFlag = false; App.EmeFlag = false; App.StopCollDelay1mSCnt = STOP_TIME_MS; - App.EmergencyDelay1mSCnt = EME_START_MS; + App.SlopeCollDelay1mSCnt = SLOPE_START_MS; } else if((abs(Oll_diff1) - EME_STOP_VPT) < abs(Orl_diff1) && (abs(Oll_diff2) - EME_STOP_VPT) < abs(Orl_diff2) @@ -475,7 +480,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.UpFlag = false; App.EmeFlag = false; App.StopCollDelay1mSCnt = STOP_TIME_MS; - App.EmergencyDelay1mSCnt = EME_START_MS; + App.SlopeCollDelay1mSCnt = SLOPE_START_MS; } else if(((App.CentreRunFalg == false ) && (Orl_diff1 < App.ADTrack.Stop_VPT)) || ((App.CentreRunFalg == true) && ((Oll_diff1 + Orl_diff1) < (App.ADTrack.Stop_VPT)))) @@ -486,7 +491,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.UpFlag = false; App.EmeFlag = false; App.StopCollDelay1mSCnt = STOP_TIME_MS; - App.EmergencyDelay1mSCnt = EME_START_MS; + App.SlopeCollDelay1mSCnt = SLOPE_START_MS; } else if(abs(Oll_diff1) > (abs(Orl_diff1) - EME_STOP_VPT) && abs(Oll_diff2) > (abs(Orl_diff2) - EME_STOP_VPT) @@ -512,7 +517,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.UpFlag = false; App.EmeFlag = false; App.StopCollDelay1mSCnt = STOP_TIME_MS; - App.EmergencyDelay1mSCnt = EME_START_MS; + App.SlopeCollDelay1mSCnt = SLOPE_START_MS; } else if(((App.CentreRunFalg == false ) && (Oll_diff1 > -App.ADTrack.Stop_VPT)) || ((App.CentreRunFalg == true) && ((Oll_diff1 + Orl_diff1) > -App.ADTrack.Stop_VPT))) @@ -523,7 +528,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.UpFlag = false; App.EmeFlag = false; App.StopCollDelay1mSCnt = STOP_TIME_MS; - App.EmergencyDelay1mSCnt = EME_START_MS; + App.SlopeCollDelay1mSCnt = SLOPE_START_MS; } else if((abs(Oll_diff1) - EME_STOP_VPT) < abs(Orl_diff1) && (abs(Oll_diff2) - EME_STOP_VPT) < abs(Orl_diff2) @@ -546,7 +551,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.UpFlag = false; App.EmeFlag = false; App.StopCollDelay1mSCnt = STOP_TIME_MS; - App.EmergencyDelay1mSCnt = EME_START_MS; + App.SlopeCollDelay1mSCnt = SLOPE_START_MS; } else if(((App.CentreRunFalg == false ) && (Orl_diff1 > -App.ADTrack.Stop_VPT)) || ((App.CentreRunFalg == true) && ((Oll_diff1 + Orl_diff1) > -(App.ADTrack.Stop_VPT)))) @@ -557,7 +562,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.UpFlag = false; App.EmeFlag = false; App.StopCollDelay1mSCnt = STOP_TIME_MS; - App.EmergencyDelay1mSCnt = EME_START_MS; + App.SlopeCollDelay1mSCnt = SLOPE_START_MS; } else if(abs(Oll_diff1) > (abs(Orl_diff1) - EME_STOP_VPT) && abs(Oll_diff2) > (abs(Orl_diff2) - EME_STOP_VPT) @@ -876,6 +881,8 @@ static void App1mSRoutine(void) App.RunCollDelay1mSCnt--; if(App.EmergencyDelay1mSCnt > 0) App.EmergencyDelay1mSCnt--; + if(App.SlopeCollDelay1mSCnt > 0) + App.SlopeCollDelay1mSCnt--; App.FeedDogDlyCnt++; } void FeedDogHandler(void)