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/source/User/Inc/main.h
+1 -1
View File
@@ -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
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)
{
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];