YuanHongbin/Acc_Support_29V_HV2.x
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

算法优化

Browse Source
This commit is contained in:
DESKTOP-KQHDCM4\97555
2025-10-30 16:28:23 +08:00
parent cde156a030
commit 7dfff142c2
2 changed files with 21 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Project/HC32F460/source/User/Src/main.c
+19 -12
View File
@@ -245,6 +245,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.SlopeCollDelay1mSCnt = SLOPE_START_MS;
}
}
else if(App.StopFlag == true && App.StopCollDelay1mSCnt <= 0)
@@ -320,7 +321,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (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))
&& (App.LeftLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)
&& (App.SlopeCollDelay1mSCnt <= 0))
|| (
(Olu_diff1 >= App.ADTrack.LeftUpVPT*5/6)
&& (Olu_diff2 >= App.ADTrack.LeftUpVPT*5/6)
@@ -347,7 +349,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (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))
&& (App.LeftLowCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)
&& ((App.SlopeCollDelay1mSCnt <= 0)))
|| (
(Olu_diff1 <= -App.ADTrack.LeftUpVPT)
&& (Olu_diff1 <= -App.ADTrack.LeftUpVPT)
@@ -378,7 +381,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (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))
&& (App.RightLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)
&& (App.SlopeCollDelay1mSCnt <= 0))
|| ((Oru_diff1 >= App.ADTrack.RightUpVPT*5/6)
&& (Oru_diff2 >= App.ADTrack.RightUpVPT*5/6)
&& (Oru_diff3 >= App.ADTrack.RightUpVPT*5/6)
@@ -404,7 +408,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (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))
&& (App.RightLowCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)
&& (App.SlopeCollDelay1mSCnt <= 0))
|| ((Oru_diff1 <= -App.ADTrack.RightUpVPT)
&& (Oru_diff2 <= -App.ADTrack.RightUpVPT)
&& (Oru_diff3 <= -App.ADTrack.RightUpVPT)
@@ -441,7 +446,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
App.SlopeCollDelay1mSCnt = SLOPE_START_MS;
}
else if(((App.CentreRunFalg == false ) && (Oll_diff1 < App.ADTrack.Stop_VPT))
|| ((App.CentreRunFalg == true) && ((Oll_diff1 + Orl_diff1) < (App.ADTrack.Stop_VPT))))
@@ -452,7 +457,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
App.SlopeCollDelay1mSCnt = SLOPE_START_MS;
}
else if((abs(Oll_diff1) - EME_STOP_VPT) < abs(Orl_diff1)
&& (abs(Oll_diff2) - EME_STOP_VPT) < abs(Orl_diff2)
@@ -475,7 +480,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
App.SlopeCollDelay1mSCnt = SLOPE_START_MS;
}
else if(((App.CentreRunFalg == false ) && (Orl_diff1 < App.ADTrack.Stop_VPT))
|| ((App.CentreRunFalg == true) && ((Oll_diff1 + Orl_diff1) < (App.ADTrack.Stop_VPT))))
@@ -486,7 +491,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
App.SlopeCollDelay1mSCnt = SLOPE_START_MS;
}
else if(abs(Oll_diff1) > (abs(Orl_diff1) - EME_STOP_VPT)
&& abs(Oll_diff2) > (abs(Orl_diff2) - EME_STOP_VPT)
@@ -512,7 +517,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
App.SlopeCollDelay1mSCnt = SLOPE_START_MS;
}
else if(((App.CentreRunFalg == false ) && (Oll_diff1 > -App.ADTrack.Stop_VPT))
|| ((App.CentreRunFalg == true) && ((Oll_diff1 + Orl_diff1) > -App.ADTrack.Stop_VPT)))
@@ -523,7 +528,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
App.SlopeCollDelay1mSCnt = SLOPE_START_MS;
}
else if((abs(Oll_diff1) - EME_STOP_VPT) < abs(Orl_diff1)
&& (abs(Oll_diff2) - EME_STOP_VPT) < abs(Orl_diff2)
@@ -546,7 +551,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
App.SlopeCollDelay1mSCnt = SLOPE_START_MS;
}
else if(((App.CentreRunFalg == false ) && (Orl_diff1 > -App.ADTrack.Stop_VPT))
|| ((App.CentreRunFalg == true) && ((Oll_diff1 + Orl_diff1) > -(App.ADTrack.Stop_VPT))))
@@ -557,7 +562,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
App.SlopeCollDelay1mSCnt = SLOPE_START_MS;
}
else if(abs(Oll_diff1) > (abs(Orl_diff1) - EME_STOP_VPT)
&& abs(Oll_diff2) > (abs(Orl_diff2) - EME_STOP_VPT)
@@ -876,6 +881,8 @@ static void App1mSRoutine(void)
App.RunCollDelay1mSCnt--;
if(App.EmergencyDelay1mSCnt > 0)
App.EmergencyDelay1mSCnt--;
if(App.SlopeCollDelay1mSCnt > 0)
App.SlopeCollDelay1mSCnt--;
App.FeedDogDlyCnt++;
}
void FeedDogHandler(void)