From cde156a030715aabc2b2b63be9f969257fb4b1b4 Mon Sep 17 00:00:00 2001 From: "DESKTOP-KQHDCM4\\97555" <975559679@qq.com> Date: Thu, 30 Oct 2025 15:52:37 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8F=8C=E6=9F=B1=E6=94=B9=E8=89=AF=E7=89=88?= =?UTF-8?q?=E8=B0=83=E8=AF=95=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 | 29 ++--- Project/HC32F460/source/User/Inc/main.h | 20 ++-- Project/HC32F460/source/User/Src/main.c | 147 +++++++++++------------- 3 files changed, 85 insertions(+), 111 deletions(-) diff --git a/Project/HC32F460/MDK/HC32F460.uvoptx b/Project/HC32F460/MDK/HC32F460.uvoptx index 8cd6355..10a7be7 100644 --- a/Project/HC32F460/MDK/HC32F460.uvoptx +++ b/Project/HC32F460/MDK/HC32F460.uvoptx @@ -103,7 +103,7 @@ 1 0 0 - 3 + 4 @@ -114,9 +114,14 @@ - BIN\CMSIS_AGDI.dll + Segger\JL2CM3.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 ARMRTXEVENTFLAGS @@ -210,26 +215,6 @@ 1 App.EmergencyDelay1mSCnt,0x0A - - 12 - 1 - App.ADTrack.LeftLow_Org_AdZero,0x0A - - - 13 - 1 - Oll_diff1,0x0A - - - 14 - 1 - Orl_diff1,0x0A - - - 15 - 1 - -(App.ADTrack.Stop_VPT),0x0A - 0 diff --git a/Project/HC32F460/source/User/Inc/main.h b/Project/HC32F460/source/User/Inc/main.h index d66b8f7..21f9d8f 100644 --- a/Project/HC32F460/source/User/Inc/main.h +++ b/Project/HC32F460/source/User/Inc/main.h @@ -12,24 +12,24 @@ #define SLOPE_VPT 1000 #define FOLLOW_VPT 20 -#define SHIELD_VPT 2500 -#define EME_STOP_VPT 10000 +#define SHIELD_VPT 10000 +#define EME_STOP_VPT 4000 #define STOP_TIME_MS 0 #define RUN_TIME_MS 0 -#define EME_START_MS 3000 -#define EME_STOP_MS 3000 +#define EME_START_MS 1000 +#define EME_STOP_MS 2000 #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_DOWN() (SET_TWIN_BIT1(),CLR_TWIN_BIT0()) +#define BDC_STOP() (SET_TWIN_BIT0(),SET_TWIN_BIT1()) +#define BDC_UP() (SET_TWIN_BIT0(),CLR_TWIN_BIT1()) +#define BDC_DOWN() (SET_TWIN_BIT1(),CLR_TWIN_BIT0()) -#define BDC_STOP() Motor_Control(MOTOR_CMD_STOP) -#define BDC_UP() Motor_Control(MOTOR_CMD_FORWARD) -#define BDC_DOWN() Motor_Control(MOTOR_CMD_REVERSE) +//#define BDC_STOP() Motor_Control(MOTOR_CMD_STOP) +//#define BDC_UP() Motor_Control(MOTOR_CMD_FORWARD) +//#define BDC_DOWN() Motor_Control(MOTOR_CMD_REVERSE) /*¿ØÖƵȼ¶*/ typedef enum { diff --git a/Project/HC32F460/source/User/Src/main.c b/Project/HC32F460/source/User/Src/main.c index 87e0757..0eceebe 100644 --- a/Project/HC32F460/source/User/Src/main.c +++ b/Project/HC32F460/source/User/Src/main.c @@ -95,6 +95,19 @@ void GradeControl(ControlLevel_m level) { switch(level){ 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.LeftUpVPT = 10000; App.ADTrack.LeftLowVPT = 10000; @@ -104,10 +117,23 @@ void GradeControl(ControlLevel_m level) App.ADTrack.LeftLowShieldVPT = SHIELD_VPT; App.ADTrack.RightUpShieldVPT = 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; break;} - case LEVEL_2:{ + case LEVEL_4:{ App.ADTrack.Slope_VPT = SLOPE_VPT; App.ADTrack.LeftUpVPT = 20000; App.ADTrack.LeftLowVPT = 20000; @@ -117,46 +143,20 @@ void GradeControl(ControlLevel_m level) App.ADTrack.LeftLowShieldVPT = SHIELD_VPT; App.ADTrack.RightUpShieldVPT = SHIELD_VPT; App.ADTrack.RightLowShieldVPT = SHIELD_VPT; - App.ADTrack.Stop_VPT = 20000; - 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; + App.ADTrack.Stop_VPT = 15000; break;} case LEVEL_5:{ App.ADTrack.Slope_VPT = SLOPE_VPT; - App.ADTrack.LeftUpVPT = 50000; - App.ADTrack.LeftLowVPT = 50000; - App.ADTrack.RightUpVPT = 50000; - App.ADTrack.RightLowVPT = 50000; + App.ADTrack.LeftUpVPT = 25000; + App.ADTrack.LeftLowVPT = 25000; + App.ADTrack.RightUpVPT = 25000; + App.ADTrack.RightLowVPT = 25000; App.ADTrack.LeftUpShieldVPT = SHIELD_VPT; App.ADTrack.LeftLowShieldVPT = SHIELD_VPT; App.ADTrack.RightUpShieldVPT = SHIELD_VPT; App.ADTrack.RightLowShieldVPT = SHIELD_VPT; - App.ADTrack.Stop_VPT = 50000/2; + App.ADTrack.Stop_VPT = 20000; 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)) { 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))) { 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.EmeFlag = false; 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)) { 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.EmeFlag = false; App.RunCollDelay1mSCnt = RUN_TIME_MS; - //App.EmergencyDelay1mSCnt = EME_START_MS; + App.EmergencyDelay1mSCnt = EME_START_MS; } -// } } else if(abs(Oll_diff1) < abs(Orl_diff1)) { 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))) { 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.EmeFlag = false; 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)) { 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.EmeFlag = false; App.RunCollDelay1mSCnt = RUN_TIME_MS; - //App.EmergencyDelay1mSCnt = EME_START_MS; + App.EmergencyDelay1mSCnt = EME_START_MS; } -// } } } 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[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))) + (Olu_diff1 >= App.ADTrack.LeftUpVPT*5/6) + && (Olu_diff2 >= App.ADTrack.LeftUpVPT*5/6) + && (Olu_diff3 >= App.ADTrack.LeftUpVPT*5/6) + && (Oll_diff1 <= -(App.ADTrack.LeftLowVPT*5/6)) + && (Oll_diff2 <= -(App.ADTrack.LeftLowVPT*5/6)) + && (Oll_diff3 <= -(App.ADTrack.LeftLowVPT*5/6)))) { 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(); @@ -350,7 +339,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.DownFlag = false; App.EmeFlag = false; 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) @@ -368,7 +357,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (Oll_diff1 >= App.ADTrack.LeftLowVPT))) { 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(); @@ -377,7 +366,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.StopFlag = false; App.EmeFlag = false; 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[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))) + || ((Oru_diff1 >= App.ADTrack.RightUpVPT*5/6) + && (Oru_diff2 >= App.ADTrack.RightUpVPT*5/6) + && (Oru_diff3 >= App.ADTrack.RightUpVPT*5/6) + && (Orl_diff1 <= -(App.ADTrack.RightLowVPT*5/6)) + && (Orl_diff2 <= -(App.ADTrack.RightLowVPT*5/6)) + && (Orl_diff3 <= -(App.ADTrack.RightLowVPT*5/6)))) { if((abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT) && (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.EmeFlag = false; 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) @@ -434,7 +423,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.UpFlag = false; App.EmeFlag = false; 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.EmergencyDelay1mSCnt = EME_START_MS; } - else if((abs(Oll_diff1) + EME_STOP_VPT) < abs(Orl_diff1) - && (abs(Oll_diff2) + EME_STOP_VPT) < abs(Orl_diff2) - && (abs(Oll_diff3) + EME_STOP_VPT) < abs(Orl_diff3) + else if((abs(Oll_diff1) - EME_STOP_VPT) < abs(Orl_diff1) + && (abs(Oll_diff2) - EME_STOP_VPT) < abs(Orl_diff2) + && (abs(Oll_diff3) - EME_STOP_VPT) < abs(Orl_diff3) && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false) { 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.EmergencyDelay1mSCnt = EME_START_MS; } - else if(abs(Oll_diff1) > (abs(Orl_diff1) + EME_STOP_VPT) - && abs(Oll_diff2) > (abs(Orl_diff2) + EME_STOP_VPT) - && abs(Oll_diff3) > (abs(Orl_diff3) + 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_diff3) > (abs(Orl_diff3) - EME_STOP_VPT) && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false) { 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.EmergencyDelay1mSCnt = EME_START_MS; } - else if((abs(Oll_diff1) + EME_STOP_VPT) < abs(Orl_diff1) - && (abs(Oll_diff2) + EME_STOP_VPT) < abs(Orl_diff2) - && (abs(Oll_diff3) + EME_STOP_VPT) < abs(Orl_diff3) + else if((abs(Oll_diff1) - EME_STOP_VPT) < abs(Orl_diff1) + && (abs(Oll_diff2) - EME_STOP_VPT) < abs(Orl_diff2) + && (abs(Oll_diff3) - EME_STOP_VPT) < abs(Orl_diff3) && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false) { 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.EmergencyDelay1mSCnt = EME_START_MS; } - else if(abs(Oll_diff1) > (abs(Orl_diff1) + EME_STOP_VPT) - && abs(Oll_diff2) > (abs(Orl_diff2) + EME_STOP_VPT) - && abs(Oll_diff3) > (abs(Orl_diff3) + 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_diff3) > (abs(Orl_diff3) - EME_STOP_VPT) && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false) { BDC_STOP();