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
2026-02-27 15:11:26 +08:00
parent 89939cf1e1
commit 4b1eb44562
5 changed files with 178 additions and 116 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Project/HC32F460/MDK/HC32F460.uvoptx
+8 -3
View File
@@ -120,7 +120,7 @@
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>JL2CM3</Key> <Key>JL2CM3</Key>
<Name>-U2147483647 -O78 -S2 -ZTIFSpeedSel5000 -A0 -C0 -JU1 -JI127.0.0.1 -JP0 -RST0 -TO18 -TC10000000 -TP21 -TDS8027 -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> <Name>-U2147483647 -O78 -S2 -ZTIFSpeedSel5000 -A0 -C0 -JU1 -JI127.0.0.1 -JP0 -RST0 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8027 -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> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
@@ -213,12 +213,17 @@
<Ww> <Ww>
<count>11</count> <count>11</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>App.EmergencyDelay1mSCnt,0x0A</ItemText> <ItemText>App.StopCollDelay1mSCnt,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>12</count> <count>12</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>App.ADTrack.LeftLowShieldVPT,0x0A</ItemText> <ItemText>App.SlopeCollDelay1mSCnt,0x0A</ItemText>
</Ww>
<Ww>
<count>13</count>
<WinNumber>1</WinNumber>
<ItemText>App.EmergencyDelay1mSCnt,0x0A</ItemText>
</Ww> </Ww>
</WatchWindow1> </WatchWindow1>
<Tracepoint> <Tracepoint>
Project/HC32F460/source/Module/dc_motor/dc_motor.c
+5 -5
View File
@@ -358,10 +358,10 @@ void AdcLoopHander(void)
DMA_Cmd(M4_DMA1, Disable); DMA_Cmd(M4_DMA1, Disable);
calc_adc_val(g_adc_val); /* 计算ADC的平均值 */ calc_adc_val(g_adc_val); /* 计算ADC的平均值 */
DCMOTOR_DBG_LOG("ADC1_IN1 value %d.\t Temp:%.1fC \r\n", g_adc_val[0u], get_temp((float)g_adc_val[0]*Adc)); // DCMOTOR_DBG_LOG("ADC1_IN1 value %d.\t Temp:%.1fC \r\n", g_adc_val[0u], get_temp((float)g_adc_val[0]*Adc));
DCMOTOR_DBG_LOG("ADC2_IN2 value %d.\t Valtage:%.1fV \r\n", g_adc_val[1u], (float)g_adc_val[1]*ADC2VBUS); // DCMOTOR_DBG_LOG("ADC2_IN2 value %d.\t Valtage:%.1fV \r\n", g_adc_val[1u], (float)g_adc_val[1]*ADC2VBUS);
temp_c = (g_adc_val[2]-init_adc_val)*ADC2CURT; // temp_c = (g_adc_val[2]-init_adc_val)*ADC2CURT;
DCMOTOR_DBG_LOG("ADC3_IN3 value %d.\t Current:%.1fmA \r\n", g_adc_val[2u], temp_c); // DCMOTOR_DBG_LOG("ADC3_IN3 value %d.\t Current:%.1fmA \r\n", g_adc_val[2u], temp_c);
// g_motor_data.current = temp_c; // g_motor_data.current = temp_c;
@@ -370,7 +370,7 @@ void AdcLoopHander(void)
{ {
g_motor_data.current = 0.0; g_motor_data.current = 0.0;
} }
DCMOTOR_DBG_LOG("\t\t\t g_motor_data.current:%.1fmA \r\n", g_motor_data.current); // DCMOTOR_DBG_LOG("\t\t\t g_motor_data.current:%.1fmA \r\n", g_motor_data.current);
DMA_Cmd(M4_DMA1, Enable); DMA_Cmd(M4_DMA1, Enable);
} }
} }
Project/HC32F460/source/User/Inc/main.h
+5 -4
View File
@@ -8,16 +8,17 @@
#define SDCARD_BLOCK_MAX (10000) #define SDCARD_BLOCK_MAX (10000)
#define OSC_CNT 40 #define OSC_CNT 40
#define SLOPE_CNT 30 #define SLOPE_CNT 20
#define SLOPE_VPT 1000 #define SLOPE_VPT 1000
#define FOLLOW_VPT 20 #define FOLLOW_VPT 20
#define SHIELD_VPT 4000 #define SHIELD_VPT 5000
#define EME_STOP_VPT 4000 #define EME_STOP_VPT 4000
#define CENTRE_VPT 2000
#define STOP_TIME_MS 0 #define STOP_TIME_MS 1000
#define RUN_TIME_MS 0 #define RUN_TIME_MS 0
#define EME_START_MS 2000 #define EME_START_MS 0
#define EME_STOP_MS 2000 #define EME_STOP_MS 2000
#define SLOPE_START_MS 3000 #define SLOPE_START_MS 3000
Project/HC32F460/source/User/Src/bsp.c
+3 -1
View File
@@ -449,6 +449,8 @@ static void GPIO_Config(void)
stcPortInit.enPullUp = Enable;//上拉 stcPortInit.enPullUp = Enable;//上拉
PORT_Init(TWIN_BIT0_PORTx, TWIN_BIT0_PINx, &stcPortInit); PORT_Init(TWIN_BIT0_PORTx, TWIN_BIT0_PINx, &stcPortInit);
PORT_Init(TWIN_BIT1_PORTx, TWIN_BIT1_PINx, &stcPortInit); PORT_Init(TWIN_BIT1_PORTx, TWIN_BIT1_PINx, &stcPortInit);
SET_TWIN_BIT0();
SET_TWIN_BIT1();
} }
/*********************************左上传感器 /*********************************左上传感器
******************************************************************************* *******************************************************************************
@@ -1356,7 +1358,7 @@ void BSP_Init(void)
ExtiCh06_Init(); ExtiCh06_Init();
ExtiCh04_Init(); ExtiCh04_Init();
DbgUart_Config(500000); DbgUart_Config(700000);
//RS485Uart_Config(9600); //RS485Uart_Config(9600);
//Rtc_Config(); //Rtc_Config();
Project/HC32F460/source/User/Src/main.c
+157 -103
View File
@@ -36,10 +36,10 @@ static void show(uint8_t ch,int32_t ad)
} }
void LEFTUP_CS1237AinADataProcessCallBack(int32_t AD) void LEFTUP_CS1237AinADataProcessCallBack(int32_t AD)
{ {
if(abs(AD - App.LeftUpCS1237_TrendArray[0]) > 2000000) // if(abs(AD - App.LeftUpCS1237_TrendArray[0]) > 2000000)
{ // {
AD = App.LeftUpCS1237_TrendArray[0]; // AD = App.LeftUpCS1237_TrendArray[0];
} // }
show(1,AD); show(1,AD);
for(int i = OSC_CNT-1; i > 0; i--) for(int i = OSC_CNT-1; i > 0; i--)
{ {
@@ -51,10 +51,10 @@ void LEFTUP_CS1237AinADataProcessCallBack(int32_t AD)
void LEFTLOW_CS1237AinADataProcessCallBack(int32_t AD) void LEFTLOW_CS1237AinADataProcessCallBack(int32_t AD)
{ {
if(abs(AD - App.LeftLowCS1237_TrendArray[0]) > 2000000) // if(abs(AD - App.LeftLowCS1237_TrendArray[0]) > 2000000)
{ // {
AD = App.LeftLowCS1237_TrendArray[0]; // AD = App.LeftLowCS1237_TrendArray[0];
} // }
show(2,AD); show(2,AD);
for(int i = OSC_CNT-1; i > 0; i--) for(int i = OSC_CNT-1; i > 0; i--)
{ {
@@ -65,10 +65,10 @@ void LEFTLOW_CS1237AinADataProcessCallBack(int32_t AD)
} }
void RIGHTUP_CS1237AinADataProcessCallBack(int32_t AD) void RIGHTUP_CS1237AinADataProcessCallBack(int32_t AD)
{ {
if(abs(AD - App.RightUpCS1237_TrendArray[0]) > 2000000) // if(abs(AD - App.RightUpCS1237_TrendArray[0]) > 2000000)
{ // {
AD = App.RightUpCS1237_TrendArray[0]; // AD = App.RightUpCS1237_TrendArray[0];
} // }
show(3,AD); show(3,AD);
for(int i = OSC_CNT-1; i > 0; i--) for(int i = OSC_CNT-1; i > 0; i--)
{ {
@@ -79,10 +79,10 @@ void RIGHTUP_CS1237AinADataProcessCallBack(int32_t AD)
} }
void RIGHTLOW_CS1237AinADataProcessCallBack(int32_t AD) void RIGHTLOW_CS1237AinADataProcessCallBack(int32_t AD)
{ {
if(abs(AD - App.RightLowCS1237_TrendArray[0]) > 2000000) // if(abs(AD - App.RightLowCS1237_TrendArray[0]) > 2000000)
{ // {
AD = App.RightLowCS1237_TrendArray[0]; // AD = App.RightLowCS1237_TrendArray[0];
} // }
show(4,AD); show(4,AD);
for(int i = OSC_CNT-1; i > 0; i--) for(int i = OSC_CNT-1; i > 0; i--)
{ {
@@ -222,10 +222,10 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
int32_t LeftSlopeSum = abs(App.LeftUpCS1237_SlopeArray[0]) + abs(App.LeftLowCS1237_SlopeArray[0]); int32_t LeftSlopeSum = abs(App.LeftUpCS1237_SlopeArray[0]) + abs(App.LeftLowCS1237_SlopeArray[0]);
int32_t RightSlopeSum = abs(App.RightUpCS1237_SlopeArray[0]) + abs(App.RightLowCS1237_SlopeArray[0]); int32_t RightSlopeSum = abs(App.RightUpCS1237_SlopeArray[0]) + abs(App.RightLowCS1237_SlopeArray[0]);
int32_t LeftLowDiff = abs(Oll_diff1); int32_t LeftDiff = abs(Oll_diff1) + abs(Olu_diff1);
int32_t RightLowDiff = abs(Orl_diff1); int32_t RightDiff = abs(Orl_diff1) + abs(Oru_diff1);
int32_t LeftDiffSum = abs(lu_diff[0]); int32_t LeftDiffSum = abs(lu_diff[0]) + abs(ll_diff[0]);
int32_t RightDiffSum = abs(ru_diff[0]); int32_t RightDiffSum = abs(ru_diff[0]) + abs(rl_diff[0]);
if(App.EmeFlag == true) if(App.EmeFlag == true)
{ {
@@ -253,76 +253,98 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.LeftRunFalg = false; App.LeftRunFalg = false;
App.RightRunFalg = false; App.RightRunFalg = false;
App.CentreRunFalg = false; App.CentreRunFalg = false;
if(((lu_ad[0] < App.ADTrack.LeftUp_Org_AdZero-1000) if(((lu_ad[0] < App.ADTrack.LeftUp_Org_AdZero-CENTRE_VPT)//判断中间操作
&& (ll_ad[0] > App.ADTrack.LeftLow_Org_AdZero+1000) && (ll_ad[0] > App.ADTrack.LeftLow_Org_AdZero+CENTRE_VPT)
&& (ru_ad[0] < App.ADTrack.RightUp_Org_AdZero-1000) && (ru_ad[0] < App.ADTrack.RightUp_Org_AdZero-CENTRE_VPT)
&& (rl_ad[0] > App.ADTrack.RightLow_Org_AdZero+1000)) && (rl_ad[0] > App.ADTrack.RightLow_Org_AdZero+CENTRE_VPT)
|| ((lu_ad[0] > App.ADTrack.LeftUp_Org_AdZero+1000) && (lu_ad[1] < App.ADTrack.LeftUp_Org_AdZero-CENTRE_VPT)
&& (ll_ad[0] < App.ADTrack.LeftLow_Org_AdZero-1000) && (ll_ad[1] > App.ADTrack.LeftLow_Org_AdZero+CENTRE_VPT)
&& (ru_ad[0] > App.ADTrack.RightUp_Org_AdZero+1000) && (ru_ad[1] < App.ADTrack.RightUp_Org_AdZero-CENTRE_VPT)
&& (rl_ad[0] < App.ADTrack.RightLow_Org_AdZero-1000))) && (rl_ad[1] > App.ADTrack.RightLow_Org_AdZero+CENTRE_VPT)
&& (lu_ad[2] < App.ADTrack.LeftUp_Org_AdZero-CENTRE_VPT)
&& (ll_ad[2] > App.ADTrack.LeftLow_Org_AdZero+CENTRE_VPT)
&& (ru_ad[2] < App.ADTrack.RightUp_Org_AdZero-CENTRE_VPT)
&& (rl_ad[2] > App.ADTrack.RightLow_Org_AdZero+CENTRE_VPT))
|| ((lu_ad[0] > App.ADTrack.LeftUp_Org_AdZero+CENTRE_VPT)
&& (ll_ad[0] < App.ADTrack.LeftLow_Org_AdZero-CENTRE_VPT)
&& (ru_ad[0] > App.ADTrack.RightUp_Org_AdZero+CENTRE_VPT)
&& (rl_ad[0] < App.ADTrack.RightLow_Org_AdZero-CENTRE_VPT)
&& (lu_ad[1] > App.ADTrack.LeftUp_Org_AdZero+CENTRE_VPT)
&& (ll_ad[1] < App.ADTrack.LeftLow_Org_AdZero-CENTRE_VPT)
&& (ru_ad[1] > App.ADTrack.RightUp_Org_AdZero+CENTRE_VPT)
&& (rl_ad[1] < App.ADTrack.RightLow_Org_AdZero-CENTRE_VPT)
&& (lu_ad[2] > App.ADTrack.LeftUp_Org_AdZero+CENTRE_VPT)
&& (ll_ad[2] < App.ADTrack.LeftLow_Org_AdZero-CENTRE_VPT)
&& (ru_ad[2] > App.ADTrack.RightUp_Org_AdZero+CENTRE_VPT)
&& (rl_ad[2] < App.ADTrack.RightLow_Org_AdZero-CENTRE_VPT)))
{ {
App.CentreRunFalg = true; App.CentreRunFalg = true;
if(abs(Oll_diff1) > abs(Orl_diff1)) if(LeftDiff > RightDiff)
{ {
App.LeftRunFalg = true; App.LeftRunFalg = true;
if((Oll_diff1+Orl_diff1) < -(App.ADTrack.Stop_VPT)) if((Oll_diff1+Orl_diff1) < -(App.ADTrack.Stop_VPT))
{ {
BDC_UP(); BDC_UP();
App.UpFlag = true; App.UpFlag = true;
App.StopFlag = false; App.StopFlag = false;
App.DownFlag = false; App.DownFlag = false;
App.EmeFlag = false; App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
} }
if((Oll_diff1+Orl_diff1) > (App.ADTrack.Stop_VPT)) if((Oll_diff1+Orl_diff1) > (App.ADTrack.Stop_VPT))
{ {
BDC_DOWN(); BDC_DOWN();
App.UpFlag = false; App.UpFlag = false;
App.StopFlag = false; App.StopFlag = false;
App.DownFlag = true; App.DownFlag = true;
App.EmeFlag = false; App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
} }
} }
else if(abs(Oll_diff1) < abs(Orl_diff1)) else if(RightDiff > LeftDiff)
{ {
App.RightRunFalg = true; App.RightRunFalg = true;
if((Oll_diff1+Orl_diff1) < -(App.ADTrack.Stop_VPT)) if((Oll_diff1+Orl_diff1) < -(App.ADTrack.Stop_VPT))
{ {
BDC_UP(); BDC_UP();
App.UpFlag = true; App.UpFlag = true;
App.StopFlag = false; App.StopFlag = false;
App.DownFlag = false; App.DownFlag = false;
App.EmeFlag = false; App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
} }
if((Oll_diff1+Orl_diff1) > (App.ADTrack.Stop_VPT)) if((Oll_diff1+Orl_diff1) > (App.ADTrack.Stop_VPT))
{ {
BDC_DOWN(); BDC_DOWN();
App.UpFlag = false; App.UpFlag = false;
App.StopFlag = false; App.StopFlag = false;
App.DownFlag = true; App.DownFlag = true;
App.EmeFlag = false; App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
} }
} }
} }
else if((LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum)) else if((LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum))////判断左边操作
{ {
App.LeftRunFalg = true; App.LeftRunFalg = true;
if(((App.LeftUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) if(((App.SlopeCollDelay1mSCnt <= 0)
&& (App.LeftUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) //判断正面按压
&& (App.LeftUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) && (App.LeftUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT) && (App.LeftUpCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) && (App.LeftLowCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
&& (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))
&& (App.SlopeCollDelay1mSCnt <= 0))
|| ((Oll_diff1 <= -(App.ADTrack.LeftLowVPT*5/6)) || ((Oll_diff1 <= -(App.ADTrack.LeftLowVPT*5/6))
&& (Oll_diff2 <= -(App.ADTrack.LeftLowVPT*5/6)) && (Oll_diff2 <= -(App.ADTrack.LeftLowVPT*5/6))
&& (Oll_diff3 <= -(App.ADTrack.LeftLowVPT*5/6)))) && (Oll_diff3 <= -(App.ADTrack.LeftLowVPT*5/6))
&& (Olu_diff1 >= (App.ADTrack.LeftUpVPT*5/6))
&& (Olu_diff2 >= (App.ADTrack.LeftUpVPT*5/6))
&& (Olu_diff3 >= (App.ADTrack.LeftUpVPT*5/6))))
{ {
if((abs(Oll_diff1) > abs(Orl_diff1) + App.ADTrack.LeftLowShieldVPT) if((abs(Oll_diff1) > abs(Orl_diff1) + App.ADTrack.LeftLowShieldVPT)//判断侧面推拉
&& (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT)
&& (abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT)
&& (abs(Olu_diff1) > abs(Oru_diff1) + App.ADTrack.LeftUpShieldVPT)
) )
{ {
BDC_UP(); BDC_UP();
@@ -333,18 +355,24 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
} }
} }
else if(((App.LeftUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) else if(((App.SlopeCollDelay1mSCnt <= 0)
&& (App.LeftUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) //判断上下拉方向
&& (App.LeftUpCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) && (App.LeftUpCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT) && (App.LeftUpCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) && (App.LeftLowCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT)
&& (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))
&& ((App.SlopeCollDelay1mSCnt <= 0))) || ((Oll_diff1 >= (App.ADTrack.LeftLowVPT*5/6))
|| ((Oll_diff1 >= App.ADTrack.LeftLowVPT) && (Oll_diff2 >= (App.ADTrack.LeftLowVPT*5/6))
&& (Oll_diff1 >= App.ADTrack.LeftLowVPT) && (Oll_diff3 >= (App.ADTrack.LeftLowVPT*5/6))
&& (Oll_diff1 >= App.ADTrack.LeftLowVPT))) && (Olu_diff1 <= -(App.ADTrack.LeftUpVPT*5/6))
&& (Olu_diff2 <= -(App.ADTrack.LeftUpVPT*5/6))
&& (Olu_diff3 <= -(App.ADTrack.LeftUpVPT*5/6))))
{ {
if((abs(Oll_diff1) > abs(Orl_diff1) + App.ADTrack.LeftLowShieldVPT) if((abs(Oll_diff1) > abs(Orl_diff1) + App.ADTrack.LeftLowShieldVPT)//判断侧面推拉
&& (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT)
&& (abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT)
&& (abs(Olu_diff1) > abs(Oru_diff1) + App.ADTrack.LeftUpShieldVPT)
) )
{ {
BDC_DOWN(); BDC_DOWN();
@@ -356,21 +384,27 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
} }
} }
} }
else if(LeftSlopeSum < RightSlopeSum && LeftDiffSum < RightDiffSum) else if(LeftSlopeSum < RightSlopeSum && LeftDiffSum < RightDiffSum)//上下拉屏幕右边
{ {
App.RightRunFalg = true; App.RightRunFalg = true;
if(((App.RightUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) if(((App.SlopeCollDelay1mSCnt <= 0)
&& (App.RightUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) //判断上下拉方向
&& (App.RightUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) && (App.RightUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT)
&& (App.RightUpCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT) && (App.RightUpCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)
&& (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))
&& (App.SlopeCollDelay1mSCnt <= 0))
|| ((Orl_diff1 <= -(App.ADTrack.RightLowVPT*5/6)) || ((Orl_diff1 <= -(App.ADTrack.RightLowVPT*5/6))
&& (Orl_diff2 <= -(App.ADTrack.RightLowVPT*5/6)) && (Orl_diff2 <= -(App.ADTrack.RightLowVPT*5/6))
&& (Orl_diff3 <= -(App.ADTrack.RightLowVPT*5/6)))) && (Orl_diff3 <= -(App.ADTrack.RightLowVPT*5/6))
&& (Oru_diff1 >= (App.ADTrack.RightUpVPT*5/6))
&& (Oru_diff2 >= (App.ADTrack.RightUpVPT*5/6))
&& (Oru_diff3 >= (App.ADTrack.RightUpVPT*5/6))))
{ {
if((abs(Orl_diff1) > abs(Oll_diff1) + App.ADTrack.RightLowShieldVPT) if((abs(Orl_diff1) > abs(Oll_diff1) + App.ADTrack.RightLowShieldVPT)//判断侧面推拉
&& (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT)
&& (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT)
&& (abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT)
) )
{ {
BDC_UP(); BDC_UP();
@@ -381,19 +415,24 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
} }
} }
else if(((App.RightUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) else if(((App.SlopeCollDelay1mSCnt <= 0)
&& (App.RightUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) //判断上下拉方向
&& (App.RightUpCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) && (App.RightUpCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT)
&& (App.RightUpCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT) && (App.RightUpCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)
&& (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))
&& (App.SlopeCollDelay1mSCnt <= 0)) || ((Orl_diff1 >= (App.ADTrack.RightLowVPT*5/6))
|| ((Orl_diff1 >= App.ADTrack.RightLowVPT) && (Orl_diff2 >= (App.ADTrack.RightLowVPT*5/6))
&& (Orl_diff2 >= App.ADTrack.RightLowVPT) && (Orl_diff3 >= (App.ADTrack.RightLowVPT*5/6))
&& (Orl_diff3 >= App.ADTrack.RightLowVPT)) && (Oru_diff1 <= -(App.ADTrack.RightUpVPT*5/6))
) && (Oru_diff2 <= -(App.ADTrack.RightUpVPT*5/6))
&& (Oru_diff3 <= -(App.ADTrack.RightUpVPT*5/6))))
{ {
if((abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT) if((abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT)//判断侧面推拉
&& (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT)
&& (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT)
&& (abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT)
) )
{ {
BDC_DOWN(); BDC_DOWN();
@@ -441,6 +480,9 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
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)
&& (abs(Olu_diff1) + EME_STOP_VPT) < abs(Oru_diff1)
&& (abs(Olu_diff2) + EME_STOP_VPT) < abs(Oru_diff2)
&& (abs(Olu_diff3) + EME_STOP_VPT) < abs(Oru_diff3)
&& App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false) && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
{ {
BDC_STOP(); BDC_STOP();
@@ -481,6 +523,9 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
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)
&& abs(Olu_diff1) > (abs(Oru_diff1) + EME_STOP_VPT)
&& abs(Olu_diff2) > (abs(Oru_diff2) + EME_STOP_VPT)
&& abs(Olu_diff3) > (abs(Oru_diff3) + EME_STOP_VPT)
&& App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false) && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
{ {
BDC_STOP(); BDC_STOP();
@@ -524,6 +569,9 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
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)
&& (abs(Olu_diff1) + EME_STOP_VPT) < abs(Oru_diff1)
&& (abs(Olu_diff2) + EME_STOP_VPT) < abs(Oru_diff2)
&& (abs(Olu_diff3) + EME_STOP_VPT) < abs(Oru_diff3)
&& App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false) && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
{ {
BDC_STOP(); BDC_STOP();
@@ -564,6 +612,9 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
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)
&& abs(Olu_diff1) > (abs(Oru_diff1) + EME_STOP_VPT)
&& abs(Olu_diff2) > (abs(Oru_diff2) + EME_STOP_VPT)
&& abs(Olu_diff3) > (abs(Oru_diff3) + EME_STOP_VPT)
&& App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false) && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
{ {
BDC_STOP(); BDC_STOP();
@@ -573,32 +624,33 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
} }
} }
if (lu_K.peaks_positive if (
&& lu_K.valleys_negative // lu_K.peaks_positive
&& App.LeftUpCS1237_SlopeArray[0] < FOLLOW_VPT // && lu_K.valleys_negative
App.LeftUpCS1237_SlopeArray[0] < FOLLOW_VPT
&& App.LeftUpCS1237_SlopeArray[0] > -FOLLOW_VPT && App.LeftUpCS1237_SlopeArray[0] > -FOLLOW_VPT
&& App.LeftUpCS1237_SlopeArray[1] < FOLLOW_VPT && App.LeftUpCS1237_SlopeArray[1] < FOLLOW_VPT
&& App.LeftUpCS1237_SlopeArray[1] > -FOLLOW_VPT && App.LeftUpCS1237_SlopeArray[1] > -FOLLOW_VPT
&& App.LeftUpCS1237_SlopeArray[2] < FOLLOW_VPT && App.LeftUpCS1237_SlopeArray[2] < FOLLOW_VPT
&& App.LeftUpCS1237_SlopeArray[2] > -FOLLOW_VPT && App.LeftUpCS1237_SlopeArray[2] > -FOLLOW_VPT
&& ll_K.peaks_positive // && ll_K.peaks_positive
&& ll_K.valleys_negative // && ll_K.valleys_negative
&& App.LeftLowCS1237_SlopeArray[0] < FOLLOW_VPT && App.LeftLowCS1237_SlopeArray[0] < FOLLOW_VPT
&& App.LeftLowCS1237_SlopeArray[0] > -FOLLOW_VPT && App.LeftLowCS1237_SlopeArray[0] > -FOLLOW_VPT
&& App.LeftLowCS1237_SlopeArray[1] < FOLLOW_VPT && App.LeftLowCS1237_SlopeArray[1] < FOLLOW_VPT
&& App.LeftLowCS1237_SlopeArray[1] > -FOLLOW_VPT && App.LeftLowCS1237_SlopeArray[1] > -FOLLOW_VPT
&& App.LeftLowCS1237_SlopeArray[2] < FOLLOW_VPT && App.LeftLowCS1237_SlopeArray[2] < FOLLOW_VPT
&& App.LeftLowCS1237_SlopeArray[2] > -FOLLOW_VPT && App.LeftLowCS1237_SlopeArray[2] > -FOLLOW_VPT
&& ru_K.peaks_positive // && ru_K.peaks_positive
&& ru_K.valleys_negative // && ru_K.valleys_negative
&& App.RightUpCS1237_SlopeArray[0] < FOLLOW_VPT && App.RightUpCS1237_SlopeArray[0] < FOLLOW_VPT
&& App.RightUpCS1237_SlopeArray[0] > -FOLLOW_VPT && App.RightUpCS1237_SlopeArray[0] > -FOLLOW_VPT
&& App.RightUpCS1237_SlopeArray[1] < FOLLOW_VPT && App.RightUpCS1237_SlopeArray[1] < FOLLOW_VPT
&& App.RightUpCS1237_SlopeArray[1] > -FOLLOW_VPT && App.RightUpCS1237_SlopeArray[1] > -FOLLOW_VPT
&& App.RightUpCS1237_SlopeArray[2] < FOLLOW_VPT && App.RightUpCS1237_SlopeArray[2] < FOLLOW_VPT
&& App.RightUpCS1237_SlopeArray[2] > -FOLLOW_VPT && App.RightUpCS1237_SlopeArray[2] > -FOLLOW_VPT
&& rl_K.peaks_positive // && rl_K.peaks_positive
&& rl_K.valleys_negative // && rl_K.valleys_negative
&& App.RightLowCS1237_SlopeArray[0] < FOLLOW_VPT && App.RightLowCS1237_SlopeArray[0] < FOLLOW_VPT
&& App.RightLowCS1237_SlopeArray[0] > -FOLLOW_VPT && App.RightLowCS1237_SlopeArray[0] > -FOLLOW_VPT
&& App.RightLowCS1237_SlopeArray[1] < FOLLOW_VPT && App.RightLowCS1237_SlopeArray[1] < FOLLOW_VPT
@@ -646,6 +698,8 @@ static void AppInit(void)
RIGHTUP_CS1237_Init(); RIGHTUP_CS1237_Init();
RIGHTLOW_CS1237_Init(); RIGHTLOW_CS1237_Init();
BDC_STOP();
App.Status = APP_STATUS_WAIT; App.Status = APP_STATUS_WAIT;
App.TD1mSDelayCnt = 1000; App.TD1mSDelayCnt = 1000;
// App.Status = APP_STATUS_UP1; // App.Status = APP_STATUS_UP1;
@@ -892,7 +946,7 @@ void FeedDogHandler(void)
int main(void) int main(void)
{ {
BSP_Init(); BSP_Init();
AppInit(); AppInit();
while(1) { while(1) {
#if(USE_WDT == 1) #if(USE_WDT == 1)
FeedDogHandler(); FeedDogHandler();