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> <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> </SetRegEntry>
</TargetDriverDllRegistry> </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> <WatchWindow1>
<Ww> <Ww>
<count>0</count> <count>0</count>
Project/HC32F460/source/User/Inc/main.h
+1 -1
View File
@@ -11,7 +11,7 @@
#define SLOPE_CNT 20 #define SLOPE_CNT 20
#define SLOPE_VPT 1000 #define SLOPE_VPT 1000
#define PRESS_VPT 10 #define PRESS_VPT 3
#define FOLLOW_VPT 10 #define FOLLOW_VPT 10
#define SHIELD_VPT 2 #define SHIELD_VPT 2
#define EME_STOP_VPT 4000 #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 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 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 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 LuSideDiff = lu_ad[0] - lu_ad[5];
int32_t LlSideDiff = ll_ad[0] - ll_ad[5]; int32_t LlSideDiff = ll_ad[0] - ll_ad[5];
int32_t RuSideDiff = ru_ad[0] - ru_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.CentreRunFalg = true;
App.CentCollDelay1mSCnt = CENT_TIME_MS; App.CentCollDelay1mSCnt = CENT_TIME_MS;
if(abs(Oll_diff1) > abs(Orl_diff1) if(abs(Oll_diff1) + 100 > abs(Orl_diff1)
&& abs(Oll_diff2) > abs(Orl_diff2) && abs(Oll_diff2) + 100 > abs(Orl_diff2)
&& abs(Oll_diff3) > abs(Orl_diff3) && abs(Oll_diff3) + 100 > abs(Orl_diff3)
&& abs(Olu_diff1) > abs(Oru_diff1) && abs(Olu_diff1) + 100 > abs(Oru_diff1)
&& abs(Olu_diff2) > abs(Oru_diff2) && abs(Olu_diff2) + 100 > abs(Oru_diff2)
&& abs(Olu_diff3) > abs(Oru_diff3) && abs(Olu_diff3) + 100 > abs(Oru_diff3)
)//判断偏左还是偏右 )//判断偏左还是偏右
{ {
App.LeftRunFalg = true; 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_diff2+Oru_diff2) > (App.ADTrack.Stop_VPT))
&& ((Olu_diff3+Oru_diff3) > (App.ADTrack.Stop_VPT)) && ((Olu_diff3+Oru_diff3) > (App.ADTrack.Stop_VPT))
&& ((Oll_diff1+Orl_diff1) < -(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)) && ((Oll_diff3+Orl_diff3) < -(App.ADTrack.Stop_VPT))
) )
{ {
// if(((CompareAndCountAbs(ll_ad,lu_ad,App.aaa,\ if((App.RightUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
// App.ADTrack.LeftLow_Org_AdZero,App.ADTrack.LeftUp_Org_AdZero,\ && (App.RightUpCS1237_SlopeArray[1] <= -PRESS_VPT)
// App.ccc) \ && (App.RightUpCS1237_SlopeArray[2] <= -PRESS_VPT)
// + \ && (App.RightLowCS1237_SlopeArray[0] >= PRESS_VPT)
// CompareAndCountAbs(rl_ad,ru_ad,App.aaa,\ && (App.RightLowCS1237_SlopeArray[1] >= PRESS_VPT)
// App.ADTrack.RightLow_Org_AdZero,App.ADTrack.RightUp_Org_AdZero,\ && (App.RightLowCS1237_SlopeArray[2] >= PRESS_VPT)
// App.ccc) && (App.LeftUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
// ) >= App.bbb) && (App.LeftUpCS1237_SlopeArray[1] <= -PRESS_VPT)
// )//判断是否不是正面压 && (App.LeftUpCS1237_SlopeArray[2] <= -PRESS_VPT)
// if((abs(Oru_diff1) < abs(Orl_diff1)*2)//判断是否不是正面压 && (App.LeftLowCS1237_SlopeArray[0] >= PRESS_VPT)
// && (abs(Oru_diff2) < abs(Orl_diff2)*2) && (App.LeftLowCS1237_SlopeArray[1] >= PRESS_VPT)
// && (abs(Oru_diff3) < abs(Orl_diff3)*2)) && (App.LeftLowCS1237_SlopeArray[2] >= PRESS_VPT))
{ {
if(App.MotorPara.Current_height < 1195) 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_diff2+Oru_diff2) < -(App.ADTrack.Stop_VPT))
&& ((Olu_diff3+Oru_diff3) < -(App.ADTrack.Stop_VPT)) && ((Olu_diff3+Oru_diff3) < -(App.ADTrack.Stop_VPT))
&& ((Oll_diff1+Orl_diff1) > (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)) && ((Oll_diff3+Orl_diff3) > (App.ADTrack.Stop_VPT))
) )
{ {
// if(((CompareAndCountAbs(lu_ad,ll_ad,App.aaa,\ if((App.RightUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
// App.ADTrack.LeftUp_Org_AdZero,App.ADTrack.LeftLow_Org_AdZero,\ && (App.RightUpCS1237_SlopeArray[1] >= PRESS_VPT)
// App.ccc) \ && (App.RightUpCS1237_SlopeArray[2] >= PRESS_VPT)
// + \ && (App.RightLowCS1237_SlopeArray[0] <= -PRESS_VPT)
// CompareAndCountAbs(ru_ad,rl_ad,App.aaa,\ && (App.RightLowCS1237_SlopeArray[1] <= -PRESS_VPT)
// App.ADTrack.RightUp_Org_AdZero,App.ADTrack.RightLow_Org_AdZero,\ && (App.RightLowCS1237_SlopeArray[2] <= -PRESS_VPT)
// App.ccc) && (App.LeftUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
// ) >= App.bbb) && (App.LeftUpCS1237_SlopeArray[1] >= PRESS_VPT)
// )//判断是否不是正面压 && (App.LeftUpCS1237_SlopeArray[2] >= PRESS_VPT)
// if((abs(Oru_diff1) > abs(Orl_diff1)*2)//判断是否不是正面压 && (App.LeftLowCS1237_SlopeArray[0] <= -PRESS_VPT)
// && (abs(Oru_diff2) > abs(Orl_diff2)*2) && (App.LeftLowCS1237_SlopeArray[1] <= -PRESS_VPT)
// && (abs(Oru_diff3) > abs(Orl_diff3)*2)) && (App.LeftLowCS1237_SlopeArray[2] <= -PRESS_VPT))
{ {
if(App.MotorPara.Current_height > 725) 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) else if(abs(Oll_diff1) + 100 < abs(Orl_diff1)//判断偏左
&& abs(Oll_diff2) < abs(Orl_diff2) && abs(Oll_diff2) < abs(Orl_diff2) + 100
&& abs(Oll_diff3) < abs(Orl_diff3) && abs(Oll_diff3) < abs(Orl_diff3) + 100
&& abs(Olu_diff1) < abs(Oru_diff1) && abs(Olu_diff1) < abs(Oru_diff1) + 100
&& abs(Olu_diff2) < abs(Oru_diff2) && abs(Olu_diff2) < abs(Oru_diff2) + 100
&& abs(Olu_diff3) < abs(Oru_diff3) && abs(Olu_diff3) < abs(Oru_diff3) + 100)
)//判断偏左还是偏右
{ {
App.RightRunFalg = true; 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_diff2+Oru_diff2) > (App.ADTrack.Stop_VPT))
&& ((Olu_diff3+Oru_diff3) > (App.ADTrack.Stop_VPT)) && ((Olu_diff3+Oru_diff3) > (App.ADTrack.Stop_VPT))
&& ((Oll_diff1+Orl_diff1) < -(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)) && ((Oll_diff3+Orl_diff3) < -(App.ADTrack.Stop_VPT))
) )
{ {
// if(((//|arr1[i] - zero1| + offset > |arr2[i] - zero2| if((App.RightUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
// CompareAndCountAbs(ll_ad,lu_ad,App.aaa,\ && (App.RightUpCS1237_SlopeArray[1] <= -PRESS_VPT)
// App.ADTrack.LeftLow_Org_AdZero,App.ADTrack.LeftUp_Org_AdZero,\ && (App.RightUpCS1237_SlopeArray[2] <= -PRESS_VPT)
// App.ccc) + \ && (App.RightLowCS1237_SlopeArray[0] >= PRESS_VPT)
// CompareAndCountAbs(rl_ad,ru_ad,App.aaa,\ && (App.RightLowCS1237_SlopeArray[1] >= PRESS_VPT)
// App.ADTrack.RightLow_Org_AdZero,App.ADTrack.RightUp_Org_AdZero,\ && (App.RightLowCS1237_SlopeArray[2] >= PRESS_VPT)
// App.ccc)) >= App.bbb) && (App.LeftUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
// )//判断是否不是正面压 && (App.LeftUpCS1237_SlopeArray[1] <= -PRESS_VPT)
// if((abs(Olu_diff1) < abs(Oll_diff1)*2)//判断是否不是正面压 && (App.LeftUpCS1237_SlopeArray[2] <= -PRESS_VPT)
// && (abs(Olu_diff2) < abs(Oll_diff2)*2) && (App.LeftLowCS1237_SlopeArray[0] >= PRESS_VPT)
// && (abs(Olu_diff3) < abs(Oll_diff3)*2)) && (App.LeftLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] >= PRESS_VPT))
{ {
if(App.MotorPara.Current_height < 1195) 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_diff2+Oru_diff2) < -(App.ADTrack.Stop_VPT))
&& ((Olu_diff3+Oru_diff3) < -(App.ADTrack.Stop_VPT)) && ((Olu_diff3+Oru_diff3) < -(App.ADTrack.Stop_VPT))
&& ((Oll_diff1+Orl_diff1) > (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)) && ((Oll_diff3+Orl_diff3) > (App.ADTrack.Stop_VPT))
) )
{ {
// if(((//|arr1[i] - zero1| + offset > |arr2[i] - zero2| if((App.RightUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
// CompareAndCountAbs(lu_ad,ll_ad,App.aaa,\ && (App.RightUpCS1237_SlopeArray[1] >= PRESS_VPT)
// App.ADTrack.LeftUp_Org_AdZero,App.ADTrack.LeftLow_Org_AdZero,\ && (App.RightUpCS1237_SlopeArray[2] >= PRESS_VPT)
// App.ccc) + \ && (App.RightLowCS1237_SlopeArray[0] <= -PRESS_VPT)
// CompareAndCountAbs(ru_ad,rl_ad,App.aaa,\ && (App.RightLowCS1237_SlopeArray[1] <= -PRESS_VPT)
// App.ADTrack.RightUp_Org_AdZero,App.ADTrack.RightLow_Org_AdZero,\ && (App.RightLowCS1237_SlopeArray[2] <= -PRESS_VPT)
// App.ccc)) >= App.bbb) && (App.LeftUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
// )//判断是否不是正面压 && (App.LeftUpCS1237_SlopeArray[1] >= PRESS_VPT)
// if((abs(Olu_diff1) > abs(Oll_diff1)*2)//判断是否不是正面压 && (App.LeftUpCS1237_SlopeArray[2] >= PRESS_VPT)
// && (abs(Olu_diff2) > abs(Oll_diff2)*2) && (App.LeftLowCS1237_SlopeArray[0] <= -PRESS_VPT)
// && (abs(Olu_diff3) > abs(Oll_diff3)*2)) && (App.LeftLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] <= -PRESS_VPT))
{ {
if(App.MotorPara.Current_height > 725) 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(App.CaliZeroFlag == true)//判断是否校零
{ {
if((abs(Olu_diff1) > abs(Oru_diff1) * SHIELD_VPT)//判断是否不是侧面推拉 // if((abs(Olu_diff1) > abs(Oru_diff1) * SHIELD_VPT)//判断是否不是侧面推拉
&& (abs(Olu_diff2) > abs(Oru_diff2) * SHIELD_VPT) // && (abs(Olu_diff2) > abs(Oru_diff2) * SHIELD_VPT)
&& (abs(Olu_diff3) > abs(Oru_diff3) * SHIELD_VPT) // && (abs(Olu_diff3) > abs(Oru_diff3) * SHIELD_VPT)
&& (abs(Oll_diff1) > abs(Orl_diff1) * SHIELD_VPT) // && (abs(Oll_diff1) > abs(Orl_diff1) * SHIELD_VPT)
&& (abs(Oll_diff3) > abs(Orl_diff2) * SHIELD_VPT) // && (abs(Oll_diff3) > abs(Orl_diff2) * SHIELD_VPT)
&& (abs(Oll_diff3) > abs(Orl_diff3) * SHIELD_VPT) // && (abs(Oll_diff3) > abs(Orl_diff3) * SHIELD_VPT)
) // )
{ {
if(App.MotorPara.Current_height < 1195) 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(App.CaliZeroFlag == true)//判断是否校零
{ {
if((abs(Olu_diff1) > abs(Oru_diff1) * SHIELD_VPT)//判断是否不是侧面推拉 // if((abs(Olu_diff1) > abs(Oru_diff1) * SHIELD_VPT)//判断是否不是侧面推拉
&& (abs(Olu_diff2) > abs(Oru_diff2) * SHIELD_VPT) // && (abs(Olu_diff2) > abs(Oru_diff2) * SHIELD_VPT)
&& (abs(Olu_diff3) > abs(Oru_diff3) * SHIELD_VPT) // && (abs(Olu_diff3) > abs(Oru_diff3) * SHIELD_VPT)
&& (abs(Oll_diff1) > abs(Orl_diff1) * SHIELD_VPT) // && (abs(Oll_diff1) > abs(Orl_diff1) * SHIELD_VPT)
&& (abs(Oll_diff3) > abs(Orl_diff2) * SHIELD_VPT) // && (abs(Oll_diff3) > abs(Orl_diff2) * SHIELD_VPT)
&& (abs(Oll_diff3) > abs(Orl_diff3) * SHIELD_VPT) // && (abs(Oll_diff3) > abs(Orl_diff3) * SHIELD_VPT)
) // )
{ {
if(App.MotorPara.Current_height > 725) 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[0] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[1] >= PRESS_VPT) && (App.RightLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] >= 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[1] >= PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] <= -PRESS_VPT) && (App.LeftUpCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] <= -PRESS_VPT) && (App.LeftLowCS1237_SlopeArray[0] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] <= -PRESS_VPT) && (App.LeftLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] <= -PRESS_VPT)) && (App.LeftLowCS1237_SlopeArray[2] >= PRESS_VPT))
) )
{ {
// if(App.CaliZeroFlag == true)//判断是否校零 // if(App.CaliZeroFlag == true)//判断是否校零
{ {
if((abs(Oru_diff1) > abs(Olu_diff1) * SHIELD_VPT)//判断是否不是侧面推拉 // if((abs(Oru_diff1) > abs(Olu_diff1) * SHIELD_VPT)//判断是否不是侧面推拉
&& (abs(Oru_diff2) > abs(Olu_diff2) * SHIELD_VPT) // && (abs(Oru_diff2) > abs(Olu_diff2) * SHIELD_VPT)
&& (abs(Oru_diff3) > abs(Olu_diff3) * SHIELD_VPT) // && (abs(Oru_diff3) > abs(Olu_diff3) * SHIELD_VPT)
&& (abs(Orl_diff1) > abs(Oll_diff1) * SHIELD_VPT) // && (abs(Orl_diff1) > abs(Oll_diff1) * SHIELD_VPT)
&& (abs(Orl_diff3) > abs(Oll_diff2) * SHIELD_VPT) // && (abs(Orl_diff3) > abs(Oll_diff2) * SHIELD_VPT)
&& (abs(Orl_diff3) > abs(Oll_diff3) * SHIELD_VPT) // && (abs(Orl_diff3) > abs(Oll_diff3) * SHIELD_VPT)
) // )
{ {
if(App.MotorPara.Current_height < 1195) 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[0] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[1] <= -PRESS_VPT) && (App.RightLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] <= -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[1] <= -PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] >= PRESS_VPT) && (App.LeftUpCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] >= PRESS_VPT) && (App.LeftLowCS1237_SlopeArray[0] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] >= PRESS_VPT) && (App.LeftLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] >= PRESS_VPT)) && (App.LeftLowCS1237_SlopeArray[2] <= -PRESS_VPT))
) )
{ {
// if(App.CaliZeroFlag == true)//判断是否校零 // if(App.CaliZeroFlag == true)//判断是否校零
{ {
if((abs(Oru_diff1) > abs(Olu_diff1) * SHIELD_VPT)//判断是否不是侧面推拉 // if((abs(Oru_diff1) > abs(Olu_diff1) * SHIELD_VPT)//判断是否不是侧面推拉
&& (abs(Oru_diff2) > abs(Olu_diff2) * SHIELD_VPT) // && (abs(Oru_diff2) > abs(Olu_diff2) * SHIELD_VPT)
&& (abs(Oru_diff3) > abs(Olu_diff3) * SHIELD_VPT) // && (abs(Oru_diff3) > abs(Olu_diff3) * SHIELD_VPT)
&& (abs(Orl_diff1) > abs(Oll_diff1) * SHIELD_VPT) // && (abs(Orl_diff1) > abs(Oll_diff1) * SHIELD_VPT)
&& (abs(Orl_diff3) > abs(Oll_diff2) * SHIELD_VPT) // && (abs(Orl_diff3) > abs(Oll_diff2) * SHIELD_VPT)
&& (abs(Orl_diff3) > abs(Oll_diff3) * SHIELD_VPT) // && (abs(Orl_diff3) > abs(Oll_diff3) * SHIELD_VPT)
) // )
{ {
if(App.MotorPara.Current_height > 725) if(App.MotorPara.Current_height > 725)
{ {
@@ -1105,7 +1104,7 @@ static void AppLoopHandler(void)
break;} break;}
case APP_STATUS_MOTOR_INIT:{ case APP_STATUS_MOTOR_INIT:{
Beeper_Start(200,0,1); Beeper_Start(200,200,2);
LED_RED_ON(); LED_RED_ON();
LED_GREEN_OFF(); LED_GREEN_OFF();
BDC_UP(); BDC_UP();
@@ -1251,7 +1250,7 @@ static void AppLoopHandler(void)
case APP_STATUS_WAIT_STEADY:{ case APP_STATUS_WAIT_STEADY:{
if(App.TD1mSDelayCnt <= 0) if(App.TD1mSDelayCnt <= 0)
{ {
Beeper_Start(200,200,2); Beeper_Start(300,0,1);
App.SideFlag = false; App.SideFlag = false;
App.CaliZeroFlag = false; App.CaliZeroFlag = false;
App.UpFlag = false; App.UpFlag = false;