From 53897050bcaf1f1217fba86d688049a221b92971 Mon Sep 17 00:00:00 2001 From: "DESKTOP-KQHDCM4\\97555" <975559679@qq.com> Date: Fri, 10 Jul 2026 16:28:43 +0800 Subject: [PATCH] =?UTF-8?q?=E5=B7=A6=E4=B8=8A=E5=92=8C=E5=8F=B3=E4=B8=8B?= =?UTF-8?q?=E4=BC=A0=E6=84=9F=E5=99=A8=E5=8F=8D=E6=8E=A5=EF=BC=8C=E6=8C=89?= =?UTF-8?q?=E5=8E=8B=E5=8F=B3=E4=B8=8A=E8=A7=92=E5=B1=8F=E5=B9=95=E6=97=B6?= =?UTF-8?q?=E4=BC=9A=E8=AF=AF=E8=A7=A6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Project/HC32F460/MDK/HC32F460.uvoptx | 19 +- Project/HC32F460/source/User/Inc/main.h | 2 +- Project/HC32F460/source/User/Src/main.c | 221 ++++++++++++------------ 3 files changed, 129 insertions(+), 113 deletions(-) diff --git a/Project/HC32F460/MDK/HC32F460.uvoptx b/Project/HC32F460/MDK/HC32F460.uvoptx index 5b7904c..470fd12 100644 --- a/Project/HC32F460/MDK/HC32F460.uvoptx +++ b/Project/HC32F460/MDK/HC32F460.uvoptx @@ -153,7 +153,24 @@ UL2CM3(-S0 -C0 -P0 -FD1FFF8000 -FC1000 -FN2 -FF0HC32F460_512K -FS00 -FL080000 -FF1HC32F460_otp -FS13000C00 -FL13FC -FP0($$Device:HC32F460JETA$FlashARM\HC32F460_512K.FLM) -FP1($$Device:HC32F460JETA$FlashARM\HC32F460_otp.FLM)) - + + + 0 + 0 + 504 + 1 +
32426
+ 0 + 0 + 0 + 0 + 0 + 1 + ..\source\User\Src\main.c + + \\BombSensor\../source/User/Src/main.c\504 + + 0 diff --git a/Project/HC32F460/source/User/Inc/main.h b/Project/HC32F460/source/User/Inc/main.h index c8a0986..7106d67 100644 --- a/Project/HC32F460/source/User/Inc/main.h +++ b/Project/HC32F460/source/User/Inc/main.h @@ -11,7 +11,7 @@ #define SLOPE_CNT 20 #define SLOPE_VPT 1000 -#define PRESS_VPT 10 +#define PRESS_VPT 3 #define FOLLOW_VPT 10 #define SHIELD_VPT 2 #define EME_STOP_VPT 4000 diff --git a/Project/HC32F460/source/User/Src/main.c b/Project/HC32F460/source/User/Src/main.c index 3d81cc9..68e6320 100644 --- a/Project/HC32F460/source/User/Src/main.c +++ b/Project/HC32F460/source/User/Src/main.c @@ -266,8 +266,6 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ 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]) + abs(ll_diff[0]) + abs(ll_diff[1]) + abs(ll_diff[2]); int32_t RightDiffSum = abs(ru_diff[0]) + abs(ru_diff[1]) + abs(ru_diff[2]) + abs(rl_diff[0]) + abs(rl_diff[1]) + abs(rl_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]; @@ -327,16 +325,16 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ { App.CentreRunFalg = true; App.CentCollDelay1mSCnt = CENT_TIME_MS; - if(abs(Oll_diff1) > abs(Orl_diff1) - && abs(Oll_diff2) > abs(Orl_diff2) - && abs(Oll_diff3) > abs(Orl_diff3) - && abs(Olu_diff1) > abs(Oru_diff1) - && abs(Olu_diff2) > abs(Oru_diff2) - && abs(Olu_diff3) > abs(Oru_diff3) + if(abs(Oll_diff1) + 100 > abs(Orl_diff1) + && abs(Oll_diff2) + 100 > abs(Orl_diff2) + && abs(Oll_diff3) + 100 > abs(Orl_diff3) + && abs(Olu_diff1) + 100 > abs(Oru_diff1) + && abs(Olu_diff2) + 100 > abs(Oru_diff2) + && abs(Olu_diff3) + 100 > abs(Oru_diff3) )//判断偏左还是偏右 { App.LeftRunFalg = true; - if(((Olu_diff1+Oru_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)) && ((Oll_diff1+Orl_diff1) < -(App.ADTrack.Stop_VPT)) @@ -344,18 +342,18 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && ((Oll_diff3+Orl_diff3) < -(App.ADTrack.Stop_VPT)) ) { -// if(((CompareAndCountAbs(ll_ad,lu_ad,App.aaa,\ -// App.ADTrack.LeftLow_Org_AdZero,App.ADTrack.LeftUp_Org_AdZero,\ -// App.ccc) \ -// + \ -// CompareAndCountAbs(rl_ad,ru_ad,App.aaa,\ -// App.ADTrack.RightLow_Org_AdZero,App.ADTrack.RightUp_Org_AdZero,\ -// App.ccc) -// ) >= App.bbb) -// )//判断是否不是正面压 -// if((abs(Oru_diff1) < abs(Orl_diff1)*2)//判断是否不是正面压 -// && (abs(Oru_diff2) < abs(Orl_diff2)*2) -// && (abs(Oru_diff3) < abs(Orl_diff3)*2)) + 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) + && (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)) { if(App.MotorPara.Current_height < 1195) { @@ -369,7 +367,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ } } } - else if(((Olu_diff1+Oru_diff1) < -(App.ADTrack.Stop_VPT))//判断上下拉方向 + else 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)) && ((Oll_diff1+Orl_diff1) > (App.ADTrack.Stop_VPT)) @@ -377,18 +375,18 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && ((Oll_diff3+Orl_diff3) > (App.ADTrack.Stop_VPT)) ) { -// if(((CompareAndCountAbs(lu_ad,ll_ad,App.aaa,\ -// App.ADTrack.LeftUp_Org_AdZero,App.ADTrack.LeftLow_Org_AdZero,\ -// App.ccc) \ -// + \ -// CompareAndCountAbs(ru_ad,rl_ad,App.aaa,\ -// App.ADTrack.RightUp_Org_AdZero,App.ADTrack.RightLow_Org_AdZero,\ -// App.ccc) -// ) >= App.bbb) -// )//判断是否不是正面压 -// if((abs(Oru_diff1) > abs(Orl_diff1)*2)//判断是否不是正面压 -// && (abs(Oru_diff2) > abs(Orl_diff2)*2) -// && (abs(Oru_diff3) > abs(Orl_diff3)*2)) + 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) + && (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)) { if(App.MotorPara.Current_height > 725) { @@ -403,16 +401,15 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ } } } - else if(abs(Oll_diff1) < abs(Orl_diff1) - && abs(Oll_diff2) < abs(Orl_diff2) - && abs(Oll_diff3) < abs(Orl_diff3) - && abs(Olu_diff1) < abs(Oru_diff1) - && abs(Olu_diff2) < abs(Oru_diff2) - && abs(Olu_diff3) < abs(Oru_diff3) - )//判断偏左还是偏右 + else if(abs(Oll_diff1) + 100 < abs(Orl_diff1)//判断偏左 + && abs(Oll_diff2) < abs(Orl_diff2) + 100 + && abs(Oll_diff3) < abs(Orl_diff3) + 100 + && abs(Olu_diff1) < abs(Oru_diff1) + 100 + && abs(Olu_diff2) < abs(Oru_diff2) + 100 + && abs(Olu_diff3) < abs(Oru_diff3) + 100) { App.RightRunFalg = true; - if(((Olu_diff1+Oru_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)) && ((Oll_diff1+Orl_diff1) < -(App.ADTrack.Stop_VPT)) @@ -420,17 +417,18 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && ((Oll_diff3+Orl_diff3) < -(App.ADTrack.Stop_VPT)) ) { -// if(((//|arr1[i] - zero1| + offset > |arr2[i] - zero2| -// CompareAndCountAbs(ll_ad,lu_ad,App.aaa,\ -// App.ADTrack.LeftLow_Org_AdZero,App.ADTrack.LeftUp_Org_AdZero,\ -// App.ccc) + \ -// CompareAndCountAbs(rl_ad,ru_ad,App.aaa,\ -// App.ADTrack.RightLow_Org_AdZero,App.ADTrack.RightUp_Org_AdZero,\ -// App.ccc)) >= App.bbb) -// )//判断是否不是正面压 -// if((abs(Olu_diff1) < abs(Oll_diff1)*2)//判断是否不是正面压 -// && (abs(Olu_diff2) < abs(Oll_diff2)*2) -// && (abs(Olu_diff3) < abs(Oll_diff3)*2)) + 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) + && (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)) { if(App.MotorPara.Current_height < 1195) { @@ -444,7 +442,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ } } } - else if(((Olu_diff1+Oru_diff1) < -(App.ADTrack.Stop_VPT))//判断上下拉方向 + else 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)) && ((Oll_diff1+Orl_diff1) > (App.ADTrack.Stop_VPT)) @@ -452,17 +450,18 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && ((Oll_diff3+Orl_diff3) > (App.ADTrack.Stop_VPT)) ) { -// if(((//|arr1[i] - zero1| + offset > |arr2[i] - zero2| -// CompareAndCountAbs(lu_ad,ll_ad,App.aaa,\ -// App.ADTrack.LeftUp_Org_AdZero,App.ADTrack.LeftLow_Org_AdZero,\ -// App.ccc) + \ -// CompareAndCountAbs(ru_ad,rl_ad,App.aaa,\ -// App.ADTrack.RightUp_Org_AdZero,App.ADTrack.RightLow_Org_AdZero,\ -// App.ccc)) >= App.bbb) -// )//判断是否不是正面压 -// if((abs(Olu_diff1) > abs(Oll_diff1)*2)//判断是否不是正面压 -// && (abs(Olu_diff2) > abs(Oll_diff2)*2) -// && (abs(Olu_diff3) > abs(Oll_diff3)*2)) + 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) + && (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)) { if(App.MotorPara.Current_height > 725) { @@ -500,7 +499,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (App.RightLowCS1237_SlopeArray[0] <= -PRESS_VPT) && (App.RightLowCS1237_SlopeArray[1] <= -PRESS_VPT) && (App.RightLowCS1237_SlopeArray[2] <= -PRESS_VPT)) - || ((App.LeftUpCS1237_SlopeArray[0] <= -PRESS_VPT)//判断是否不是正面压 + || ((App.LeftUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压 && (App.LeftUpCS1237_SlopeArray[1] <= -PRESS_VPT) && (App.LeftUpCS1237_SlopeArray[2] <= -PRESS_VPT) && (App.LeftLowCS1237_SlopeArray[0] >= PRESS_VPT) @@ -516,13 +515,13 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ { // if(App.CaliZeroFlag == true)//判断是否校零 { - if((abs(Olu_diff1) > abs(Oru_diff1) * SHIELD_VPT)//判断是否不是侧面推拉 - && (abs(Olu_diff2) > abs(Oru_diff2) * SHIELD_VPT) - && (abs(Olu_diff3) > abs(Oru_diff3) * SHIELD_VPT) - && (abs(Oll_diff1) > abs(Orl_diff1) * SHIELD_VPT) - && (abs(Oll_diff3) > abs(Orl_diff2) * SHIELD_VPT) - && (abs(Oll_diff3) > abs(Orl_diff3) * SHIELD_VPT) - ) +// if((abs(Olu_diff1) > abs(Oru_diff1) * SHIELD_VPT)//判断是否不是侧面推拉 +// && (abs(Olu_diff2) > abs(Oru_diff2) * SHIELD_VPT) +// && (abs(Olu_diff3) > abs(Oru_diff3) * SHIELD_VPT) +// && (abs(Oll_diff1) > abs(Orl_diff1) * SHIELD_VPT) +// && (abs(Oll_diff3) > abs(Orl_diff2) * SHIELD_VPT) +// && (abs(Oll_diff3) > abs(Orl_diff3) * SHIELD_VPT) +// ) { if(App.MotorPara.Current_height < 1195) { @@ -552,19 +551,19 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (App.LeftLowCS1237_SlopeArray[0] <= -PRESS_VPT) && (App.LeftLowCS1237_SlopeArray[1] <= -PRESS_VPT) && (App.LeftLowCS1237_SlopeArray[2] <= -PRESS_VPT) - &&(App.RightUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压 + && (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)) - ||((App.LeftUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压 + || ((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) - &&(App.RightUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压 + && (App.RightUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压 && (App.RightUpCS1237_SlopeArray[1] >= PRESS_VPT) && (App.RightUpCS1237_SlopeArray[2] >= PRESS_VPT) && (App.RightLowCS1237_SlopeArray[0] >= PRESS_VPT) @@ -574,13 +573,13 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ { // if(App.CaliZeroFlag == true)//判断是否校零 { - if((abs(Olu_diff1) > abs(Oru_diff1) * SHIELD_VPT)//判断是否不是侧面推拉 - && (abs(Olu_diff2) > abs(Oru_diff2) * SHIELD_VPT) - && (abs(Olu_diff3) > abs(Oru_diff3) * SHIELD_VPT) - && (abs(Oll_diff1) > abs(Orl_diff1) * SHIELD_VPT) - && (abs(Oll_diff3) > abs(Orl_diff2) * SHIELD_VPT) - && (abs(Oll_diff3) > abs(Orl_diff3) * SHIELD_VPT) - ) +// if((abs(Olu_diff1) > abs(Oru_diff1) * SHIELD_VPT)//判断是否不是侧面推拉 +// && (abs(Olu_diff2) > abs(Oru_diff2) * SHIELD_VPT) +// && (abs(Olu_diff3) > abs(Oru_diff3) * SHIELD_VPT) +// && (abs(Oll_diff1) > abs(Orl_diff1) * SHIELD_VPT) +// && (abs(Oll_diff3) > abs(Orl_diff2) * SHIELD_VPT) +// && (abs(Oll_diff3) > abs(Orl_diff3) * SHIELD_VPT) +// ) { if(App.MotorPara.Current_height > 725) { @@ -625,23 +624,23 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (App.RightLowCS1237_SlopeArray[0] >= PRESS_VPT) && (App.RightLowCS1237_SlopeArray[1] >= PRESS_VPT) && (App.RightLowCS1237_SlopeArray[2] >= PRESS_VPT) - && (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)) + && (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)) ) { // if(App.CaliZeroFlag == true)//判断是否校零 { - if((abs(Oru_diff1) > abs(Olu_diff1) * SHIELD_VPT)//判断是否不是侧面推拉 - && (abs(Oru_diff2) > abs(Olu_diff2) * SHIELD_VPT) - && (abs(Oru_diff3) > abs(Olu_diff3) * SHIELD_VPT) - && (abs(Orl_diff1) > abs(Oll_diff1) * SHIELD_VPT) - && (abs(Orl_diff3) > abs(Oll_diff2) * SHIELD_VPT) - && (abs(Orl_diff3) > abs(Oll_diff3) * SHIELD_VPT) - ) +// if((abs(Oru_diff1) > abs(Olu_diff1) * SHIELD_VPT)//判断是否不是侧面推拉 +// && (abs(Oru_diff2) > abs(Olu_diff2) * SHIELD_VPT) +// && (abs(Oru_diff3) > abs(Olu_diff3) * SHIELD_VPT) +// && (abs(Orl_diff1) > abs(Oll_diff1) * SHIELD_VPT) +// && (abs(Orl_diff3) > abs(Oll_diff2) * SHIELD_VPT) +// && (abs(Orl_diff3) > abs(Oll_diff3) * SHIELD_VPT) +// ) { if(App.MotorPara.Current_height < 1195) { @@ -682,23 +681,23 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (App.RightLowCS1237_SlopeArray[0] <= -PRESS_VPT) && (App.RightLowCS1237_SlopeArray[1] <= -PRESS_VPT) && (App.RightLowCS1237_SlopeArray[2] <= -PRESS_VPT) - && (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)) + && (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)) ) { // if(App.CaliZeroFlag == true)//判断是否校零 { - if((abs(Oru_diff1) > abs(Olu_diff1) * SHIELD_VPT)//判断是否不是侧面推拉 - && (abs(Oru_diff2) > abs(Olu_diff2) * SHIELD_VPT) - && (abs(Oru_diff3) > abs(Olu_diff3) * SHIELD_VPT) - && (abs(Orl_diff1) > abs(Oll_diff1) * SHIELD_VPT) - && (abs(Orl_diff3) > abs(Oll_diff2) * SHIELD_VPT) - && (abs(Orl_diff3) > abs(Oll_diff3) * SHIELD_VPT) - ) +// if((abs(Oru_diff1) > abs(Olu_diff1) * SHIELD_VPT)//判断是否不是侧面推拉 +// && (abs(Oru_diff2) > abs(Olu_diff2) * SHIELD_VPT) +// && (abs(Oru_diff3) > abs(Olu_diff3) * SHIELD_VPT) +// && (abs(Orl_diff1) > abs(Oll_diff1) * SHIELD_VPT) +// && (abs(Orl_diff3) > abs(Oll_diff2) * SHIELD_VPT) +// && (abs(Orl_diff3) > abs(Oll_diff3) * SHIELD_VPT) +// ) { if(App.MotorPara.Current_height > 725) { @@ -1105,7 +1104,7 @@ static void AppLoopHandler(void) break;} case APP_STATUS_MOTOR_INIT:{ - Beeper_Start(200,0,1); + Beeper_Start(200,200,2); LED_RED_ON(); LED_GREEN_OFF(); BDC_UP(); @@ -1251,7 +1250,7 @@ static void AppLoopHandler(void) case APP_STATUS_WAIT_STEADY:{ if(App.TD1mSDelayCnt <= 0) { - Beeper_Start(200,200,2); + Beeper_Start(300,0,1); App.SideFlag = false; App.CaliZeroFlag = false; App.UpFlag = false;