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/MDK/HC32F460.uvoptx
+18 -1
View File
@@ -153,7 +153,24 @@
<Name>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))</Name>
</SetRegEntry>
</TargetDriverDllRegistry>
<Breakpoint/>
<Breakpoint>
<Bp>
<Number>0</Number>
<Type>0</Type>
<LineNumber>504</LineNumber>
<EnabledFlag>1</EnabledFlag>
<Address>32426</Address>
<ByteObject>0</ByteObject>
<HtxType>0</HtxType>
<ManyObjects>0</ManyObjects>
<SizeOfObject>0</SizeOfObject>
<BreakByAccess>0</BreakByAccess>
<BreakIfRCount>1</BreakIfRCount>
<Filename>..\source\User\Src\main.c</Filename>
<ExecCommand></ExecCommand>
<Expression>\\BombSensor\../source/User/Src/main.c\504</Expression>
</Bp>
</Breakpoint>
<WatchWindow1>
<Ww>
<count>0</count>
Project/HC32F460/source/User/Inc/main.h
+1 -1
View File
@@ -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
Project/HC32F460/source/User/Src/main.c
+106 -107
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)
{
@@ -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)
{
@@ -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;