YuanHongbin/SingleColumn_Trolley
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

调整档位参数及停止判断逻辑：档位重划分并新增LEVEL_5，中位停止仅判diff1~3，放宽高度限位校零条件

Browse Source
This commit is contained in:
DESKTOP-KQHDCM4\97555
2026-09-04 11:28:05 +08:00
parent 26cccd60a0
commit 18ebcb7c10
3 changed files with 105 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Project/MDK/HC32F460.uvoptx
+5
View File
@@ -270,6 +270,11 @@
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>App.CentCollDelay1mSCnt,0x0A</ItemText> <ItemText>App.CentCollDelay1mSCnt,0x0A</ItemText>
</Ww> </Ww>
<Ww>
<count>23</count>
<WinNumber>1</WinNumber>
<ItemText>App.Para.Layout.ControlLevel</ItemText>
</Ww>
</WatchWindow1> </WatchWindow1>
<Tracepoint> <Tracepoint>
<THDelay>0</THDelay> <THDelay>0</THDelay>
Project/source/User/Inc/main.h
+1 -1
View File
@@ -10,7 +10,7 @@
#define FOLLOW_VPT 10 #define FOLLOW_VPT 10
#define SHIELD_VPT 8000 #define SHIELD_VPT 8000
#define EME_STOP_VPT 4000 #define EME_STOP_VPT 4000
#define CENTRE_VPT 8000 #define CENTRE_VPT 4000
#define CENTRE_SHIELD_VPT 2000 #define CENTRE_SHIELD_VPT 2000
#define STOP_TIME_MS 500 #define STOP_TIME_MS 500
Project/source/User/Src/main.c
+99 -63
View File
@@ -78,20 +78,8 @@ void RIGHTLOW_CS1237AinADataProcessCallBack(int32_t AD)
} }
void GradeControl(ControlLevel_m level) void GradeControl(ControlLevel_m level)
{ {
switch(level){ switch(level){
case LEVEL_1:{ 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.LeftUpVPT = 7000;
App.ADTrack.LeftLowVPT = 7000; App.ADTrack.LeftLowVPT = 7000;
App.ADTrack.RightUpVPT = 7000; App.ADTrack.RightUpVPT = 7000;
@@ -103,7 +91,7 @@ void GradeControl(ControlLevel_m level)
App.ADTrack.Stop_VPT = 5000; App.ADTrack.Stop_VPT = 5000;
break;} break;}
case LEVEL_3:{ case LEVEL_2:{
App.ADTrack.LeftUpVPT = 10000; App.ADTrack.LeftUpVPT = 10000;
App.ADTrack.LeftLowVPT = 10000; App.ADTrack.LeftLowVPT = 10000;
App.ADTrack.RightUpVPT = 10000; App.ADTrack.RightUpVPT = 10000;
@@ -115,7 +103,7 @@ void GradeControl(ControlLevel_m level)
App.ADTrack.Stop_VPT = 6000; App.ADTrack.Stop_VPT = 6000;
break;} break;}
case LEVEL_4:{ case LEVEL_3:{
App.ADTrack.LeftUpVPT = 15000; App.ADTrack.LeftUpVPT = 15000;
App.ADTrack.LeftLowVPT = 15000; App.ADTrack.LeftLowVPT = 15000;
App.ADTrack.RightUpVPT = 15000; App.ADTrack.RightUpVPT = 15000;
@@ -127,7 +115,7 @@ void GradeControl(ControlLevel_m level)
App.ADTrack.Stop_VPT = 10000; App.ADTrack.Stop_VPT = 10000;
break;} break;}
case LEVEL_5:{ case LEVEL_4:{
App.ADTrack.LeftUpVPT = 20000; App.ADTrack.LeftUpVPT = 20000;
App.ADTrack.LeftLowVPT = 20000; App.ADTrack.LeftLowVPT = 20000;
App.ADTrack.RightUpVPT = 20000; App.ADTrack.RightUpVPT = 20000;
@@ -138,6 +126,18 @@ void GradeControl(ControlLevel_m level)
App.ADTrack.RightLowShieldVPT = SHIELD_VPT; App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 15000; App.ADTrack.Stop_VPT = 15000;
break;} 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) #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)) && (abs(Oru_diff3) < (abs(Orl_diff3) + CENTRE_SHIELD_VPT))
)//判断是否不是正面压 )//判断是否不是正面压
{ {
BDC_UP(); BDC_UP();
LED_RED_ON(); LED_RED_ON();
LED_GREEN_OFF(); LED_GREEN_OFF();
App.CaliZeroFlag = false; App.CaliZeroFlag = false;
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;
} }
} }
else if(((Olu_diff1+Oru_diff1) < -(App.ADTrack.Stop_VPT)) 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) 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) && (Oll_diff2 > -App.ADTrack.Stop_VPT)
&& (Olu_diff3 < App.ADTrack.Stop_VPT) && (Olu_diff3 < App.ADTrack.Stop_VPT)
&& (Oll_diff3 > -App.ADTrack.Stop_VPT) && (Oll_diff3 > -App.ADTrack.Stop_VPT)
&& (Olu_diff4 < App.ADTrack.Stop_VPT) // && (Olu_diff4 < App.ADTrack.Stop_VPT)
&& (Oll_diff4 > -App.ADTrack.Stop_VPT) // && (Oll_diff4 > -App.ADTrack.Stop_VPT)
&& (Olu_diff5 < App.ADTrack.Stop_VPT) // && (Olu_diff5 < App.ADTrack.Stop_VPT)
&& (Oll_diff5 > -App.ADTrack.Stop_VPT) // && (Oll_diff5 > -App.ADTrack.Stop_VPT)
&& (Olu_diff6 < App.ADTrack.Stop_VPT) // && (Olu_diff6 < App.ADTrack.Stop_VPT)
&& (Oll_diff6 > -App.ADTrack.Stop_VPT)) // && (Oll_diff6 > -App.ADTrack.Stop_VPT)
|| (((Olu_diff1 + Oru_diff1) < (App.ADTrack.Stop_VPT*2/3)) )
|| (App.CentreRunFalg == true
&& ((Olu_diff1 + Oru_diff1) < (App.ADTrack.Stop_VPT*2/3))
&& ((Olu_diff2 + Oru_diff2) < (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_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_diff1 + Orl_diff1) > -(App.ADTrack.Stop_VPT*2/3))
&& ((Oll_diff2 + Orl_diff2) > -(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(); BDC_STOP();
LED_RED_OFF(); 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) && (Orl_diff2 > -App.ADTrack.Stop_VPT)
&& (Oru_diff3 < App.ADTrack.Stop_VPT) && (Oru_diff3 < App.ADTrack.Stop_VPT)
&& (Orl_diff3 > -App.ADTrack.Stop_VPT) && (Orl_diff3 > -App.ADTrack.Stop_VPT)
&& (Oru_diff4 < App.ADTrack.Stop_VPT) // && (Oru_diff4 < App.ADTrack.Stop_VPT)
&& (Orl_diff4 > -App.ADTrack.Stop_VPT) // && (Orl_diff4 > -App.ADTrack.Stop_VPT)
&& (Oru_diff5 < App.ADTrack.Stop_VPT) // && (Oru_diff5 < App.ADTrack.Stop_VPT)
&& (Orl_diff5 > -App.ADTrack.Stop_VPT) // && (Orl_diff5 > -App.ADTrack.Stop_VPT)
&& (Oru_diff6 < App.ADTrack.Stop_VPT) // && (Oru_diff6 < App.ADTrack.Stop_VPT)
&& (Orl_diff6 > -App.ADTrack.Stop_VPT)) // && (Orl_diff6 > -App.ADTrack.Stop_VPT)
|| (((Olu_diff1 + Oru_diff1) < (App.ADTrack.Stop_VPT*2/3)) )
|| (App.CentreRunFalg == true
&&((Olu_diff1 + Oru_diff1) < (App.ADTrack.Stop_VPT*2/3))
&& ((Olu_diff2 + Oru_diff2) < (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_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_diff1 + Orl_diff1) > -(App.ADTrack.Stop_VPT*2/3))
&& ((Oll_diff2 + Orl_diff2) > -(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(); BDC_STOP();
LED_RED_OFF(); 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) && (Oll_diff2 < App.ADTrack.Stop_VPT)
&& (Olu_diff3 > -App.ADTrack.Stop_VPT) && (Olu_diff3 > -App.ADTrack.Stop_VPT)
&& (Oll_diff3 < App.ADTrack.Stop_VPT) && (Oll_diff3 < App.ADTrack.Stop_VPT)
&& (Olu_diff4 > -App.ADTrack.Stop_VPT) // && (Olu_diff4 > -App.ADTrack.Stop_VPT)
&& (Oll_diff4 < App.ADTrack.Stop_VPT) // && (Oll_diff4 < App.ADTrack.Stop_VPT)
&& (Olu_diff5 > -App.ADTrack.Stop_VPT) // && (Olu_diff5 > -App.ADTrack.Stop_VPT)
&& (Oll_diff5 < App.ADTrack.Stop_VPT) // && (Oll_diff5 < App.ADTrack.Stop_VPT)
&& (Olu_diff6 > -App.ADTrack.Stop_VPT) // && (Olu_diff6 > -App.ADTrack.Stop_VPT)
&& (Oll_diff6 < App.ADTrack.Stop_VPT)) // && (Oll_diff6 < App.ADTrack.Stop_VPT)
|| (((Olu_diff1 + Oru_diff1) > -(App.ADTrack.Stop_VPT*2/3)) )
|| (App.CentreRunFalg == true
&& ((Olu_diff1 + Oru_diff1) > -(App.ADTrack.Stop_VPT*2/3))
&& ((Olu_diff2 + Oru_diff2) > -(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_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_diff1 + Orl_diff1) < (App.ADTrack.Stop_VPT*2/3))
&& ((Oll_diff2 + Orl_diff2) < (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(); BDC_STOP();
LED_RED_OFF(); 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) && (Orl_diff2 < App.ADTrack.Stop_VPT)
&& (Oru_diff3 > -App.ADTrack.Stop_VPT) && (Oru_diff3 > -App.ADTrack.Stop_VPT)
&& (Orl_diff3 < App.ADTrack.Stop_VPT) && (Orl_diff3 < App.ADTrack.Stop_VPT)
&& (Oru_diff4 > -App.ADTrack.Stop_VPT) // && (Oru_diff4 > -App.ADTrack.Stop_VPT)
&& (Orl_diff4 < App.ADTrack.Stop_VPT) // && (Orl_diff4 < App.ADTrack.Stop_VPT)
&& (Oru_diff5 > -App.ADTrack.Stop_VPT) // && (Oru_diff5 > -App.ADTrack.Stop_VPT)
&& (Orl_diff5 < App.ADTrack.Stop_VPT) // && (Orl_diff5 < App.ADTrack.Stop_VPT)
&& (Oru_diff6 > -App.ADTrack.Stop_VPT) // && (Oru_diff6 > -App.ADTrack.Stop_VPT)
&& (Orl_diff6 < App.ADTrack.Stop_VPT)) // && (Orl_diff6 < App.ADTrack.Stop_VPT)
|| (((Olu_diff1 + Oru_diff1) > -(App.ADTrack.Stop_VPT*2/3)) )
|| (App.CentreRunFalg == true
&& ((Olu_diff1 + Oru_diff1) > -(App.ADTrack.Stop_VPT*2/3))
&& ((Olu_diff2 + Oru_diff2) > -(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_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_diff1 + Orl_diff1) < (App.ADTrack.Stop_VPT*2/3))
&& ((Oll_diff2 + Orl_diff2) < (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(); BDC_STOP();
LED_RED_OFF(); 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
&& App.RightLowCS1237_SlopeArray[2] > -FOLLOW_VPT) && App.RightLowCS1237_SlopeArray[2] > -FOLLOW_VPT)
{ {
if(App.StopFlag == true if((App.StopFlag == true
&& App.StopCollDelay1mSCnt <= 0){ && App.StopCollDelay1mSCnt <= 0)
|| (App.MotorPara.Current_height >= 1440)
|| (App.MotorPara.Current_height <= 1000))
{
App.ADTrack.LeftUp_Org_AdZero = lu_ad[0]; App.ADTrack.LeftUp_Org_AdZero = lu_ad[0];
App.ADTrack.LeftLow_Org_AdZero = ll_ad[0]; App.ADTrack.LeftLow_Org_AdZero = ll_ad[0];
App.ADTrack.RightUp_Org_AdZero = ru_ad[0]; App.ADTrack.RightUp_Org_AdZero = ru_ad[0];