From 18ebcb7c104eb2a9499a1325d534722c3ac5e8dc Mon Sep 17 00:00:00 2001
From: "DESKTOP-KQHDCM4\\97555" <975559679@qq.com>
Date: Fri, 4 Sep 2026 11:28:05 +0800
Subject: [PATCH] =?UTF-8?q?=E8=B0=83=E6=95=B4=E6=A1=A3=E4=BD=8D=E5=8F=82?=
=?UTF-8?q?=E6=95=B0=E5=8F=8A=E5=81=9C=E6=AD=A2=E5=88=A4=E6=96=AD=E9=80=BB?=
=?UTF-8?q?=E8=BE=91=EF=BC=9A=E6=A1=A3=E4=BD=8D=E9=87=8D=E5=88=92=E5=88=86?=
=?UTF-8?q?=E5=B9=B6=E6=96=B0=E5=A2=9ELEVEL=5F5=EF=BC=8C=E4=B8=AD=E4=BD=8D?=
=?UTF-8?q?=E5=81=9C=E6=AD=A2=E4=BB=85=E5=88=A4diff1~3=EF=BC=8C=E6=94=BE?=
=?UTF-8?q?=E5=AE=BD=E9=AB=98=E5=BA=A6=E9=99=90=E4=BD=8D=E6=A0=A1=E9=9B=B6?=
=?UTF-8?q?=E6=9D=A1=E4=BB=B6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
Project/MDK/HC32F460.uvoptx | 5 +
Project/source/User/Inc/main.h | 2 +-
Project/source/User/Src/main.c | 162 ++++++++++++++++++++-------------
3 files changed, 105 insertions(+), 64 deletions(-)
diff --git a/Project/MDK/HC32F460.uvoptx b/Project/MDK/HC32F460.uvoptx
index 9c3cfd4..a349f1a 100644
--- a/Project/MDK/HC32F460.uvoptx
+++ b/Project/MDK/HC32F460.uvoptx
@@ -270,6 +270,11 @@
1
App.CentCollDelay1mSCnt,0x0A
+
+ 23
+ 1
+ App.Para.Layout.ControlLevel
+
0
diff --git a/Project/source/User/Inc/main.h b/Project/source/User/Inc/main.h
index e0cc140..cc4bb2f 100644
--- a/Project/source/User/Inc/main.h
+++ b/Project/source/User/Inc/main.h
@@ -10,7 +10,7 @@
#define FOLLOW_VPT 10
#define SHIELD_VPT 8000
#define EME_STOP_VPT 4000
-#define CENTRE_VPT 8000
+#define CENTRE_VPT 4000
#define CENTRE_SHIELD_VPT 2000
#define STOP_TIME_MS 500
diff --git a/Project/source/User/Src/main.c b/Project/source/User/Src/main.c
index e464351..e1cfdb8 100644
--- a/Project/source/User/Src/main.c
+++ b/Project/source/User/Src/main.c
@@ -78,20 +78,8 @@ void RIGHTLOW_CS1237AinADataProcessCallBack(int32_t AD)
}
void GradeControl(ControlLevel_m level)
{
- switch(level){
+ switch(level){
case LEVEL_1:{
- 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 = 4000;
- break;}
-
- case LEVEL_2:{
App.ADTrack.LeftUpVPT = 7000;
App.ADTrack.LeftLowVPT = 7000;
App.ADTrack.RightUpVPT = 7000;
@@ -103,7 +91,7 @@ void GradeControl(ControlLevel_m level)
App.ADTrack.Stop_VPT = 5000;
break;}
- case LEVEL_3:{
+ case LEVEL_2:{
App.ADTrack.LeftUpVPT = 10000;
App.ADTrack.LeftLowVPT = 10000;
App.ADTrack.RightUpVPT = 10000;
@@ -115,7 +103,7 @@ void GradeControl(ControlLevel_m level)
App.ADTrack.Stop_VPT = 6000;
break;}
- case LEVEL_4:{
+ case LEVEL_3:{
App.ADTrack.LeftUpVPT = 15000;
App.ADTrack.LeftLowVPT = 15000;
App.ADTrack.RightUpVPT = 15000;
@@ -127,7 +115,7 @@ void GradeControl(ControlLevel_m level)
App.ADTrack.Stop_VPT = 10000;
break;}
- case LEVEL_5:{
+ case LEVEL_4:{
App.ADTrack.LeftUpVPT = 20000;
App.ADTrack.LeftLowVPT = 20000;
App.ADTrack.RightUpVPT = 20000;
@@ -138,6 +126,18 @@ void GradeControl(ControlLevel_m level)
App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 15000;
break;}
+
+ case LEVEL_5:{
+ 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 = 18000;
+ break;}
}
}
#if(USE_AGC == 1)
@@ -356,15 +356,15 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (abs(Oru_diff3) < (abs(Orl_diff3) + CENTRE_SHIELD_VPT))
)//判断是否不是正面压
{
- BDC_UP();
- LED_RED_ON();
- LED_GREEN_OFF();
- App.CaliZeroFlag = false;
- App.UpFlag = true;
- App.StopFlag = false;
- App.DownFlag = false;
- App.EmeFlag = false;
- App.RunCollDelay1mSCnt = RUN_TIME_MS;
+ BDC_UP();
+ LED_RED_ON();
+ LED_GREEN_OFF();
+ App.CaliZeroFlag = false;
+ App.UpFlag = true;
+ App.StopFlag = false;
+ App.DownFlag = false;
+ App.EmeFlag = false;
+ App.RunCollDelay1mSCnt = RUN_TIME_MS;
}
}
else if(((Olu_diff1+Oru_diff1) < -(App.ADTrack.Stop_VPT))
@@ -529,6 +529,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
}
}
}
+
}
else if(App.UpFlag == true && App.RunCollDelay1mSCnt <= 0)
{
@@ -541,19 +542,27 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (Oll_diff2 > -App.ADTrack.Stop_VPT)
&& (Olu_diff3 < App.ADTrack.Stop_VPT)
&& (Oll_diff3 > -App.ADTrack.Stop_VPT)
- && (Olu_diff4 < App.ADTrack.Stop_VPT)
- && (Oll_diff4 > -App.ADTrack.Stop_VPT)
- && (Olu_diff5 < App.ADTrack.Stop_VPT)
- && (Oll_diff5 > -App.ADTrack.Stop_VPT)
- && (Olu_diff6 < App.ADTrack.Stop_VPT)
- && (Oll_diff6 > -App.ADTrack.Stop_VPT))
- || (((Olu_diff1 + Oru_diff1) < (App.ADTrack.Stop_VPT*2/3))
+// && (Olu_diff4 < App.ADTrack.Stop_VPT)
+// && (Oll_diff4 > -App.ADTrack.Stop_VPT)
+// && (Olu_diff5 < App.ADTrack.Stop_VPT)
+// && (Oll_diff5 > -App.ADTrack.Stop_VPT)
+// && (Olu_diff6 < App.ADTrack.Stop_VPT)
+// && (Oll_diff6 > -App.ADTrack.Stop_VPT)
+ )
+ || (App.CentreRunFalg == true
+ && ((Olu_diff1 + Oru_diff1) < (App.ADTrack.Stop_VPT*2/3))
&& ((Olu_diff2 + Oru_diff2) < (App.ADTrack.Stop_VPT*2/3))
&& ((Olu_diff3 + Oru_diff3) < (App.ADTrack.Stop_VPT*2/3))
+// && ((Olu_diff4 + Oru_diff4) < (App.ADTrack.Stop_VPT*2/3))
+// && ((Olu_diff5 + Oru_diff5) < (App.ADTrack.Stop_VPT*2/3))
+// && ((Olu_diff6 + Oru_diff6) < (App.ADTrack.Stop_VPT*2/3))
&& ((Oll_diff1 + Orl_diff1) > -(App.ADTrack.Stop_VPT*2/3))
&& ((Oll_diff2 + Orl_diff2) > -(App.ADTrack.Stop_VPT*2/3))
- && ((Oll_diff3 + Orl_diff3) > -(App.ADTrack.Stop_VPT*2/3)))
- )
+ && ((Oll_diff3 + Orl_diff3) > -(App.ADTrack.Stop_VPT*2/3))
+// && ((Oll_diff4 + Orl_diff4) > -(App.ADTrack.Stop_VPT*2/3))
+// && ((Oll_diff5 + Orl_diff5) > -(App.ADTrack.Stop_VPT*2/3))
+// && ((Oll_diff6 + Orl_diff6) > -(App.ADTrack.Stop_VPT*2/3))
+ ))
{
BDC_STOP();
LED_RED_OFF();
@@ -587,19 +596,27 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (Orl_diff2 > -App.ADTrack.Stop_VPT)
&& (Oru_diff3 < App.ADTrack.Stop_VPT)
&& (Orl_diff3 > -App.ADTrack.Stop_VPT)
- && (Oru_diff4 < App.ADTrack.Stop_VPT)
- && (Orl_diff4 > -App.ADTrack.Stop_VPT)
- && (Oru_diff5 < App.ADTrack.Stop_VPT)
- && (Orl_diff5 > -App.ADTrack.Stop_VPT)
- && (Oru_diff6 < App.ADTrack.Stop_VPT)
- && (Orl_diff6 > -App.ADTrack.Stop_VPT))
- || (((Olu_diff1 + Oru_diff1) < (App.ADTrack.Stop_VPT*2/3))
+// && (Oru_diff4 < App.ADTrack.Stop_VPT)
+// && (Orl_diff4 > -App.ADTrack.Stop_VPT)
+// && (Oru_diff5 < App.ADTrack.Stop_VPT)
+// && (Orl_diff5 > -App.ADTrack.Stop_VPT)
+// && (Oru_diff6 < App.ADTrack.Stop_VPT)
+// && (Orl_diff6 > -App.ADTrack.Stop_VPT)
+ )
+ || (App.CentreRunFalg == true
+ &&((Olu_diff1 + Oru_diff1) < (App.ADTrack.Stop_VPT*2/3))
&& ((Olu_diff2 + Oru_diff2) < (App.ADTrack.Stop_VPT*2/3))
&& ((Olu_diff3 + Oru_diff3) < (App.ADTrack.Stop_VPT*2/3))
+// && ((Olu_diff4 + Oru_diff4) < (App.ADTrack.Stop_VPT*2/3))
+// && ((Olu_diff5 + Oru_diff5) < (App.ADTrack.Stop_VPT*2/3))
+// && ((Olu_diff6 + Oru_diff6) < (App.ADTrack.Stop_VPT*2/3))
&& ((Oll_diff1 + Orl_diff1) > -(App.ADTrack.Stop_VPT*2/3))
&& ((Oll_diff2 + Orl_diff2) > -(App.ADTrack.Stop_VPT*2/3))
- && ((Oll_diff3 + Orl_diff3) > -(App.ADTrack.Stop_VPT*2/3)))
- )
+ && ((Oll_diff3 + Orl_diff3) > -(App.ADTrack.Stop_VPT*2/3))
+// && ((Oll_diff4 + Orl_diff4) > -(App.ADTrack.Stop_VPT*2/3))
+// && ((Oll_diff5 + Orl_diff5) > -(App.ADTrack.Stop_VPT*2/3))
+// && ((Oll_diff6 + Orl_diff6) > -(App.ADTrack.Stop_VPT*2/3))
+ ))
{
BDC_STOP();
LED_RED_OFF();
@@ -636,19 +653,27 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (Oll_diff2 < App.ADTrack.Stop_VPT)
&& (Olu_diff3 > -App.ADTrack.Stop_VPT)
&& (Oll_diff3 < App.ADTrack.Stop_VPT)
- && (Olu_diff4 > -App.ADTrack.Stop_VPT)
- && (Oll_diff4 < App.ADTrack.Stop_VPT)
- && (Olu_diff5 > -App.ADTrack.Stop_VPT)
- && (Oll_diff5 < App.ADTrack.Stop_VPT)
- && (Olu_diff6 > -App.ADTrack.Stop_VPT)
- && (Oll_diff6 < App.ADTrack.Stop_VPT))
- || (((Olu_diff1 + Oru_diff1) > -(App.ADTrack.Stop_VPT*2/3))
+// && (Olu_diff4 > -App.ADTrack.Stop_VPT)
+// && (Oll_diff4 < App.ADTrack.Stop_VPT)
+// && (Olu_diff5 > -App.ADTrack.Stop_VPT)
+// && (Oll_diff5 < App.ADTrack.Stop_VPT)
+// && (Olu_diff6 > -App.ADTrack.Stop_VPT)
+// && (Oll_diff6 < App.ADTrack.Stop_VPT)
+ )
+ || (App.CentreRunFalg == true
+ && ((Olu_diff1 + Oru_diff1) > -(App.ADTrack.Stop_VPT*2/3))
&& ((Olu_diff2 + Oru_diff2) > -(App.ADTrack.Stop_VPT*2/3))
&& ((Olu_diff3 + Oru_diff3) > -(App.ADTrack.Stop_VPT*2/3))
+// && ((Olu_diff4 + Oru_diff4) > -(App.ADTrack.Stop_VPT*2/3))
+// && ((Olu_diff5 + Oru_diff5) > -(App.ADTrack.Stop_VPT*2/3))
+// && ((Olu_diff6 + Oru_diff6) > -(App.ADTrack.Stop_VPT*2/3))
&& ((Oll_diff1 + Orl_diff1) < (App.ADTrack.Stop_VPT*2/3))
&& ((Oll_diff2 + Orl_diff2) < (App.ADTrack.Stop_VPT*2/3))
- && ((Oll_diff3 + Orl_diff3) < (App.ADTrack.Stop_VPT*2/3)))
- )
+ && ((Oll_diff3 + Orl_diff3) < (App.ADTrack.Stop_VPT*2/3))
+// && ((Oll_diff4 + Orl_diff4) < (App.ADTrack.Stop_VPT*2/3))
+// && ((Oll_diff5 + Orl_diff5) < (App.ADTrack.Stop_VPT*2/3))
+// && ((Oll_diff6 + Orl_diff6) < (App.ADTrack.Stop_VPT*2/3))
+ ))
{
BDC_STOP();
LED_RED_OFF();
@@ -682,19 +707,27 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (Orl_diff2 < App.ADTrack.Stop_VPT)
&& (Oru_diff3 > -App.ADTrack.Stop_VPT)
&& (Orl_diff3 < App.ADTrack.Stop_VPT)
- && (Oru_diff4 > -App.ADTrack.Stop_VPT)
- && (Orl_diff4 < App.ADTrack.Stop_VPT)
- && (Oru_diff5 > -App.ADTrack.Stop_VPT)
- && (Orl_diff5 < App.ADTrack.Stop_VPT)
- && (Oru_diff6 > -App.ADTrack.Stop_VPT)
- && (Orl_diff6 < App.ADTrack.Stop_VPT))
- || (((Olu_diff1 + Oru_diff1) > -(App.ADTrack.Stop_VPT*2/3))
+// && (Oru_diff4 > -App.ADTrack.Stop_VPT)
+// && (Orl_diff4 < App.ADTrack.Stop_VPT)
+// && (Oru_diff5 > -App.ADTrack.Stop_VPT)
+// && (Orl_diff5 < App.ADTrack.Stop_VPT)
+// && (Oru_diff6 > -App.ADTrack.Stop_VPT)
+// && (Orl_diff6 < App.ADTrack.Stop_VPT)
+ )
+ || (App.CentreRunFalg == true
+ && ((Olu_diff1 + Oru_diff1) > -(App.ADTrack.Stop_VPT*2/3))
&& ((Olu_diff2 + Oru_diff2) > -(App.ADTrack.Stop_VPT*2/3))
&& ((Olu_diff3 + Oru_diff3) > -(App.ADTrack.Stop_VPT*2/3))
+// && ((Olu_diff4 + Oru_diff4) > -(App.ADTrack.Stop_VPT*2/3))
+// && ((Olu_diff5 + Oru_diff5) > -(App.ADTrack.Stop_VPT*2/3))
+// && ((Olu_diff6 + Oru_diff6) > -(App.ADTrack.Stop_VPT*2/3))
&& ((Oll_diff1 + Orl_diff1) < (App.ADTrack.Stop_VPT*2/3))
&& ((Oll_diff2 + Orl_diff2) < (App.ADTrack.Stop_VPT*2/3))
- && ((Oll_diff3 + Orl_diff3) < (App.ADTrack.Stop_VPT*2/3)))
- )
+ && ((Oll_diff3 + Orl_diff3) < (App.ADTrack.Stop_VPT*2/3))
+// && ((Oll_diff4 + Orl_diff4) < (App.ADTrack.Stop_VPT*2/3))
+// && ((Oll_diff5 + Orl_diff5) < (App.ADTrack.Stop_VPT*2/3))
+// && ((Oll_diff6 + Orl_diff6) < (App.ADTrack.Stop_VPT*2/3))
+ ))
{
BDC_STOP();
LED_RED_OFF();
@@ -755,8 +788,11 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& App.RightLowCS1237_SlopeArray[2] < FOLLOW_VPT
&& App.RightLowCS1237_SlopeArray[2] > -FOLLOW_VPT)
{
- if(App.StopFlag == true
- && App.StopCollDelay1mSCnt <= 0){
+ if((App.StopFlag == true
+ && App.StopCollDelay1mSCnt <= 0)
+ || (App.MotorPara.Current_height >= 1440)
+ || (App.MotorPara.Current_height <= 1000))
+ {
App.ADTrack.LeftUp_Org_AdZero = lu_ad[0];
App.ADTrack.LeftLow_Org_AdZero = ll_ad[0];
App.ADTrack.RightUp_Org_AdZero = ru_ad[0];