diff --git a/Project/HC32F460/MDK/HC32F460.uvoptx b/Project/HC32F460/MDK/HC32F460.uvoptx index df0776c..c738b1b 100644 --- a/Project/HC32F460/MDK/HC32F460.uvoptx +++ b/Project/HC32F460/MDK/HC32F460.uvoptx @@ -225,11 +225,6 @@ 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 3e3526b..82d5c43 100644 --- a/Project/HC32F460/source/User/Inc/main.h +++ b/Project/HC32F460/source/User/Inc/main.h @@ -14,7 +14,7 @@ #define FOLLOW_VPT 20 #define SHIELD_VPT 2500 -#define STOP_TIME_MS 500 +#define STOP_TIME_MS 0 #define RUN_TIME_MS 0 #define TRAVEL_TIME_MS 30000 diff --git a/Project/HC32F460/source/User/Src/main.c b/Project/HC32F460/source/User/Src/main.c index c122142..742f3c3 100644 --- a/Project/HC32F460/source/User/Src/main.c +++ b/Project/HC32F460/source/User/Src/main.c @@ -217,37 +217,67 @@ 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(((lu_ad[0] < App.ADTrack.LeftUp_Org_AdZero-1000) + && (ll_ad[0] > App.ADTrack.LeftLow_Org_AdZero+1000) + && (ru_ad[0] < App.ADTrack.RightUp_Org_AdZero-1000) + && (rl_ad[0] > App.ADTrack.RightLow_Org_AdZero+1000)) + || ((lu_ad[0] > App.ADTrack.LeftUp_Org_AdZero+1000) + && (ll_ad[0] < App.ADTrack.LeftLow_Org_AdZero-1000) + && (ru_ad[0] > App.ADTrack.RightUp_Org_AdZero+1000) + && (rl_ad[0] < App.ADTrack.RightLow_Org_AdZero-1000))) { App.CentreRunFalg = true; if(abs(Oll_diff1) > abs(Orl_diff1)) { App.LeftRunFalg = true; + if((App.LeftLowCS1237_SlopeArray[0] > App.ADTrack.Slope_VPT/3)) + { + if(Oll_diff1 < -App.ADTrack.LeftLowVPT/2) + { + BDC_UP(); + App.UpFlag = true; + App.StopFlag = false; + App.DownFlag = false; + App.RunCollDelay1mSCnt = RUN_TIME_MS; + } + } + else if((App.LeftLowCS1237_SlopeArray[0] < -App.ADTrack.Slope_VPT/3)) + { + if(Oll_diff1 > App.ADTrack.LeftLowVPT/2) + { + BDC_DOWN(); + App.UpFlag = false; + App.StopFlag = false; + App.DownFlag = true; + App.RunCollDelay1mSCnt = RUN_TIME_MS; + } + } } else if(abs(Oll_diff1) < abs(Orl_diff1)) { App.RightRunFalg = true; - } - 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 + 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; + if((App.RightLowCS1237_SlopeArray[0] > App.ADTrack.Slope_VPT/3)) + { + if(Orl_diff1 < -App.ADTrack.RightLowVPT/2) + { + BDC_UP(); + App.UpFlag = true; + App.StopFlag = false; + App.DownFlag = false; + App.RunCollDelay1mSCnt = RUN_TIME_MS; + } + } + else if((App.RightLowCS1237_SlopeArray[0] < -App.ADTrack.Slope_VPT/3)) + { + if(Orl_diff1 > App.ADTrack.RightLowVPT/2) + { + BDC_DOWN(); + App.UpFlag = false; + App.StopFlag = false; + App.DownFlag = true; + App.RunCollDelay1mSCnt = RUN_TIME_MS; + } + } } } else if((LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum)) @@ -360,7 +390,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ if(App.LeftRunFalg == true) { 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)) + || ((App.CentreRunFalg == true ) && (App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) < -App.ADTrack.Slope_VPT)) { BDC_STOP(); App.StopFlag = true; @@ -380,8 +410,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ } else if(App.RightRunFalg == true) { - 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)) + 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)) { BDC_STOP(); App.StopFlag = true; @@ -405,7 +435,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ if(App.LeftRunFalg == true) { 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)) + || ((App.CentreRunFalg == true ) && (App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT)) { BDC_STOP(); App.StopFlag = true; @@ -426,7 +456,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ else if(App.RightRunFalg == true) { 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)) + || ((App.CentreRunFalg == true ) && (App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT)) { BDC_STOP(); App.StopFlag = true; @@ -538,10 +568,10 @@ static void AppInit(void) RIGHTUP_CS1237_Init(); RIGHTLOW_CS1237_Init(); - App.Status = APP_STATUS_WAIT; - App.TD1mSDelayCnt = 1000; -// App.Status = APP_STATUS_UP1; -// App.TD1mSDelayCnt = TRAVEL_TIME_MS; +// App.Status = APP_STATUS_WAIT; +// App.TD1mSDelayCnt = 1000; + App.Status = APP_STATUS_UP1; + App.TD1mSDelayCnt = TRAVEL_TIME_MS; App.RStatus = READ_STATUS_START; GradeControl(App.Para.Layout.ControlLevel);//µÈ¼¶¿ØÖÆ