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/Inc/main.h
+2
View File
@@ -19,6 +19,7 @@
#define RUN_TIME_MS 0 #define RUN_TIME_MS 0
#define EME_START_MS 1000 #define EME_START_MS 1000
#define EME_STOP_MS 2000 #define EME_STOP_MS 2000
#define SLOPE_START_MS 3000
#define TRAVEL_TIME_MS 30000 #define TRAVEL_TIME_MS 30000
@@ -107,6 +108,7 @@ typedef struct {
uint32_t StopCollDelay1mSCnt; uint32_t StopCollDelay1mSCnt;
uint32_t RunCollDelay1mSCnt; uint32_t RunCollDelay1mSCnt;
uint32_t EmergencyDelay1mSCnt; uint32_t EmergencyDelay1mSCnt;
uint32_t SlopeCollDelay1mSCnt;
uint8_t FeedDogDlyCnt; uint8_t FeedDogDlyCnt;
bool UpFlag;//上升标志位 bool UpFlag;//上升标志位
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.UpFlag = false;
App.EmeFlag = false; App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS; App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.SlopeCollDelay1mSCnt = SLOPE_START_MS;
} }
} }
else if(App.StopFlag == true && App.StopCollDelay1mSCnt <= 0) 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.LeftUpCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) && (App.LeftLowCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] >= 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_diff1 >= App.ADTrack.LeftUpVPT*5/6)
&& (Olu_diff2 >= 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.LeftUpCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) && (App.LeftLowCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] <= -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)
&& (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.RightUpCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)
&& (App.RightLowCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) && (App.RightLowCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
&& (App.RightLowCS1237_SlopeArray[1] >= 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_diff1 >= App.ADTrack.RightUpVPT*5/6)
&& (Oru_diff2 >= App.ADTrack.RightUpVPT*5/6) && (Oru_diff2 >= App.ADTrack.RightUpVPT*5/6)
&& (Oru_diff3 >= 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.RightUpCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)
&& (App.RightLowCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) && (App.RightLowCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT)
&& (App.RightLowCS1237_SlopeArray[1] <= -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_diff1 <= -App.ADTrack.RightUpVPT)
&& (Oru_diff2 <= -App.ADTrack.RightUpVPT) && (Oru_diff2 <= -App.ADTrack.RightUpVPT)
&& (Oru_diff3 <= -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.UpFlag = false;
App.EmeFlag = false; App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS; 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)) else if(((App.CentreRunFalg == false ) && (Oll_diff1 < App.ADTrack.Stop_VPT))
|| ((App.CentreRunFalg == true) && ((Oll_diff1 + Orl_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.UpFlag = false;
App.EmeFlag = false; App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS; 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) else if((abs(Oll_diff1) - EME_STOP_VPT) < abs(Orl_diff1)
&& (abs(Oll_diff2) - EME_STOP_VPT) < abs(Orl_diff2) && (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.UpFlag = false;
App.EmeFlag = false; App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS; 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)) else if(((App.CentreRunFalg == false ) && (Orl_diff1 < App.ADTrack.Stop_VPT))
|| ((App.CentreRunFalg == true) && ((Oll_diff1 + 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.UpFlag = false;
App.EmeFlag = false; App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS; 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) else if(abs(Oll_diff1) > (abs(Orl_diff1) - EME_STOP_VPT)
&& abs(Oll_diff2) > (abs(Orl_diff2) - 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.UpFlag = false;
App.EmeFlag = false; App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS; 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)) else if(((App.CentreRunFalg == false ) && (Oll_diff1 > -App.ADTrack.Stop_VPT))
|| ((App.CentreRunFalg == true) && ((Oll_diff1 + Orl_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.UpFlag = false;
App.EmeFlag = false; App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS; 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) else if((abs(Oll_diff1) - EME_STOP_VPT) < abs(Orl_diff1)
&& (abs(Oll_diff2) - EME_STOP_VPT) < abs(Orl_diff2) && (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.UpFlag = false;
App.EmeFlag = false; App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS; 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)) else if(((App.CentreRunFalg == false ) && (Orl_diff1 > -App.ADTrack.Stop_VPT))
|| ((App.CentreRunFalg == true) && ((Oll_diff1 + 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.UpFlag = false;
App.EmeFlag = false; App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS; 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) else if(abs(Oll_diff1) > (abs(Orl_diff1) - EME_STOP_VPT)
&& abs(Oll_diff2) > (abs(Orl_diff2) - EME_STOP_VPT) && abs(Oll_diff2) > (abs(Orl_diff2) - EME_STOP_VPT)
@@ -876,6 +881,8 @@ static void App1mSRoutine(void)
App.RunCollDelay1mSCnt--; App.RunCollDelay1mSCnt--;
if(App.EmergencyDelay1mSCnt > 0) if(App.EmergencyDelay1mSCnt > 0)
App.EmergencyDelay1mSCnt--; App.EmergencyDelay1mSCnt--;
if(App.SlopeCollDelay1mSCnt > 0)
App.SlopeCollDelay1mSCnt--;
App.FeedDogDlyCnt++; App.FeedDogDlyCnt++;
} }
void FeedDogHandler(void) void FeedDogHandler(void)