diff --git a/Project/HC32F460/MDK/HC32F460.uvoptx b/Project/HC32F460/MDK/HC32F460.uvoptx index c738b1b..df0776c 100644 --- a/Project/HC32F460/MDK/HC32F460.uvoptx +++ b/Project/HC32F460/MDK/HC32F460.uvoptx @@ -225,6 +225,11 @@ 1 App.CentreRunFalg,0x0A + + 14 + 1 + App.EvenRunDelay1mSCnt + diff --git a/Project/HC32F460/source/User/Inc/main.h b/Project/HC32F460/source/User/Inc/main.h index 412a337..3e3526b 100644 --- a/Project/HC32F460/source/User/Inc/main.h +++ b/Project/HC32F460/source/User/Inc/main.h @@ -98,6 +98,7 @@ typedef struct { uint32_t Read1mSDelayCnt; uint32_t Uart1Delay1mSCnt; uint32_t StopCollDelay1mSCnt; + uint32_t RunCollDelay1mSCnt; uint32_t EvenRunDelay1mSCnt; uint8_t FeedDogDlyCnt; diff --git a/Project/HC32F460/source/User/Src/main.c b/Project/HC32F460/source/User/Src/main.c index 7f279fc..c122142 100644 --- a/Project/HC32F460/source/User/Src/main.c +++ b/Project/HC32F460/source/User/Src/main.c @@ -217,10 +217,10 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.LeftRunFalg = false; App.RightRunFalg = false; App.CentreRunFalg = false; - if(((ll_ad[0] < App.ADTrack.LeftLow_Org_AdZero) - && (rl_ad[0] < App.ADTrack.RightLow_Org_AdZero)) - || ((ll_ad[0] > App.ADTrack.LeftLow_Org_AdZero) - && (rl_ad[0] > App.ADTrack.RightLow_Org_AdZero))) + if(((ll_ad[0] > App.ADTrack.LeftLow_Org_AdZero) + && (rl_ad[0] > App.ADTrack.RightLow_Org_AdZero)) + || ((ll_ad[0] < App.ADTrack.LeftLow_Org_AdZero) + && (rl_ad[0] < App.ADTrack.RightLow_Org_AdZero))) { App.CentreRunFalg = true; if(abs(Oll_diff1) > abs(Orl_diff1)) @@ -231,21 +231,23 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ { App.RightRunFalg = true; } - if(Oll_diff1 < -App.ADTrack.LeftLowVPT - || Orl_diff1 < -App.ADTrack.RightLowVPT) + if(((Oll_diff1 + Orl_diff1) < -(App.ADTrack.LeftLowVPT + App.ADTrack.RightLowVPT)/2) + && (((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) < -App.ADTrack.Slope_VPT/4))) { BDC_UP(); App.UpFlag = true; App.StopFlag = false; App.DownFlag = false; + App.RunCollDelay1mSCnt = RUN_TIME_MS; } - else if(Oll_diff1 > App.ADTrack.LeftLowVPT - || Orl_diff1 > App.ADTrack.RightLowVPT) + else if(((Oll_diff1 + Orl_diff1) > (App.ADTrack.LeftLowVPT + App.ADTrack.RightLowVPT)/2) + && (((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT/4))) { BDC_DOWN(); App.UpFlag = false; App.StopFlag = false; App.DownFlag = true; + App.RunCollDelay1mSCnt = RUN_TIME_MS; } } else if((LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum)) @@ -353,11 +355,12 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ } } } - else if(App.DownFlag == true) + else if(App.DownFlag == true && App.RunCollDelay1mSCnt <= 0) { if(App.LeftRunFalg == true) { - if(ll_slope > App.ADTrack.Slope_VPT) + if(((App.CentreRunFalg == false ) && (ll_slope > App.ADTrack.Slope_VPT)) + || ((App.CentreRunFalg == true ) && (App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) < -App.ADTrack.Slope_VPT/3)) { BDC_STOP(); App.StopFlag = true; @@ -365,7 +368,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.UpFlag = false; App.StopCollDelay1mSCnt = STOP_TIME_MS; } - else if(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)))) { BDC_STOP(); App.StopFlag = true; @@ -376,7 +380,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ } else if(App.RightRunFalg == true) { - if(rl_slope > App.ADTrack.Slope_VPT) + if(((App.CentreRunFalg == false ) && (ll_slope > App.ADTrack.Slope_VPT)) + || ((App.CentreRunFalg == true ) && (App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) < -App.ADTrack.Slope_VPT/3)) { BDC_STOP(); App.StopFlag = true; @@ -384,7 +389,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.UpFlag = false; App.StopCollDelay1mSCnt = STOP_TIME_MS; } - else if(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)))) { BDC_STOP(); App.StopFlag = true; @@ -394,11 +400,12 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ } } } - else if(App.UpFlag == true) + else if(App.UpFlag == true && App.RunCollDelay1mSCnt <= 0) { if(App.LeftRunFalg == true) { - if(ll_slope < -App.ADTrack.Slope_VPT) + if(((App.CentreRunFalg == false ) && (ll_slope < -App.ADTrack.Slope_VPT)) + || ((App.CentreRunFalg == true ) && (App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT/3)) { BDC_STOP(); App.StopFlag = true; @@ -406,7 +413,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.UpFlag = false; App.StopCollDelay1mSCnt = STOP_TIME_MS; } - else if(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)))) { BDC_STOP(); App.StopFlag = true; @@ -417,7 +425,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ } else if(App.RightRunFalg == true) { - if(rl_slope < -App.ADTrack.Slope_VPT) + if(((App.CentreRunFalg == false ) && (rl_slope < -App.ADTrack.Slope_VPT)) + || ((App.CentreRunFalg == true ) && (App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT/3)) { BDC_STOP(); App.StopFlag = true; @@ -425,7 +434,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.UpFlag = false; App.StopCollDelay1mSCnt = STOP_TIME_MS; } - else if(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)))) { BDC_STOP(); App.StopFlag = true; @@ -529,9 +539,9 @@ static void AppInit(void) RIGHTLOW_CS1237_Init(); App.Status = APP_STATUS_WAIT; - App.Status = APP_STATUS_UP1; - App.TD1mSDelayCnt = TRAVEL_TIME_MS; - //App.TD1mSDelayCnt = 1000; + App.TD1mSDelayCnt = 1000; +// App.Status = APP_STATUS_UP1; +// App.TD1mSDelayCnt = TRAVEL_TIME_MS; App.RStatus = READ_STATUS_START; GradeControl(App.Para.Layout.ControlLevel);//µÈ¼¶¿ØÖÆ @@ -756,6 +766,8 @@ static void App1mSRoutine(void) App.Uart1Delay1mSCnt--; if(App.StopCollDelay1mSCnt > 0) App.StopCollDelay1mSCnt--; + if(App.RunCollDelay1mSCnt > 0) + App.RunCollDelay1mSCnt--; if(App.EvenRunning == true) App.EvenRunDelay1mSCnt++;