From c2976eff6bed68bdc0288df52ca6fc6a7c0ff5fa Mon Sep 17 00:00:00 2001 From: "DESKTOP-KQHDCM4\\97555" <975559679@qq.com> Date: Tue, 21 Oct 2025 18:28:49 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8F=8C=E6=9F=B1=E5=8F=8C=E7=94=B5=E6=9C=BAS?= =?UTF-8?q?=E5=9E=8B=E4=BC=A0=E6=84=9F=E5=99=A8=E7=8B=AC=E7=AB=8B=E5=88=86?= =?UTF-8?q?=E6=94=AF=E6=89=80=E6=9C=89=E5=9F=BA=E6=9C=AC=E5=8A=9F=E8=83=BD?= =?UTF-8?q?=E5=AE=8C=E6=88=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Project/HC32F460/MDK/HC32F460.uvoptx | 183 ++------ .../source/Module/dc_motor/dc_motor.h | 4 - Project/HC32F460/source/User/Inc/main.h | 13 +- Project/HC32F460/source/User/Src/main.c | 397 ++++++++++-------- 4 files changed, 258 insertions(+), 339 deletions(-) diff --git a/Project/HC32F460/MDK/HC32F460.uvoptx b/Project/HC32F460/MDK/HC32F460.uvoptx index 69ea6a3..4212ed3 100644 --- a/Project/HC32F460/MDK/HC32F460.uvoptx +++ b/Project/HC32F460/MDK/HC32F460.uvoptx @@ -12,7 +12,7 @@ *.lib *.txt; *.h; *.inc; *.md *.plm - *.cpp + *.cpp; *.cc; *.cxx 0 @@ -117,21 +117,6 @@ BIN\CMSIS_AGDI.dll - - 0 - JL2CM3 - -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) - - - 0 - UL2CM3 - UL2CM3(-S0 -C0 -P0 ) -FN2 -FC1000 -FD1FFF8000 -FF0HC32F460_512K -FF1HC32F460_otp -FL080000 -FL13FC -FS00 -FS13000C00 -FP0($$Device:HC32F460JETA$FlashARM\HC32F460_512K.FLM) -FP1($$Device:HC32F460JETA$FlashARM\HC32F460_otp.FLM) - - - 0 - CMSIS_AGDI - -X"Any" -UAny -O206 -S9 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -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) - 0 ARMRTXEVENTFLAGS @@ -152,45 +137,18 @@ DLGUARM - -<<<<<<< HEAD - -======= - - + 0 - 0 - 291 - 1 -
4146
- 0 - 0 - 0 - 0 - 0 - 1 - ..\source\Module\dc_motor\dc_motor.c - - \\BombSensor\../source/Module/dc_motor/dc_motor.c\291 - - - 1 - 0 - 343 - 1 -
0
- 0 - 0 - 0 - 0 - 0 - 0 - ..\source\Module\dc_motor\dc_motor.c - - - - ->>>>>>> Motor_Test + CMSIS_AGDI + -X"Any" -UAny -O206 -S0 -C0 -P00000000 -N00("") -D00(00000000) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -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) + + + 0 + UL2CM3 + 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)) + + + 0 @@ -210,83 +168,61 @@ 3 1 - App.LeftUpCS1237_SlopeArray[0],0x0A + App.EmeFlag 4 1 - App.LeftLowCS1237_SlopeArray[0],0x0A + App.ADTrack.LeftUp_Org_AdZero,0x0A 5 1 - App.RightUpCS1237_SlopeArray[0],0x0A + App.ADTrack.LeftLow_Org_AdZero,0x0A 6 1 - App.RightLowCS1237_SlopeArray[0],0x0A + App.ADTrack.RightUp_Org_AdZero,0x0A 7 1 - App.ADTrack.LeftUp_Org_AdZero,0x0A + App.ADTrack.RightLow_Org_AdZero,0x0A 8 1 - App.ADTrack.LeftLow_Org_AdZero,0x0A + App.LeftRunFalg 9 1 - App.ADTrack.RightUp_Org_AdZero,0x0A + App.RightRunFalg 10 1 - App.ADTrack.RightLow_Org_AdZero,0x0A + App.CentreRunFalg 11 1 - App.LeftRunFalg,0x0A + LeftLowDiff,0x0A 12 1 - App.RightRunFalg,0x0A - - - 13 - 1 - App.CentreRunFalg,0x0A - - - 18 - 1 - g_motor_data + RightLowDiff,0x0A - - - 1 - 0 - 0x1FFFA910 - 0 - - 0 0 1 -<<<<<<< HEAD 1 -======= - 0 ->>>>>>> Motor_Test 0 0 0 @@ -324,10 +260,10 @@ 1 - 0 + 1 0 2 - 5000000 + 1000000 @@ -427,72 +363,7 @@ - - - 0 - 0 - 163 - 1 -
0
- 0 - 0 - 0 - 0 - 0 - 0 - ..\source\User\Src\main.c - - - - - 1 - 0 - 185 - 1 -
0
- 0 - 0 - 0 - 0 - 0 - 0 - ..\source\User\Src\main.c - - - - - 2 - 0 - 211 - 1 -
0
- 0 - 0 - 0 - 0 - 0 - 0 - ..\source\User\Src\main.c - - - - - 3 - 0 - 235 - 1 -
0
- 0 - 0 - 0 - 0 - 0 - 0 - ..\source\User\Src\main.c - - - - + 0 @@ -1152,7 +1023,7 @@ CS1237 - 1 + 0 0 0 0 @@ -1208,7 +1079,7 @@ dc_motor - 1 + 0 0 0 0 diff --git a/Project/HC32F460/source/Module/dc_motor/dc_motor.h b/Project/HC32F460/source/Module/dc_motor/dc_motor.h index 8125ac2..172d40b 100644 --- a/Project/HC32F460/source/Module/dc_motor/dc_motor.h +++ b/Project/HC32F460/source/Module/dc_motor/dc_motor.h @@ -28,11 +28,7 @@ #define MAX_SPEED 480 // 最大速度(PWM占空比) #define ACCEL_STEP 10 // 加速步进值 #define DECEL_STEP 10 // 减速步进值 -<<<<<<< HEAD #define ACCEL_INTERVAL 10 // 加减速间隔(ms) -======= -#define ACCEL_INTERVAL 2 // 加减速间隔(ms) ->>>>>>> Motor_Test diff --git a/Project/HC32F460/source/User/Inc/main.h b/Project/HC32F460/source/User/Inc/main.h index 82d5c43..cb6d967 100644 --- a/Project/HC32F460/source/User/Inc/main.h +++ b/Project/HC32F460/source/User/Inc/main.h @@ -5,7 +5,7 @@ #define USE_FILE_SYSTEM 1 -#define SDCARD_BLOCK_MAX (10000) +#define SDCARD_BLOCK_MAX (10000) #define OSC_CNT 40 #define SLOPE_CNT 30 @@ -13,15 +13,18 @@ #define SLOPE_VPT 1000 #define FOLLOW_VPT 20 #define SHIELD_VPT 2500 +#define EME_STOP_VPT 10000 -#define STOP_TIME_MS 0 +#define STOP_TIME_MS 500 #define RUN_TIME_MS 0 +#define EME_START_MS 500 +#define EME_STOP_MS 3000 #define TRAVEL_TIME_MS 30000 #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()) /*控制等级*/ @@ -99,13 +102,13 @@ typedef struct { uint32_t Uart1Delay1mSCnt; uint32_t StopCollDelay1mSCnt; uint32_t RunCollDelay1mSCnt; - uint32_t EvenRunDelay1mSCnt; + uint32_t EmergencyDelay1mSCnt; uint8_t FeedDogDlyCnt; bool UpFlag;//上升标志位 bool DownFlag;//下降标志位 bool StopFlag;//停止标志位 - bool EvenRunning;//平稳运行标志位 + bool EmeFlag;//应急标志位 bool LeftRunFalg; bool RightRunFalg; bool CentreRunFalg; diff --git a/Project/HC32F460/source/User/Src/main.c b/Project/HC32F460/source/User/Src/main.c index 1919926..9d55dd2 100644 --- a/Project/HC32F460/source/User/Src/main.c +++ b/Project/HC32F460/source/User/Src/main.c @@ -203,184 +203,221 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ PeakValleyCheck ru_K = check_peaks_valleys(App.RightUpCS1237_SlopeArray, SLOPE_CNT); PeakValleyCheck rl_K = check_peaks_valleys(App.RightLowCS1237_SlopeArray, SLOPE_CNT); - if(App.StopFlag == true) + 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 LeftLowDiff = abs(Oll_diff1); + int32_t RightLowDiff = abs(Orl_diff1); + int32_t LeftDiffSum = abs(lu_diff[0]); + int32_t RightDiffSum = abs(ru_diff[0]); + + if(App.EmeFlag == true) { - if(App.StopCollDelay1mSCnt <= 0) + if(App.DownFlag == true && App.EmergencyDelay1mSCnt > 0) { - uint32_t LeftSlopeSum = abs(App.LeftUpCS1237_SlopeArray[0]) + abs(App.LeftLowCS1237_SlopeArray[0]); - uint32_t RightSlopeSum = abs(App.RightUpCS1237_SlopeArray[0]) + abs(App.RightLowCS1237_SlopeArray[0]); -// uint32_t LeftDiffSum = abs(lu_diff[0]) + abs(ll_diff[0]); -// uint32_t RightDiffSum = abs(ru_diff[0]) + abs(rl_diff[0]); - uint32_t LeftDiffSum = abs(ll_diff[0]); - uint32_t RightDiffSum = abs(rl_diff[0]); - - App.LeftRunFalg = false; - App.RightRunFalg = false; - App.CentreRunFalg = false; - if(((lu_ad[0] < App.ADTrack.LeftUp_Org_AdZero-1000) - && (ll_ad[0] > App.ADTrack.LeftLow_Org_AdZero+1000) - && (ru_ad[0] < App.ADTrack.RightUp_Org_AdZero-1000) - && (rl_ad[0] > App.ADTrack.RightLow_Org_AdZero+1000)) - || ((lu_ad[0] > App.ADTrack.LeftUp_Org_AdZero+1000) - && (ll_ad[0] < App.ADTrack.LeftLow_Org_AdZero-1000) - && (ru_ad[0] > App.ADTrack.RightUp_Org_AdZero+1000) - && (rl_ad[0] < App.ADTrack.RightLow_Org_AdZero-1000))) - { - App.CentreRunFalg = true; - if(abs(Oll_diff1) > abs(Orl_diff1)) - { - App.LeftRunFalg = true; - if((App.LeftLowCS1237_SlopeArray[0] > App.ADTrack.Slope_VPT/3)) - { - if(Oll_diff1 < -App.ADTrack.LeftLowVPT/2) - { - BDC_UP(); - App.UpFlag = true; - App.StopFlag = false; - App.DownFlag = false; - App.RunCollDelay1mSCnt = RUN_TIME_MS; - } - } - else if((App.LeftLowCS1237_SlopeArray[0] < -App.ADTrack.Slope_VPT/3)) - { - if(Oll_diff1 > App.ADTrack.LeftLowVPT/2) - { - BDC_DOWN(); - App.UpFlag = false; - App.StopFlag = false; - App.DownFlag = true; - App.RunCollDelay1mSCnt = RUN_TIME_MS; - } - } - } - else if(abs(Oll_diff1) < abs(Orl_diff1)) - { - App.RightRunFalg = true; - if((App.RightLowCS1237_SlopeArray[0] > App.ADTrack.Slope_VPT/3)) - { - if(Orl_diff1 < -App.ADTrack.RightLowVPT/2) - { - BDC_UP(); - App.UpFlag = true; - App.StopFlag = false; - App.DownFlag = false; - App.RunCollDelay1mSCnt = RUN_TIME_MS; - } - } - else if((App.RightLowCS1237_SlopeArray[0] < -App.ADTrack.Slope_VPT/3)) - { - if(Orl_diff1 > App.ADTrack.RightLowVPT/2) - { - BDC_DOWN(); - App.UpFlag = false; - App.StopFlag = false; - App.DownFlag = true; - App.RunCollDelay1mSCnt = RUN_TIME_MS; - } - } - } - } - else if((LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum)) + BDC_UP(); + } + else if(App.UpFlag == true && App.EmergencyDelay1mSCnt > 0) + { + BDC_DOWN(); + } + else if(App.EmergencyDelay1mSCnt <= 0) + { + BDC_STOP(); + App.StopFlag = true; + App.DownFlag = false; + App.UpFlag = false; + App.EmeFlag = false; + App.StopCollDelay1mSCnt = STOP_TIME_MS; + } + } + else if(App.StopFlag == true && App.StopCollDelay1mSCnt <= 0) + { + App.LeftRunFalg = false; + App.RightRunFalg = false; + App.CentreRunFalg = false; + if(((lu_ad[0] < App.ADTrack.LeftUp_Org_AdZero-1000) + && (ll_ad[0] > App.ADTrack.LeftLow_Org_AdZero+1000) + && (ru_ad[0] < App.ADTrack.RightUp_Org_AdZero-1000) + && (rl_ad[0] > App.ADTrack.RightLow_Org_AdZero+1000)) + || ((lu_ad[0] > App.ADTrack.LeftUp_Org_AdZero+1000) + && (ll_ad[0] < App.ADTrack.LeftLow_Org_AdZero-1000) + && (ru_ad[0] > App.ADTrack.RightUp_Org_AdZero+1000) + && (rl_ad[0] < App.ADTrack.RightLow_Org_AdZero-1000))) + { + App.CentreRunFalg = true; + if(abs(Oll_diff1) > abs(Orl_diff1)) { App.LeftRunFalg = true; - if(((App.LeftUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) - && (App.LeftUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) - && (App.LeftUpCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT) - && (App.LeftLowCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) - && (App.LeftLowCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) - && (App.LeftLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)) - || ( - (Olu_diff1 >= App.ADTrack.LeftUpVPT) - && (Olu_diff2 >= App.ADTrack.LeftUpVPT) - && (Olu_diff3 >= App.ADTrack.LeftUpVPT) - && (Oll_diff1 <= -App.ADTrack.LeftLowVPT) - && (Oll_diff2 <= -App.ADTrack.LeftLowVPT) - && (Oll_diff3 <= -App.ADTrack.LeftLowVPT))) - { - if((abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT) - && (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT) - ) +// if((App.LeftLowCS1237_SlopeArray[0] > App.ADTrack.Slope_VPT/3)) +// { + if(Oll_diff1 < -App.ADTrack.LeftLowVPT/3) { BDC_UP(); App.UpFlag = true; App.StopFlag = false; App.DownFlag = false; + App.EmeFlag = false; + App.RunCollDelay1mSCnt = RUN_TIME_MS; + App.EmergencyDelay1mSCnt = EME_START_MS; } - } - else if(((App.LeftUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) - && (App.LeftUpCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) - && (App.LeftUpCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT) - && (App.LeftLowCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) - && (App.LeftLowCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) - && (App.LeftLowCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)) - || ( - (Olu_diff1 <= -App.ADTrack.LeftUpVPT) - && (Olu_diff1 <= -App.ADTrack.LeftUpVPT) - && (Olu_diff1 <= -App.ADTrack.LeftUpVPT) - && (Oll_diff1 >= App.ADTrack.LeftLowVPT) - && (Oll_diff1 >= App.ADTrack.LeftLowVPT) - && (Oll_diff1 >= App.ADTrack.LeftLowVPT))) - { - if((abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT) - && (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT) - ) +// } +// else if((App.LeftLowCS1237_SlopeArray[0] < -App.ADTrack.Slope_VPT/3)) +// { + if(Oll_diff1 > App.ADTrack.LeftLowVPT/3) { BDC_DOWN(); App.UpFlag = false; - App.DownFlag = true; App.StopFlag = false; + App.DownFlag = true; + App.EmeFlag = false; + App.RunCollDelay1mSCnt = RUN_TIME_MS; + App.EmergencyDelay1mSCnt = EME_START_MS; } - } +// } } - else if(LeftSlopeSum < RightSlopeSum && LeftDiffSum < RightDiffSum) + else if(abs(Oll_diff1) < abs(Orl_diff1)) { App.RightRunFalg = true; - if(((App.RightUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) - && (App.RightUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) - && (App.RightUpCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT) - && (App.RightLowCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) - && (App.RightLowCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) - && (App.RightLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)) - || ((Oru_diff1 >= App.ADTrack.RightUpVPT) - && (Oru_diff2 >= App.ADTrack.RightUpVPT) - && (Oru_diff3 >= App.ADTrack.RightUpVPT) - && (Orl_diff1 <= -App.ADTrack.RightLowVPT) - && (Orl_diff2 <= -App.ADTrack.RightLowVPT) - && (Orl_diff3 <= -App.ADTrack.RightLowVPT))) + if((App.RightLowCS1237_SlopeArray[0] > App.ADTrack.Slope_VPT/3)) { - if((abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT) - && (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT) - ) + if(Orl_diff1 < -App.ADTrack.RightLowVPT/3) { BDC_UP(); App.UpFlag = true; App.StopFlag = false; App.DownFlag = false; + App.EmeFlag = false; + App.RunCollDelay1mSCnt = RUN_TIME_MS; + App.EmergencyDelay1mSCnt = EME_START_MS; } } - else if(((App.RightUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) - && (App.RightUpCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) - && (App.RightUpCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT) - && (App.RightLowCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) - && (App.RightLowCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) - && (App.RightLowCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)) - || ((Oru_diff1 <= -App.ADTrack.RightUpVPT) - && (Oru_diff2 <= -App.ADTrack.RightUpVPT) - && (Oru_diff3 <= -App.ADTrack.RightUpVPT) - && (Orl_diff1 >= App.ADTrack.RightLowVPT) - && (Orl_diff2 >= App.ADTrack.RightLowVPT) - && (Orl_diff3 >= App.ADTrack.RightLowVPT)) + else if((App.RightLowCS1237_SlopeArray[0] < -App.ADTrack.Slope_VPT/3)) + { + if(Orl_diff1 > App.ADTrack.RightLowVPT/3) + { + BDC_DOWN(); + App.UpFlag = false; + App.StopFlag = false; + App.DownFlag = true; + App.EmeFlag = false; + App.RunCollDelay1mSCnt = RUN_TIME_MS; + App.EmergencyDelay1mSCnt = EME_START_MS; + } + } + } + } + else if((LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum)) + { + App.LeftRunFalg = true; + if(((App.LeftUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) + && (App.LeftUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) + && (App.LeftUpCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT) + && (App.LeftLowCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) + && (App.LeftLowCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) + && (App.LeftLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)) + || ( + (Olu_diff1 >= App.ADTrack.LeftUpVPT) + && (Olu_diff2 >= App.ADTrack.LeftUpVPT) + && (Olu_diff3 >= App.ADTrack.LeftUpVPT) + && (Oll_diff1 <= -App.ADTrack.LeftLowVPT) + && (Oll_diff2 <= -App.ADTrack.LeftLowVPT) + && (Oll_diff3 <= -App.ADTrack.LeftLowVPT))) + { + if((abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT) + // && (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT) ) { - if((abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT) - && (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT) - ) - { - BDC_DOWN(); - App.DownFlag = true; - App.StopFlag = false; - App.UpFlag = false; - } + BDC_UP(); + App.UpFlag = true; + App.StopFlag = false; + App.DownFlag = false; + App.EmeFlag = false; + App.RunCollDelay1mSCnt = RUN_TIME_MS; + App.EmergencyDelay1mSCnt = EME_START_MS; + } + } + else if(((App.LeftUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) + && (App.LeftUpCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) + && (App.LeftUpCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT) + && (App.LeftLowCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) + && (App.LeftLowCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) + && (App.LeftLowCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)) + || ( + (Olu_diff1 <= -App.ADTrack.LeftUpVPT) + && (Olu_diff1 <= -App.ADTrack.LeftUpVPT) + && (Olu_diff1 <= -App.ADTrack.LeftUpVPT) + && (Oll_diff1 >= App.ADTrack.LeftLowVPT) + && (Oll_diff1 >= App.ADTrack.LeftLowVPT) + && (Oll_diff1 >= App.ADTrack.LeftLowVPT))) + { + if((abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT) + // && (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT) + ) + { + BDC_DOWN(); + App.UpFlag = false; + App.DownFlag = true; + App.StopFlag = false; + App.EmeFlag = false; + App.RunCollDelay1mSCnt = RUN_TIME_MS; + App.EmergencyDelay1mSCnt = EME_START_MS; + } + } + } + else if(LeftSlopeSum < RightSlopeSum && LeftDiffSum < RightDiffSum) + { + App.RightRunFalg = true; + if(((App.RightUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) + && (App.RightUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) + && (App.RightUpCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT) + && (App.RightLowCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) + && (App.RightLowCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) + && (App.RightLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)) + || ((Oru_diff1 >= App.ADTrack.RightUpVPT) + && (Oru_diff2 >= App.ADTrack.RightUpVPT) + && (Oru_diff3 >= App.ADTrack.RightUpVPT) + && (Orl_diff1 <= -App.ADTrack.RightLowVPT) + && (Orl_diff2 <= -App.ADTrack.RightLowVPT) + && (Orl_diff3 <= -App.ADTrack.RightLowVPT))) + { + if((abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT) + && (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT) + ) + { + BDC_UP(); + App.UpFlag = true; + App.StopFlag = false; + App.DownFlag = false; + App.EmeFlag = false; + App.RunCollDelay1mSCnt = RUN_TIME_MS; + App.EmergencyDelay1mSCnt = EME_START_MS; + } + } + else if(((App.RightUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) + && (App.RightUpCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) + && (App.RightUpCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT) + && (App.RightLowCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) + && (App.RightLowCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) + && (App.RightLowCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)) + || ((Oru_diff1 <= -App.ADTrack.RightUpVPT) + && (Oru_diff2 <= -App.ADTrack.RightUpVPT) + && (Oru_diff3 <= -App.ADTrack.RightUpVPT) + && (Orl_diff1 >= App.ADTrack.RightLowVPT) + && (Orl_diff2 >= App.ADTrack.RightLowVPT) + && (Orl_diff3 >= App.ADTrack.RightLowVPT)) + ) + { + if((abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT) + && (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT) + ) + { + BDC_DOWN(); + App.DownFlag = true; + App.StopFlag = false; + App.UpFlag = false; + App.EmeFlag = false; + App.RunCollDelay1mSCnt = RUN_TIME_MS; + App.EmergencyDelay1mSCnt = EME_START_MS; } } } @@ -396,6 +433,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.StopFlag = true; App.DownFlag = false; App.UpFlag = false; + App.EmeFlag = false; App.StopCollDelay1mSCnt = STOP_TIME_MS; } else if(((App.CentreRunFalg == false ) && (Oll_diff1 < App.ADTrack.Stop_VPT)) @@ -405,8 +443,15 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.StopFlag = true; App.DownFlag = false; App.UpFlag = false; + App.EmeFlag = false; App.StopCollDelay1mSCnt = STOP_TIME_MS; } + else if((LeftLowDiff + EME_STOP_VPT) < RightLowDiff && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false) + { + BDC_STOP(); + App.EmeFlag = true; + App.EmergencyDelay1mSCnt = EME_STOP_MS; + } } else if(App.RightRunFalg == true) { @@ -417,6 +462,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.StopFlag = true; App.DownFlag = false; App.UpFlag = false; + App.EmeFlag = false; App.StopCollDelay1mSCnt = STOP_TIME_MS; } else if(((App.CentreRunFalg == false ) && (Orl_diff1 < App.ADTrack.Stop_VPT)) @@ -426,8 +472,15 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.StopFlag = true; App.DownFlag = false; App.UpFlag = false; + App.EmeFlag = false; App.StopCollDelay1mSCnt = STOP_TIME_MS; } + else if(LeftLowDiff > (RightLowDiff + EME_STOP_VPT) && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false) + { + BDC_STOP(); + App.EmeFlag = true; + App.EmergencyDelay1mSCnt = EME_STOP_MS; + } } } else if(App.UpFlag == true && App.RunCollDelay1mSCnt <= 0) @@ -441,6 +494,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.StopFlag = true; App.DownFlag = false; App.UpFlag = false; + App.EmeFlag = false; App.StopCollDelay1mSCnt = STOP_TIME_MS; } else if(((App.CentreRunFalg == false ) && (Oll_diff1 > -App.ADTrack.Stop_VPT)) @@ -450,8 +504,15 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.StopFlag = true; App.DownFlag = false; App.UpFlag = false; + App.EmeFlag = false; App.StopCollDelay1mSCnt = STOP_TIME_MS; } + else if((LeftLowDiff + EME_STOP_VPT) < RightLowDiff && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false) + { + BDC_STOP(); + App.EmeFlag = true; + App.EmergencyDelay1mSCnt = EME_STOP_MS; + } } else if(App.RightRunFalg == true) { @@ -462,6 +523,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.StopFlag = true; App.DownFlag = false; App.UpFlag = false; + App.EmeFlag = false; App.StopCollDelay1mSCnt = STOP_TIME_MS; } else if(((App.CentreRunFalg == false ) && (Orl_diff1 > -App.ADTrack.Stop_VPT)) @@ -471,8 +533,15 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.StopFlag = true; App.DownFlag = false; App.UpFlag = false; + App.EmeFlag = false; App.StopCollDelay1mSCnt = STOP_TIME_MS; } + else if(LeftLowDiff > (RightLowDiff + EME_STOP_VPT) && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false) + { + BDC_STOP(); + App.EmeFlag = true; + App.EmergencyDelay1mSCnt = EME_STOP_MS; + } } } @@ -515,25 +584,6 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.ADTrack.RightUp_Org_AdZero = ru_ad[0]; App.ADTrack.RightLow_Org_AdZero = rl_ad[0]; } - else if(App.StopFlag == false) - { - App.EvenRunning = true; - App.EvenRunDelay1mSCnt = 0; - } - } - else - { - App.EvenRunning = false; - App.EvenRunDelay1mSCnt = 0; - } - if(App.EvenRunDelay1mSCnt >= 2000) - { - App.EvenRunning = false; - App.EvenRunDelay1mSCnt = 0; - App.ADTrack.LeftUp_Org_AdZero = lu_ad[0]; - App.ADTrack.LeftLow_Org_AdZero = ll_ad[0]; - App.ADTrack.RightUp_Org_AdZero = ru_ad[0]; - App.ADTrack.RightLow_Org_AdZero = rl_ad[0]; } } #endif @@ -767,8 +817,8 @@ static void AppLoopHandler(void) App.WaitFlag = true; App.UpFlag = false; App.DownFlag = false; + App.EmeFlag = false; App.StopFlag = true; - App.EvenRunning = false; App.ADTrack.LeftUp_Org_AdZero = App.LeftUpCS1237_TrendArray[0]; App.ADTrack.LeftLow_Org_AdZero = App.LeftLowCS1237_TrendArray[0]; @@ -798,9 +848,8 @@ static void App1mSRoutine(void) App.StopCollDelay1mSCnt--; if(App.RunCollDelay1mSCnt > 0) App.RunCollDelay1mSCnt--; - if(App.EvenRunning == true) - App.EvenRunDelay1mSCnt++; - + if(App.EmergencyDelay1mSCnt > 0) + App.EmergencyDelay1mSCnt--; App.FeedDogDlyCnt++; } void FeedDogHandler(void) @@ -854,7 +903,7 @@ void SysTick_IrqHandler(void) RIGHTUP_CS12371mSRoutine(); RIGHTLOW_CS12371mSRoutine(); Dcmotor1msRoutine(); - Adc1mSRoutine(); + Adc1mSRoutine(); } /*****************************************************************************************