YuanHongbin/DoubleColumn_Trolley
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

左上和右下传感器反接，按压右上角屏幕时会误触

Browse Source
This commit is contained in:
DESKTOP-KQHDCM4\97555
2026-07-10 16:28:43 +08:00
parent 59d5c62b12
commit 53897050bc
3 changed files with 129 additions and 113 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Project/HC32F460/source/User/Src/main.c
+110 -111
View File
@@ -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;