YuanHongbin/Acc_Support_29V_HV2.x
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

双柱改良版调试完成

Browse Source
This commit is contained in:
DESKTOP-KQHDCM4\97555
2025-10-30 15:52:37 +08:00
parent 445e67f234
commit cde156a030
3 changed files with 85 additions and 111 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Project/HC32F460/MDK/HC32F460.uvoptx
+7 -22
View File
@@ -103,7 +103,7 @@
<bEvRecOn>1</bEvRecOn> <bEvRecOn>1</bEvRecOn>
<bSchkAxf>0</bSchkAxf> <bSchkAxf>0</bSchkAxf>
<bTchkAxf>0</bTchkAxf> <bTchkAxf>0</bTchkAxf>
<nTsel>3</nTsel> <nTsel>4</nTsel>
<sDll></sDll> <sDll></sDll>
<sDllPa></sDllPa> <sDllPa></sDllPa>
<sDlgDll></sDlgDll> <sDlgDll></sDlgDll>
@@ -114,9 +114,14 @@
<tDlgDll></tDlgDll> <tDlgDll></tDlgDll>
<tDlgPa></tDlgPa> <tDlgPa></tDlgPa>
<tIfile></tIfile> <tIfile></tIfile>
<pMon>BIN\CMSIS_AGDI.dll</pMon> <pMon>Segger\JL2CM3.dll</pMon>
</DebugOpt> </DebugOpt>
<TargetDriverDllRegistry> <TargetDriverDllRegistry>
<SetRegEntry>
<Number>0</Number>
<Key>JL2CM3</Key>
<Name>-U4294967295 -O78 -S2 -ZTIFSpeedSel5000 -A0 -C0 -JU1 -JI127.0.0.1 -JP0 -RST0 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -TB1 -TFE0 -FO15 -FD1FFF8000 -FC1000 -FN2 -FF0HC32F460_512K.FLM -FS00 -FL080000 -FP0($$Device:HC32F460JETA$FlashARM\HC32F460_512K.FLM) -FF1HC32F460_otp.FLM -FS13000C00 -FL13FC -FP1($$Device:HC32F460JETA$FlashARM\HC32F460_otp.FLM)</Name>
</SetRegEntry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>ARMRTXEVENTFLAGS</Key> <Key>ARMRTXEVENTFLAGS</Key>
@@ -210,26 +215,6 @@
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>App.EmergencyDelay1mSCnt,0x0A</ItemText> <ItemText>App.EmergencyDelay1mSCnt,0x0A</ItemText>
</Ww> </Ww>
<Ww>
<count>12</count>
<WinNumber>1</WinNumber>
<ItemText>App.ADTrack.LeftLow_Org_AdZero,0x0A</ItemText>
</Ww>
<Ww>
<count>13</count>
<WinNumber>1</WinNumber>
<ItemText>Oll_diff1,0x0A</ItemText>
</Ww>
<Ww>
<count>14</count>
<WinNumber>1</WinNumber>
<ItemText>Orl_diff1,0x0A</ItemText>
</Ww>
<Ww>
<count>15</count>
<WinNumber>1</WinNumber>
<ItemText>-(App.ADTrack.Stop_VPT),0x0A</ItemText>
</Ww>
</WatchWindow1> </WatchWindow1>
<Tracepoint> <Tracepoint>
<THDelay>0</THDelay> <THDelay>0</THDelay>
Project/HC32F460/source/User/Inc/main.h
+10 -10
View File
@@ -12,24 +12,24 @@
#define SLOPE_VPT 1000 #define SLOPE_VPT 1000
#define FOLLOW_VPT 20 #define FOLLOW_VPT 20
#define SHIELD_VPT 2500 #define SHIELD_VPT 10000
#define EME_STOP_VPT 10000 #define EME_STOP_VPT 4000
#define STOP_TIME_MS 0 #define STOP_TIME_MS 0
#define RUN_TIME_MS 0 #define RUN_TIME_MS 0
#define EME_START_MS 3000 #define EME_START_MS 1000
#define EME_STOP_MS 3000 #define EME_STOP_MS 2000
#define TRAVEL_TIME_MS 30000 #define TRAVEL_TIME_MS 30000
//#define BDC_STOP() (SET_TWIN_BIT0(),SET_TWIN_BIT1()) #define BDC_STOP() (SET_TWIN_BIT0(),SET_TWIN_BIT1())
//#define BDC_UP() (SET_TWIN_BIT0(),CLR_TWIN_BIT1()) #define BDC_UP() (SET_TWIN_BIT0(),CLR_TWIN_BIT1())
//#define BDC_DOWN() (SET_TWIN_BIT1(),CLR_TWIN_BIT0()) #define BDC_DOWN() (SET_TWIN_BIT1(),CLR_TWIN_BIT0())
#define BDC_STOP() Motor_Control(MOTOR_CMD_STOP) //#define BDC_STOP() Motor_Control(MOTOR_CMD_STOP)
#define BDC_UP() Motor_Control(MOTOR_CMD_FORWARD) //#define BDC_UP() Motor_Control(MOTOR_CMD_FORWARD)
#define BDC_DOWN() Motor_Control(MOTOR_CMD_REVERSE) //#define BDC_DOWN() Motor_Control(MOTOR_CMD_REVERSE)
/*控制等级*/ /*控制等级*/
typedef enum { typedef enum {
Project/HC32F460/source/User/Src/main.c
+68 -79
View File
@@ -95,6 +95,19 @@ void GradeControl(ControlLevel_m level)
{ {
switch(level){ switch(level){
case LEVEL_1:{ case LEVEL_1:{
App.ADTrack.Slope_VPT = SLOPE_VPT;
App.ADTrack.LeftUpVPT = 5000;
App.ADTrack.LeftLowVPT = 5000;
App.ADTrack.RightUpVPT = 5000;
App.ADTrack.RightLowVPT = 5000;
App.ADTrack.LeftUpShieldVPT = SHIELD_VPT;
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 5000*4/5;
break;}
case LEVEL_2:{
App.ADTrack.Slope_VPT = SLOPE_VPT; App.ADTrack.Slope_VPT = SLOPE_VPT;
App.ADTrack.LeftUpVPT = 10000; App.ADTrack.LeftUpVPT = 10000;
App.ADTrack.LeftLowVPT = 10000; App.ADTrack.LeftLowVPT = 10000;
@@ -104,10 +117,23 @@ void GradeControl(ControlLevel_m level)
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT; App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT; App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT; App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 10000*4/5;
break;}
case LEVEL_3:{
App.ADTrack.Slope_VPT = SLOPE_VPT;
App.ADTrack.LeftUpVPT = 15000;
App.ADTrack.LeftLowVPT = 15000;
App.ADTrack.RightUpVPT = 15000;
App.ADTrack.RightLowVPT = 15000;
App.ADTrack.LeftUpShieldVPT = SHIELD_VPT;
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 10000; App.ADTrack.Stop_VPT = 10000;
break;} break;}
case LEVEL_2:{ case LEVEL_4:{
App.ADTrack.Slope_VPT = SLOPE_VPT; App.ADTrack.Slope_VPT = SLOPE_VPT;
App.ADTrack.LeftUpVPT = 20000; App.ADTrack.LeftUpVPT = 20000;
App.ADTrack.LeftLowVPT = 20000; App.ADTrack.LeftLowVPT = 20000;
@@ -117,46 +143,20 @@ void GradeControl(ControlLevel_m level)
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT; App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT; App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT; App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 20000; App.ADTrack.Stop_VPT = 15000;
break;}
case LEVEL_3:{
App.ADTrack.Slope_VPT = SLOPE_VPT;
App.ADTrack.LeftUpVPT = 30000;
App.ADTrack.LeftLowVPT = 30000;
App.ADTrack.RightUpVPT = 30000;
App.ADTrack.RightLowVPT = 30000;
App.ADTrack.LeftUpShieldVPT = SHIELD_VPT;
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 30000/2;
break;}
case LEVEL_4:{
App.ADTrack.Slope_VPT = SLOPE_VPT;
App.ADTrack.LeftUpVPT = 40000;
App.ADTrack.LeftLowVPT = 40000;
App.ADTrack.RightUpVPT = 40000;
App.ADTrack.RightLowVPT = 40000;
App.ADTrack.LeftUpShieldVPT = SHIELD_VPT;
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 40000/2;
break;} break;}
case LEVEL_5:{ case LEVEL_5:{
App.ADTrack.Slope_VPT = SLOPE_VPT; App.ADTrack.Slope_VPT = SLOPE_VPT;
App.ADTrack.LeftUpVPT = 50000; App.ADTrack.LeftUpVPT = 25000;
App.ADTrack.LeftLowVPT = 50000; App.ADTrack.LeftLowVPT = 25000;
App.ADTrack.RightUpVPT = 50000; App.ADTrack.RightUpVPT = 25000;
App.ADTrack.RightLowVPT = 50000; App.ADTrack.RightLowVPT = 25000;
App.ADTrack.LeftUpShieldVPT = SHIELD_VPT; App.ADTrack.LeftUpShieldVPT = SHIELD_VPT;
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT; App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT; App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT; App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 50000/2; App.ADTrack.Stop_VPT = 20000;
break;} break;}
} }
} }
@@ -265,8 +265,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
if(abs(Oll_diff1) > abs(Orl_diff1)) if(abs(Oll_diff1) > abs(Orl_diff1))
{ {
App.LeftRunFalg = true; App.LeftRunFalg = true;
// if((App.LeftLowCS1237_SlopeArray[0] > App.ADTrack.Slope_VPT/3))
// {
if((Oll_diff1+Orl_diff1) < -(App.ADTrack.LeftLowVPT/(App.Para.Layout.ControlLevel+1))) if((Oll_diff1+Orl_diff1) < -(App.ADTrack.LeftLowVPT/(App.Para.Layout.ControlLevel+1)))
{ {
BDC_UP(); BDC_UP();
@@ -275,11 +274,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.DownFlag = false; App.DownFlag = false;
App.EmeFlag = false; App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
//App.EmergencyDelay1mSCnt = EME_START_MS; App.EmergencyDelay1mSCnt = EME_START_MS;
} }
// }
// else if((App.LeftLowCS1237_SlopeArray[0] < -App.ADTrack.Slope_VPT/3))
// {
if((Oll_diff1+Orl_diff1) > App.ADTrack.LeftLowVPT/(App.Para.Layout.ControlLevel+1)) if((Oll_diff1+Orl_diff1) > App.ADTrack.LeftLowVPT/(App.Para.Layout.ControlLevel+1))
{ {
BDC_DOWN(); BDC_DOWN();
@@ -288,15 +284,12 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.DownFlag = true; App.DownFlag = true;
App.EmeFlag = false; App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
//App.EmergencyDelay1mSCnt = EME_START_MS; App.EmergencyDelay1mSCnt = EME_START_MS;
} }
// }
} }
else if(abs(Oll_diff1) < abs(Orl_diff1)) else if(abs(Oll_diff1) < abs(Orl_diff1))
{ {
App.RightRunFalg = true; App.RightRunFalg = true;
// if((App.RightLowCS1237_SlopeArray[0] > App.ADTrack.Slope_VPT/3))
// {
if((Oll_diff1+Orl_diff1) < -(App.ADTrack.RightLowVPT/(App.Para.Layout.ControlLevel+1))) if((Oll_diff1+Orl_diff1) < -(App.ADTrack.RightLowVPT/(App.Para.Layout.ControlLevel+1)))
{ {
BDC_UP(); BDC_UP();
@@ -305,11 +298,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.DownFlag = false; App.DownFlag = false;
App.EmeFlag = false; App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
//App.EmergencyDelay1mSCnt = EME_START_MS; App.EmergencyDelay1mSCnt = EME_START_MS;
} }
// }
// else if((App.RightLowCS1237_SlopeArray[0] < -App.ADTrack.Slope_VPT/3))
// {
if((Oll_diff1+Orl_diff1) > App.ADTrack.RightLowVPT/(App.Para.Layout.ControlLevel+1)) if((Oll_diff1+Orl_diff1) > App.ADTrack.RightLowVPT/(App.Para.Layout.ControlLevel+1))
{ {
BDC_DOWN(); BDC_DOWN();
@@ -318,9 +308,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.DownFlag = true; App.DownFlag = true;
App.EmeFlag = false; App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
//App.EmergencyDelay1mSCnt = EME_START_MS; App.EmergencyDelay1mSCnt = EME_START_MS;
} }
// }
} }
} }
else if((LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum)) else if((LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum))
@@ -333,15 +322,15 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (App.LeftLowCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) && (App.LeftLowCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)) && (App.LeftLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT))
|| ( || (
(Olu_diff1 >= App.ADTrack.LeftUpVPT) (Olu_diff1 >= App.ADTrack.LeftUpVPT*5/6)
&& (Olu_diff2 >= App.ADTrack.LeftUpVPT) && (Olu_diff2 >= App.ADTrack.LeftUpVPT*5/6)
&& (Olu_diff3 >= App.ADTrack.LeftUpVPT) && (Olu_diff3 >= App.ADTrack.LeftUpVPT*5/6)
&& (Oll_diff1 <= -App.ADTrack.LeftLowVPT) && (Oll_diff1 <= -(App.ADTrack.LeftLowVPT*5/6))
&& (Oll_diff2 <= -App.ADTrack.LeftLowVPT) && (Oll_diff2 <= -(App.ADTrack.LeftLowVPT*5/6))
&& (Oll_diff3 <= -App.ADTrack.LeftLowVPT))) && (Oll_diff3 <= -(App.ADTrack.LeftLowVPT*5/6))))
{ {
if((abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT) if((abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT)
// && (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT) && (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT)
) )
{ {
BDC_UP(); BDC_UP();
@@ -350,7 +339,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.DownFlag = false; App.DownFlag = false;
App.EmeFlag = false; App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
//App.EmergencyDelay1mSCnt = EME_START_MS; App.EmergencyDelay1mSCnt = EME_START_MS;
} }
} }
else if(((App.LeftUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) else if(((App.LeftUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
@@ -368,7 +357,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (Oll_diff1 >= App.ADTrack.LeftLowVPT))) && (Oll_diff1 >= App.ADTrack.LeftLowVPT)))
{ {
if((abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT) if((abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT)
// && (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT) && (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT)
) )
{ {
BDC_DOWN(); BDC_DOWN();
@@ -377,7 +366,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.StopFlag = false; App.StopFlag = false;
App.EmeFlag = false; App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
//App.EmergencyDelay1mSCnt = EME_START_MS; App.EmergencyDelay1mSCnt = EME_START_MS;
} }
} }
} }
@@ -390,12 +379,12 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (App.RightLowCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) && (App.RightLowCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
&& (App.RightLowCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) && (App.RightLowCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT)
&& (App.RightLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)) && (App.RightLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT))
|| ((Oru_diff1 >= App.ADTrack.RightUpVPT) || ((Oru_diff1 >= App.ADTrack.RightUpVPT*5/6)
&& (Oru_diff2 >= App.ADTrack.RightUpVPT) && (Oru_diff2 >= App.ADTrack.RightUpVPT*5/6)
&& (Oru_diff3 >= App.ADTrack.RightUpVPT) && (Oru_diff3 >= App.ADTrack.RightUpVPT*5/6)
&& (Orl_diff1 <= -App.ADTrack.RightLowVPT) && (Orl_diff1 <= -(App.ADTrack.RightLowVPT*5/6))
&& (Orl_diff2 <= -App.ADTrack.RightLowVPT) && (Orl_diff2 <= -(App.ADTrack.RightLowVPT*5/6))
&& (Orl_diff3 <= -App.ADTrack.RightLowVPT))) && (Orl_diff3 <= -(App.ADTrack.RightLowVPT*5/6))))
{ {
if((abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT) if((abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT)
&& (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT) && (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT)
@@ -407,7 +396,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.DownFlag = false; App.DownFlag = false;
App.EmeFlag = false; App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
//App.EmergencyDelay1mSCnt = EME_START_MS; App.EmergencyDelay1mSCnt = EME_START_MS;
} }
} }
else if(((App.RightUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) else if(((App.RightUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
@@ -434,7 +423,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.UpFlag = false; App.UpFlag = false;
App.EmeFlag = false; App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
//App.EmergencyDelay1mSCnt = EME_START_MS; App.EmergencyDelay1mSCnt = EME_START_MS;
} }
} }
} }
@@ -465,9 +454,9 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.StopCollDelay1mSCnt = STOP_TIME_MS; App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS; App.EmergencyDelay1mSCnt = EME_START_MS;
} }
else if((abs(Oll_diff1) + EME_STOP_VPT) < abs(Orl_diff1) else if((abs(Oll_diff1) - EME_STOP_VPT) < abs(Orl_diff1)
&& (abs(Oll_diff2) + EME_STOP_VPT) < abs(Orl_diff2) && (abs(Oll_diff2) - EME_STOP_VPT) < abs(Orl_diff2)
&& (abs(Oll_diff3) + EME_STOP_VPT) < abs(Orl_diff3) && (abs(Oll_diff3) - EME_STOP_VPT) < abs(Orl_diff3)
&& App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false) && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
{ {
BDC_STOP(); BDC_STOP();
@@ -499,9 +488,9 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.StopCollDelay1mSCnt = STOP_TIME_MS; App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS; App.EmergencyDelay1mSCnt = EME_START_MS;
} }
else if(abs(Oll_diff1) > (abs(Orl_diff1) + EME_STOP_VPT) else if(abs(Oll_diff1) > (abs(Orl_diff1) - EME_STOP_VPT)
&& abs(Oll_diff2) > (abs(Orl_diff2) + EME_STOP_VPT) && abs(Oll_diff2) > (abs(Orl_diff2) - EME_STOP_VPT)
&& abs(Oll_diff3) > (abs(Orl_diff3) + EME_STOP_VPT) && abs(Oll_diff3) > (abs(Orl_diff3) - EME_STOP_VPT)
&& App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false) && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
{ {
BDC_STOP(); BDC_STOP();
@@ -536,9 +525,9 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.StopCollDelay1mSCnt = STOP_TIME_MS; App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS; App.EmergencyDelay1mSCnt = EME_START_MS;
} }
else if((abs(Oll_diff1) + EME_STOP_VPT) < abs(Orl_diff1) else if((abs(Oll_diff1) - EME_STOP_VPT) < abs(Orl_diff1)
&& (abs(Oll_diff2) + EME_STOP_VPT) < abs(Orl_diff2) && (abs(Oll_diff2) - EME_STOP_VPT) < abs(Orl_diff2)
&& (abs(Oll_diff3) + EME_STOP_VPT) < abs(Orl_diff3) && (abs(Oll_diff3) - EME_STOP_VPT) < abs(Orl_diff3)
&& App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false) && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
{ {
BDC_STOP(); BDC_STOP();
@@ -570,9 +559,9 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.StopCollDelay1mSCnt = STOP_TIME_MS; App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS; App.EmergencyDelay1mSCnt = EME_START_MS;
} }
else if(abs(Oll_diff1) > (abs(Orl_diff1) + EME_STOP_VPT) else if(abs(Oll_diff1) > (abs(Orl_diff1) - EME_STOP_VPT)
&& abs(Oll_diff2) > (abs(Orl_diff2) + EME_STOP_VPT) && abs(Oll_diff2) > (abs(Orl_diff2) - EME_STOP_VPT)
&& abs(Oll_diff3) > (abs(Orl_diff3) + EME_STOP_VPT) && abs(Oll_diff3) > (abs(Orl_diff3) - EME_STOP_VPT)
&& App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false) && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
{ {
BDC_STOP(); BDC_STOP();