From 0e05817f635baa2807d3e6726d15c4d48f3028a6 Mon Sep 17 00:00:00 2001 From: "DESKTOP-KQHDCM4\\97555" <975559679@qq.com> Date: Fri, 13 Mar 2026 20:00:20 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A4=8D=E6=A0=B7=E5=90=8E=E7=9A=84=E5=8F=8C?= =?UTF-8?q?=E6=9F=B1=E6=94=AF=E6=9E=B6=E8=B0=83=E8=AF=95=201=E3=80=81?= =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E4=BA=86=E6=99=83=E5=8A=A8=E5=90=8E=E6=94=AF?= =?UTF-8?q?=E6=9E=B6=E4=B9=B1=E8=B7=B3=E7=9A=84bug=202=E3=80=81=E4=BF=AE?= =?UTF-8?q?=E5=A4=8D=E4=BA=86=E4=BB=8E=E5=BA=95=E9=83=A8=E5=88=B0=E9=A1=B6?= =?UTF-8?q?=E9=83=A8=E5=90=8E=E6=8E=A7=E5=88=B6=E5=8A=9B=E5=8F=98=E5=B7=AE?= =?UTF-8?q?=E7=9A=84bug=203=E3=80=81=E6=8E=A7=E5=88=B6=E6=96=B9=E5=BC=8F?= =?UTF-8?q?=E6=94=B9=E4=B8=BA=E4=BB=A5=E4=B8=8A=E9=9D=A2=E4=B8=A4=E4=B8=AA?= =?UTF-8?q?=E4=BC=A0=E6=84=9F=E5=99=A8=E4=B8=BA=E4=B8=BB=204=E3=80=81?= =?UTF-8?q?=E6=8E=A7=E5=88=B6=E5=AE=8C=E6=88=90=E5=90=8E=E8=AE=BE=E5=A4=87?= =?UTF-8?q?=E8=BF=98=E6=9C=AA=E6=A0=A1=E9=9B=B6=EF=BC=8C=E9=A9=AC=E4=B8=8A?= =?UTF-8?q?=E8=BF=9B=E8=A1=8C=E4=BE=A7=E9=9D=A2=E6=8E=A8=E6=8B=89=E4=BC=9A?= =?UTF-8?q?=E4=BA=A7=E7=94=9F=E8=AF=AF=E6=8E=A7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Project/HC32F460/MDK/HC32F460.uvoptx | 59 +++- Project/HC32F460/MDK/HC32F460.uvprojx | 1 + Project/HC32F460/source/User/Inc/main.h | 5 +- Project/HC32F460/source/User/Src/main.c | 385 +++++++++++++++--------- 4 files changed, 302 insertions(+), 148 deletions(-) diff --git a/Project/HC32F460/MDK/HC32F460.uvoptx b/Project/HC32F460/MDK/HC32F460.uvoptx index ec37fb1..a6a444e 100644 --- a/Project/HC32F460/MDK/HC32F460.uvoptx +++ b/Project/HC32F460/MDK/HC32F460.uvoptx @@ -103,7 +103,7 @@ 1 0 0 - 4 + 3 @@ -114,7 +114,7 @@ - Segger\JL2CM3.dll + BIN\CMSIS_AGDI.dll @@ -225,6 +225,61 @@ 1 App.EmergencyDelay1mSCnt,0x0A + + 14 + 1 + App.ADTrack.Slope_VPT,0x0A + + + 15 + 1 + App.RightUpCS1237_SlopeArray[0],0x0A + + + 16 + 1 + App.ADTrack.Stop_VPT,0x0A + + + 17 + 1 + Olu_diff1,0x0A + + + 18 + 1 + Oru_diff1,0x0A + + + 19 + 1 + Olu_diff2,0x0A + + + 20 + 1 + Oru_diff2,0x0A + + + 21 + 1 + Olu_diff3,0x0A + + + 22 + 1 + Oru_diff3,0x0A + + + 23 + 1 + App.LeftUpCS1237_SlopeArray[0],0x0A + + + 24 + 1 + App.RightUpCS1237_SlopeArray[0],0x0A + 0 diff --git a/Project/HC32F460/MDK/HC32F460.uvprojx b/Project/HC32F460/MDK/HC32F460.uvprojx index 233d7a2..a610f11 100644 --- a/Project/HC32F460/MDK/HC32F460.uvprojx +++ b/Project/HC32F460/MDK/HC32F460.uvprojx @@ -10,6 +10,7 @@ Debug 0x4 ARM-ADS + 5060960::V5.06 update 7 (build 960)::.\ARMCC 5060960::V5.06 update 7 (build 960)::.\ARMCC 0 diff --git a/Project/HC32F460/source/User/Inc/main.h b/Project/HC32F460/source/User/Inc/main.h index 41ac002..52fd958 100644 --- a/Project/HC32F460/source/User/Inc/main.h +++ b/Project/HC32F460/source/User/Inc/main.h @@ -11,12 +11,13 @@ #define SLOPE_CNT 20 #define SLOPE_VPT 1000 +#define PRESS_VPT 10 #define FOLLOW_VPT 20 #define SHIELD_VPT 5000 #define EME_STOP_VPT 4000 #define CENTRE_VPT 2000 -#define STOP_TIME_MS 1000 +#define STOP_TIME_MS 500 #define RUN_TIME_MS 0 #define EME_START_MS 0 #define EME_STOP_MS 2000 @@ -112,6 +113,8 @@ typedef struct { uint32_t SlopeCollDelay1mSCnt; uint8_t FeedDogDlyCnt; + bool CaliZeroFlag;//校零标志位 + bool SideFlag;//侧压标志位 bool UpFlag;//上升标志位 bool DownFlag;//下降标志位 bool StopFlag;//停止标志位 diff --git a/Project/HC32F460/source/User/Src/main.c b/Project/HC32F460/source/User/Src/main.c index 91cc6bc..764c262 100644 --- a/Project/HC32F460/source/User/Src/main.c +++ b/Project/HC32F460/source/User/Src/main.c @@ -220,12 +220,16 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ PeakValleyCheck ru_K = check_peaks_valleys(App.RightUpCS1237_SlopeArray, SLOPE_CNT); PeakValleyCheck rl_K = check_peaks_valleys(App.RightLowCS1237_SlopeArray, SLOPE_CNT); - int32_t LeftSlopeSum = abs(App.LeftUpCS1237_SlopeArray[0]) + abs(App.LeftLowCS1237_SlopeArray[0]); - int32_t RightSlopeSum = abs(App.RightUpCS1237_SlopeArray[0]) + abs(App.RightLowCS1237_SlopeArray[0]); - int32_t LeftDiff = abs(Oll_diff1) + abs(Olu_diff1); - int32_t RightDiff = abs(Orl_diff1) + abs(Oru_diff1); - int32_t LeftDiffSum = abs(lu_diff[0]) + abs(ll_diff[0]); - int32_t RightDiffSum = abs(ru_diff[0]) + abs(rl_diff[0]); + int32_t LeftSlopeSum = abs(App.LeftUpCS1237_SlopeArray[0]) + abs(App.LeftLowCS1237_SlopeArray[0]) + abs(App.LeftUpCS1237_SlopeArray[1]) + abs(App.LeftLowCS1237_SlopeArray[1]) + abs(App.LeftUpCS1237_SlopeArray[2]) + abs(App.LeftLowCS1237_SlopeArray[2]); + int32_t RightSlopeSum = abs(App.RightUpCS1237_SlopeArray[0]) + abs(App.RightLowCS1237_SlopeArray[0]) + abs(App.RightUpCS1237_SlopeArray[1]) + abs(App.RightLowCS1237_SlopeArray[1]) + abs(App.RightUpCS1237_SlopeArray[2]) + abs(App.RightLowCS1237_SlopeArray[2]); + int32_t LeftDiffSum = abs(lu_diff[0]) + abs(lu_diff[1]) + abs(lu_diff[2]); + int32_t RightDiffSum = abs(ru_diff[0]) + abs(ru_diff[1]) + abs(ru_diff[2]); + int32_t LeftDiff = abs(Oll_diff1) + abs(Olu_diff1) + abs(Oll_diff2) + abs(Olu_diff2) + abs(Oll_diff3) + abs(Olu_diff3); + int32_t RightDiff = abs(Orl_diff1) + abs(Oru_diff1) + abs(Orl_diff2) + abs(Oru_diff2) + abs(Orl_diff3) + abs(Oru_diff3); + int32_t LuSideDiff = lu_ad[0] - lu_ad[5]; + int32_t LlSideDiff = ll_ad[0] - ll_ad[5]; + int32_t RuSideDiff = ru_ad[0] - ru_ad[5]; + int32_t RlSideDiff = rl_ad[0] - rl_ad[5]; if(App.EmeFlag == true) { @@ -282,18 +286,24 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ if(LeftDiff > RightDiff) { App.LeftRunFalg = true; - if((Oll_diff1+Orl_diff1) < -(App.ADTrack.Stop_VPT)) + if((Olu_diff1+Oru_diff1) < -(App.ADTrack.Stop_VPT) + && ((Olu_diff2+Oru_diff2) < -(App.ADTrack.Stop_VPT)) + && ((Olu_diff3+Oru_diff3) < -(App.ADTrack.Stop_VPT))) { BDC_UP(); + App.CaliZeroFlag = false; App.UpFlag = true; App.StopFlag = false; App.DownFlag = false; App.EmeFlag = false; App.RunCollDelay1mSCnt = RUN_TIME_MS; } - if((Oll_diff1+Orl_diff1) > (App.ADTrack.Stop_VPT)) + if((Olu_diff1+Oru_diff1) > (App.ADTrack.Stop_VPT) + && (Olu_diff2+Oru_diff2) > (App.ADTrack.Stop_VPT) + && (Olu_diff3+Oru_diff3) > (App.ADTrack.Stop_VPT)) { BDC_DOWN(); + App.CaliZeroFlag = false; App.UpFlag = false; App.StopFlag = false; App.DownFlag = true; @@ -304,18 +314,24 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ else if(RightDiff > LeftDiff) { App.RightRunFalg = true; - if((Oll_diff1+Orl_diff1) < -(App.ADTrack.Stop_VPT)) + if((Olu_diff1+Oru_diff1) < -(App.ADTrack.Stop_VPT) + && (Olu_diff2+Oru_diff2) < -(App.ADTrack.Stop_VPT) + && (Olu_diff3+Oru_diff3) < -(App.ADTrack.Stop_VPT)) { BDC_UP(); + App.CaliZeroFlag = false; App.UpFlag = true; App.StopFlag = false; App.DownFlag = false; App.EmeFlag = false; App.RunCollDelay1mSCnt = RUN_TIME_MS; } - if((Oll_diff1+Orl_diff1) > (App.ADTrack.Stop_VPT)) + if((Olu_diff1+Oru_diff1) > (App.ADTrack.Stop_VPT) + && (Olu_diff2+Oru_diff2) > (App.ADTrack.Stop_VPT) + && (Olu_diff3+Oru_diff3) > (App.ADTrack.Stop_VPT)) { BDC_DOWN(); + App.CaliZeroFlag = false; App.UpFlag = false; App.StopFlag = false; App.DownFlag = true; @@ -324,123 +340,199 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ } } } - else if((LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum))////判断左边操作 + else if((LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum))//判断左边操作 { App.LeftRunFalg = true; - if(((App.SlopeCollDelay1mSCnt <= 0) - && (App.LeftUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) //判断正面按压 - && (App.LeftUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) - && (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)) - || ((Oll_diff1 <= -(App.ADTrack.LeftLowVPT*5/6)) - && (Oll_diff2 <= -(App.ADTrack.LeftLowVPT*5/6)) - && (Oll_diff3 <= -(App.ADTrack.LeftLowVPT*5/6)) - && (Olu_diff1 >= (App.ADTrack.LeftUpVPT*5/6)) + if(( +// (App.SlopeCollDelay1mSCnt <= 0) +// && (App.LeftUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) //通过斜率判断上下拉方向 +// && (App.LeftUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) +// && (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)) || ( + (Olu_diff1 >= (App.ADTrack.LeftUpVPT*5/6)) //通过差值判断上下拉方向 && (Olu_diff2 >= (App.ADTrack.LeftUpVPT*5/6)) && (Olu_diff3 >= (App.ADTrack.LeftUpVPT*5/6)))) { - if((abs(Oll_diff1) > abs(Orl_diff1) + App.ADTrack.LeftLowShieldVPT)//判断侧面推拉 - && (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT) - && (abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT) - && (abs(Olu_diff1) > abs(Oru_diff1) + App.ADTrack.LeftUpShieldVPT) - ) + if((App.LeftUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压 + && (App.LeftUpCS1237_SlopeArray[1] <= -PRESS_VPT) + && (App.LeftUpCS1237_SlopeArray[2] <= -PRESS_VPT) + && (App.LeftLowCS1237_SlopeArray[0] >= PRESS_VPT) + && (App.LeftLowCS1237_SlopeArray[1] >= PRESS_VPT) + && (App.LeftLowCS1237_SlopeArray[2] >= PRESS_VPT)) { - BDC_UP(); - App.UpFlag = true; - App.StopFlag = false; - App.DownFlag = false; - App.EmeFlag = false; - App.RunCollDelay1mSCnt = RUN_TIME_MS; + if(App.CaliZeroFlag == true)//判断是否校零 + { + if((abs(Oll_diff1) > abs(Orl_diff1) + App.ADTrack.LeftLowShieldVPT)//判断是否不是侧面推拉 + && (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT) + && (abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT) + && (abs(Olu_diff1) > abs(Oru_diff1) + App.ADTrack.LeftUpShieldVPT)) + { + BDC_DOWN(); + App.CaliZeroFlag = false; + App.UpFlag = false; + App.DownFlag = true; + App.StopFlag = false; + App.EmeFlag = false; + App.RunCollDelay1mSCnt = RUN_TIME_MS; + } + } + else + { + BDC_DOWN(); + App.CaliZeroFlag = false; + App.UpFlag = false; + App.DownFlag = true; + App.StopFlag = false; + App.EmeFlag = false; + App.RunCollDelay1mSCnt = RUN_TIME_MS; + } } } - else if(((App.SlopeCollDelay1mSCnt <= 0) - && (App.LeftUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) //判断上下拉方向 - && (App.LeftUpCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) - && (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)) - || ((Oll_diff1 >= (App.ADTrack.LeftLowVPT*5/6)) - && (Oll_diff2 >= (App.ADTrack.LeftLowVPT*5/6)) - && (Oll_diff3 >= (App.ADTrack.LeftLowVPT*5/6)) - && (Olu_diff1 <= -(App.ADTrack.LeftUpVPT*5/6)) + else if(( +// (App.SlopeCollDelay1mSCnt <= 0) +// && (App.LeftUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) //通过斜率判断上下拉方向 +// && (App.LeftUpCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) +// && (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)) || ( + (Olu_diff1 <= -(App.ADTrack.LeftUpVPT*5/6))//通过差值判断上下拉方向 && (Olu_diff2 <= -(App.ADTrack.LeftUpVPT*5/6)) && (Olu_diff3 <= -(App.ADTrack.LeftUpVPT*5/6)))) { - if((abs(Oll_diff1) > abs(Orl_diff1) + App.ADTrack.LeftLowShieldVPT)//判断侧面推拉 - && (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT) - && (abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT) - && (abs(Olu_diff1) > abs(Oru_diff1) + App.ADTrack.LeftUpShieldVPT) - ) + if((App.LeftUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压 + && (App.LeftUpCS1237_SlopeArray[1] >= PRESS_VPT) + && (App.LeftUpCS1237_SlopeArray[2] >= PRESS_VPT) + && (App.LeftLowCS1237_SlopeArray[0] <= -PRESS_VPT) + && (App.LeftLowCS1237_SlopeArray[1] <= -PRESS_VPT) + && (App.LeftLowCS1237_SlopeArray[2] <= -PRESS_VPT)) { - BDC_DOWN(); - App.UpFlag = false; - App.DownFlag = true; - App.StopFlag = false; - App.EmeFlag = false; - App.RunCollDelay1mSCnt = RUN_TIME_MS; + if(App.CaliZeroFlag == true)//判断是否校零 + { + if((abs(Oll_diff1) > abs(Orl_diff1) + App.ADTrack.LeftLowShieldVPT)//判断是否不是侧面推拉 + && (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT) + && (abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT) + && (abs(Olu_diff1) > abs(Oru_diff1) + App.ADTrack.LeftUpShieldVPT)) + { + BDC_UP(); + App.CaliZeroFlag = false; + App.UpFlag = true; + App.DownFlag = false; + App.StopFlag = false; + App.EmeFlag = false; + App.RunCollDelay1mSCnt = RUN_TIME_MS; + } + } + else + { + BDC_UP(); + App.CaliZeroFlag = false; + App.UpFlag = true; + App.DownFlag = false; + App.StopFlag = false; + App.EmeFlag = false; + App.RunCollDelay1mSCnt = RUN_TIME_MS; + } } } } - else if(LeftSlopeSum < RightSlopeSum && LeftDiffSum < RightDiffSum)//上下拉屏幕右边 + else if(LeftSlopeSum < RightSlopeSum && LeftDiffSum < RightDiffSum)//判断右边操作 { App.RightRunFalg = true; - if(((App.SlopeCollDelay1mSCnt <= 0) - && (App.RightUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) //判断上下拉方向 - && (App.RightUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) - && (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)) - || ((Orl_diff1 <= -(App.ADTrack.RightLowVPT*5/6)) - && (Orl_diff2 <= -(App.ADTrack.RightLowVPT*5/6)) - && (Orl_diff3 <= -(App.ADTrack.RightLowVPT*5/6)) - && (Oru_diff1 >= (App.ADTrack.RightUpVPT*5/6)) + if(( +// (App.SlopeCollDelay1mSCnt <= 0) +// && (App.RightUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) //通过斜率判断上下拉方向 +// && (App.RightUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) +// && (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)) || ( + (Oru_diff1 >= (App.ADTrack.RightUpVPT*5/6)) //通过差值判断上下拉方向 && (Oru_diff2 >= (App.ADTrack.RightUpVPT*5/6)) && (Oru_diff3 >= (App.ADTrack.RightUpVPT*5/6)))) { - if((abs(Orl_diff1) > abs(Oll_diff1) + App.ADTrack.RightLowShieldVPT)//判断侧面推拉 - && (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT) - && (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT) - && (abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT) - ) + if((App.RightUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压 + && (App.RightUpCS1237_SlopeArray[1] <= -PRESS_VPT) + && (App.RightUpCS1237_SlopeArray[2] <= -PRESS_VPT) + && (App.RightLowCS1237_SlopeArray[0] >= PRESS_VPT) + && (App.RightLowCS1237_SlopeArray[1] >= PRESS_VPT) + && (App.RightLowCS1237_SlopeArray[2] >= PRESS_VPT)) { - BDC_UP(); - App.UpFlag = true; - App.StopFlag = false; - App.DownFlag = false; - App.EmeFlag = false; - App.RunCollDelay1mSCnt = RUN_TIME_MS; + if(App.CaliZeroFlag == true)//判断是否校零 + { + if((abs(Orl_diff1) > abs(Oll_diff1) + App.ADTrack.RightLowShieldVPT)//判断是否不是侧面推拉 + && (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT) + && (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT) + && (abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT)) + { + BDC_DOWN(); + App.CaliZeroFlag = false; + App.DownFlag = true; + App.StopFlag = false; + App.UpFlag = false; + App.EmeFlag = false; + App.RunCollDelay1mSCnt = RUN_TIME_MS; + } + } + else + { + BDC_DOWN(); + App.CaliZeroFlag = false; + App.DownFlag = true; + App.StopFlag = false; + App.UpFlag = false; + App.EmeFlag = false; + App.RunCollDelay1mSCnt = RUN_TIME_MS; + } } } - else if(((App.SlopeCollDelay1mSCnt <= 0) - && (App.RightUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) //判断上下拉方向 - && (App.RightUpCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) - && (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)) - || ((Orl_diff1 >= (App.ADTrack.RightLowVPT*5/6)) - && (Orl_diff2 >= (App.ADTrack.RightLowVPT*5/6)) - && (Orl_diff3 >= (App.ADTrack.RightLowVPT*5/6)) - && (Oru_diff1 <= -(App.ADTrack.RightUpVPT*5/6)) + else if(( +// (App.SlopeCollDelay1mSCnt <= 0) +// && (App.RightUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) //通过斜率判断上下拉方向 +// && (App.RightUpCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) +// && (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))|| ( + (Oru_diff1 <= -(App.ADTrack.RightUpVPT*5/6)) //通过差值判断上下拉方向 && (Oru_diff2 <= -(App.ADTrack.RightUpVPT*5/6)) && (Oru_diff3 <= -(App.ADTrack.RightUpVPT*5/6)))) { - if((abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT)//判断侧面推拉 - && (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT) - && (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT) - && (abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT) - ) + if((App.RightUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压 + && (App.RightUpCS1237_SlopeArray[1] >= PRESS_VPT) + && (App.RightUpCS1237_SlopeArray[2] >= PRESS_VPT) + && (App.RightLowCS1237_SlopeArray[0] <= -PRESS_VPT) + && (App.RightLowCS1237_SlopeArray[1] <= -PRESS_VPT) + && (App.RightLowCS1237_SlopeArray[2] <= -PRESS_VPT)) { - BDC_DOWN(); - App.DownFlag = true; - App.StopFlag = false; - App.UpFlag = false; - App.EmeFlag = false; - App.RunCollDelay1mSCnt = RUN_TIME_MS; + if(App.CaliZeroFlag == true)//判断是否校零 + { + if((abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT)//判断是否不是侧面推拉 + && (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT) + && (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT) + && (abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT)) + { + BDC_UP(); + App.CaliZeroFlag = false; + App.UpFlag = true; + App.StopFlag = false; + App.DownFlag = false; + App.EmeFlag = false; + App.RunCollDelay1mSCnt = RUN_TIME_MS; + } + } + else + { + BDC_UP(); + App.CaliZeroFlag = false; + App.UpFlag = true; + App.StopFlag = false; + App.DownFlag = false; + App.EmeFlag = false; + App.RunCollDelay1mSCnt = RUN_TIME_MS; + } } } } @@ -449,7 +541,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ { if(App.LeftRunFalg == true) { - if(((App.CentreRunFalg == false ) && (ll_slope > App.ADTrack.Slope_VPT)) + if(((App.CentreRunFalg == false ) && (lu_slope > App.ADTrack.Slope_VPT)) || ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0))) { BDC_STOP(); @@ -461,11 +553,11 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.SlopeCollDelay1mSCnt = SLOPE_START_MS; App.EmergencyDelay1mSCnt = EME_START_MS; } - else if(((App.CentreRunFalg == false ) && (Oll_diff1 < App.ADTrack.Stop_VPT)) + else if(((App.CentreRunFalg == false ) && (Olu_diff1 < App.ADTrack.Stop_VPT) && (Olu_diff2 < App.ADTrack.Stop_VPT) && (Olu_diff3 < App.ADTrack.Stop_VPT)) || ((App.CentreRunFalg == true) - && ((Oll_diff1 + Orl_diff1) < (App.ADTrack.Stop_VPT*2/3)) - && ((Oll_diff2 + Orl_diff2) < (App.ADTrack.Stop_VPT*2/3)) - && ((Oll_diff3 + Orl_diff3) < (App.ADTrack.Stop_VPT*2/3)) + && ((Olu_diff1 + Oru_diff1) < (App.ADTrack.Stop_VPT*2/3)) + && ((Olu_diff2 + Oru_diff2) < (App.ADTrack.Stop_VPT*2/3)) + && ((Olu_diff3 + Oru_diff3) < (App.ADTrack.Stop_VPT*2/3)) )) { BDC_STOP(); @@ -478,12 +570,12 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.EmergencyDelay1mSCnt = EME_START_MS; } else if((abs(Oll_diff1) + EME_STOP_VPT) < abs(Orl_diff1) - && (abs(Oll_diff2) + EME_STOP_VPT) < abs(Orl_diff2) - && (abs(Oll_diff3) + EME_STOP_VPT) < abs(Orl_diff3) - && (abs(Olu_diff1) + EME_STOP_VPT) < abs(Oru_diff1) - && (abs(Olu_diff2) + EME_STOP_VPT) < abs(Oru_diff2) - && (abs(Olu_diff3) + EME_STOP_VPT) < abs(Oru_diff3) - && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false) + && (abs(Oll_diff2) + EME_STOP_VPT) < abs(Orl_diff2) + && (abs(Oll_diff3) + EME_STOP_VPT) < abs(Orl_diff3) + && (abs(Olu_diff1) + EME_STOP_VPT) < abs(Oru_diff1) + && (abs(Olu_diff2) + EME_STOP_VPT) < abs(Oru_diff2) + && (abs(Olu_diff3) + EME_STOP_VPT) < abs(Oru_diff3) + && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false) { BDC_STOP(); App.EmeFlag = true; @@ -492,8 +584,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ } else if(App.RightRunFalg == true) { - 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) && (App.SlopeCollDelay1mSCnt <= 0))) + if(((App.CentreRunFalg == false ) && (ru_slope > App.ADTrack.Slope_VPT)) + || ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0))) { BDC_STOP(); App.StopFlag = true; @@ -504,11 +596,11 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.SlopeCollDelay1mSCnt = SLOPE_START_MS; App.EmergencyDelay1mSCnt = EME_START_MS; } - else if(((App.CentreRunFalg == false ) && (Orl_diff1 < App.ADTrack.Stop_VPT)) + else if(((App.CentreRunFalg == false ) && (Oru_diff1 < App.ADTrack.Stop_VPT) && (Oru_diff2 < App.ADTrack.Stop_VPT) && (Oru_diff3 < App.ADTrack.Stop_VPT)) || ((App.CentreRunFalg == true) - && ((Oll_diff1 + Orl_diff1) < (App.ADTrack.Stop_VPT*2/3)) - && ((Oll_diff2 + Orl_diff2) < (App.ADTrack.Stop_VPT*2/3)) - && ((Oll_diff3 + Orl_diff3) < (App.ADTrack.Stop_VPT*2/3)) + && ((Olu_diff1 + Oru_diff1) < (App.ADTrack.Stop_VPT*2/3)) + && ((Olu_diff2 + Oru_diff2) < (App.ADTrack.Stop_VPT*2/3)) + && ((Olu_diff3 + Oru_diff3) < (App.ADTrack.Stop_VPT*2/3)) )) { BDC_STOP(); @@ -521,12 +613,12 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.EmergencyDelay1mSCnt = EME_START_MS; } else if(abs(Oll_diff1) > (abs(Orl_diff1) + EME_STOP_VPT) - && abs(Oll_diff2) > (abs(Orl_diff2) + EME_STOP_VPT) - && abs(Oll_diff3) > (abs(Orl_diff3) + EME_STOP_VPT) - && abs(Olu_diff1) > (abs(Oru_diff1) + EME_STOP_VPT) - && abs(Olu_diff2) > (abs(Oru_diff2) + EME_STOP_VPT) - && abs(Olu_diff3) > (abs(Oru_diff3) + EME_STOP_VPT) - && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false) + && abs(Oll_diff2) > (abs(Orl_diff2) + EME_STOP_VPT) + && abs(Oll_diff3) > (abs(Orl_diff3) + EME_STOP_VPT) + && abs(Olu_diff1) > (abs(Oru_diff1) + EME_STOP_VPT) + && abs(Olu_diff2) > (abs(Oru_diff2) + EME_STOP_VPT) + && abs(Olu_diff3) > (abs(Oru_diff3) + EME_STOP_VPT) + && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false) { BDC_STOP(); App.EmeFlag = true; @@ -538,7 +630,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ { if(App.LeftRunFalg == true) { - if(((App.CentreRunFalg == false ) && (ll_slope < -App.ADTrack.Slope_VPT)) + if(((App.CentreRunFalg == false ) && (lu_slope < -App.ADTrack.Slope_VPT)) || ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0))) { BDC_STOP(); @@ -550,11 +642,11 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.SlopeCollDelay1mSCnt = SLOPE_START_MS; App.EmergencyDelay1mSCnt = EME_START_MS; } - else if(((App.CentreRunFalg == false ) && (Oll_diff1 > -App.ADTrack.Stop_VPT)) + else if(((App.CentreRunFalg == false ) && (Olu_diff1 > -App.ADTrack.Stop_VPT) && (Olu_diff2 > -App.ADTrack.Stop_VPT) && (Olu_diff3 > -App.ADTrack.Stop_VPT)) || ((App.CentreRunFalg == true) - && ((Oll_diff1 + Orl_diff1) > -(App.ADTrack.Stop_VPT*2/3)) - && ((Oll_diff2 + Orl_diff2) > -(App.ADTrack.Stop_VPT*2/3)) - && ((Oll_diff3 + Orl_diff3) > -(App.ADTrack.Stop_VPT*2/3)) + && ((Olu_diff1 + Oru_diff1) > -(App.ADTrack.Stop_VPT*2/3)) + && ((Olu_diff2 + Oru_diff2) > -(App.ADTrack.Stop_VPT*2/3)) + && ((Olu_diff3 + Oru_diff3) > -(App.ADTrack.Stop_VPT*2/3)) )) { BDC_STOP(); @@ -567,12 +659,12 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.EmergencyDelay1mSCnt = EME_START_MS; } else if((abs(Oll_diff1) + EME_STOP_VPT) < abs(Orl_diff1) - && (abs(Oll_diff2) + EME_STOP_VPT) < abs(Orl_diff2) - && (abs(Oll_diff3) + EME_STOP_VPT) < abs(Orl_diff3) - && (abs(Olu_diff1) + EME_STOP_VPT) < abs(Oru_diff1) - && (abs(Olu_diff2) + EME_STOP_VPT) < abs(Oru_diff2) - && (abs(Olu_diff3) + EME_STOP_VPT) < abs(Oru_diff3) - && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false) + && (abs(Oll_diff2) + EME_STOP_VPT) < abs(Orl_diff2) + && (abs(Oll_diff3) + EME_STOP_VPT) < abs(Orl_diff3) + && (abs(Olu_diff1) + EME_STOP_VPT) < abs(Oru_diff1) + && (abs(Olu_diff2) + EME_STOP_VPT) < abs(Oru_diff2) + && (abs(Olu_diff3) + EME_STOP_VPT) < abs(Oru_diff3) + && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false) { BDC_STOP(); App.EmeFlag = true; @@ -581,7 +673,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ } else if(App.RightRunFalg == true) { - if(((App.CentreRunFalg == false ) && (rl_slope < -App.ADTrack.Slope_VPT)) + if(((App.CentreRunFalg == false ) && (ru_slope < -App.ADTrack.Slope_VPT)) || ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0))) { BDC_STOP(); @@ -593,11 +685,11 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.SlopeCollDelay1mSCnt = SLOPE_START_MS; App.EmergencyDelay1mSCnt = EME_START_MS; } - else if(((App.CentreRunFalg == false ) && (Orl_diff1 > -App.ADTrack.Stop_VPT)) + else if(((App.CentreRunFalg == false ) && (Oru_diff1 > -App.ADTrack.Stop_VPT) && (Oru_diff2 > -App.ADTrack.Stop_VPT) && (Oru_diff3 > -App.ADTrack.Stop_VPT)) || ((App.CentreRunFalg == true) - && ((Oll_diff1 + Orl_diff1) > -(App.ADTrack.Stop_VPT*2/3)) - && ((Oll_diff2 + Orl_diff2) > -(App.ADTrack.Stop_VPT*2/3)) - && ((Oll_diff3 + Orl_diff3) > -(App.ADTrack.Stop_VPT*2/3)) + && ((Olu_diff1 + Oru_diff1) > -(App.ADTrack.Stop_VPT*2/3)) + && ((Olu_diff2 + Oru_diff2) > -(App.ADTrack.Stop_VPT*2/3)) + && ((Olu_diff3 + Oru_diff3) > -(App.ADTrack.Stop_VPT*2/3)) )) { BDC_STOP(); @@ -610,12 +702,12 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.EmergencyDelay1mSCnt = EME_START_MS; } else if(abs(Oll_diff1) > (abs(Orl_diff1) + EME_STOP_VPT) - && abs(Oll_diff2) > (abs(Orl_diff2) + EME_STOP_VPT) - && abs(Oll_diff3) > (abs(Orl_diff3) + EME_STOP_VPT) - && abs(Olu_diff1) > (abs(Oru_diff1) + EME_STOP_VPT) - && abs(Olu_diff2) > (abs(Oru_diff2) + EME_STOP_VPT) - && abs(Olu_diff3) > (abs(Oru_diff3) + EME_STOP_VPT) - && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false) + && abs(Oll_diff2) > (abs(Orl_diff2) + EME_STOP_VPT) + && abs(Oll_diff3) > (abs(Orl_diff3) + EME_STOP_VPT) + && abs(Olu_diff1) > (abs(Oru_diff1) + EME_STOP_VPT) + && abs(Olu_diff2) > (abs(Oru_diff2) + EME_STOP_VPT) + && abs(Olu_diff3) > (abs(Oru_diff3) + EME_STOP_VPT) + && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false) { BDC_STOP(); App.EmeFlag = true; @@ -663,6 +755,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.ADTrack.LeftLow_Org_AdZero = ll_ad[0]; App.ADTrack.RightUp_Org_AdZero = ru_ad[0]; App.ADTrack.RightLow_Org_AdZero = rl_ad[0]; + App.CaliZeroFlag = true; } } } @@ -897,6 +990,8 @@ static void AppLoopHandler(void) if(App.TD1mSDelayCnt <= 0) { App.WaitFlag = true; + App.SideFlag = false; + App.CaliZeroFlag = false; App.UpFlag = false; App.DownFlag = false; App.EmeFlag = false;