diff --git a/Project/HC32F460/source/User/Inc/main.h b/Project/HC32F460/source/User/Inc/main.h index 6e94e63..e1e4bc3 100644 --- a/Project/HC32F460/source/User/Inc/main.h +++ b/Project/HC32F460/source/User/Inc/main.h @@ -9,10 +9,10 @@ #define OSC_CNT 40 -#define SLOPE_CNT 20 +#define SLOPE_CNT 40 #define STOP_TIME_MS 500 -#define RUN_TIME_MS 10 +#define RUN_TIME_MS 500 #define BDC_STOP() Motor_Control(MOTOR_CMD_STOP) #define BDC_UP() Motor_Control(MOTOR_CMD_FORWARD) diff --git a/Project/HC32F460/source/User/Src/main.c b/Project/HC32F460/source/User/Src/main.c index 79183d7..f4f2696 100644 --- a/Project/HC32F460/source/User/Src/main.c +++ b/Project/HC32F460/source/User/Src/main.c @@ -140,6 +140,15 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.RightRunFalg = false; if(LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum) { + uint32_t UpSlopeSum = abs(App.LeftUpCS1237_SlopeArray[0]) + abs(App.RightUpCS1237_SlopeArray[0]); + uint32_t LowSlopeSum = abs(App.LeftLowCS1237_SlopeArray[0]) + abs(App.RightUpCS1237_SlopeArray[0]); + uint32_t UpDiffSum = abs(lu_diff[0]) + abs(ru_diff[0]); + uint32_t LowDiffSum = abs(ll_diff[0]) + abs(rl_diff[0]); + +// if(UpSlopeSum > LowSlopeSum && UpDiffSum > LowDiffSum) +// { +// +// } App.LeftRunFalg = true; if(((App.LeftUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) && (App.LeftUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) @@ -147,12 +156,12 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (App.LeftLowCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) && (App.LeftLowCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) && (App.LeftLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)) -// || ((Olu_diff1 >= App.ADTrack.LeftUpVPT) -// && (Olu_diff2 >= App.ADTrack.LeftUpVPT) -// && (Olu_diff3 >= App.ADTrack.LeftUpVPT) -// && (Oll_diff1 <= -App.ADTrack.LeftLowVPT) -// && (Oll_diff2 <= -App.ADTrack.LeftLowVPT) -// && (Oll_diff3 <= -App.ADTrack.LeftLowVPT)) + || ((Olu_diff1 >= App.ADTrack.LeftUpVPT) + && (Olu_diff2 >= App.ADTrack.LeftUpVPT) + && (Olu_diff3 >= App.ADTrack.LeftUpVPT) + && (Oll_diff1 <= -App.ADTrack.LeftLowVPT) + && (Oll_diff2 <= -App.ADTrack.LeftLowVPT) + && (Oll_diff3 <= -App.ADTrack.LeftLowVPT)) ) { BDC_UP(); @@ -167,12 +176,12 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (App.LeftLowCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) && (App.LeftLowCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) && (App.LeftLowCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)) -// || ((Olu_diff1 <= -App.ADTrack.LeftUpVPT) -// && (Olu_diff1 <= -App.ADTrack.LeftUpVPT) -// && (Olu_diff1 <= -App.ADTrack.LeftUpVPT) -// && (Oll_diff1 >= App.ADTrack.LeftLowVPT) -// && (Oll_diff1 >= App.ADTrack.LeftLowVPT) -// && (Oll_diff1 >= App.ADTrack.LeftLowVPT)) + || ((Olu_diff1 <= -App.ADTrack.LeftUpVPT) + && (Olu_diff1 <= -App.ADTrack.LeftUpVPT) + && (Olu_diff1 <= -App.ADTrack.LeftUpVPT) + && (Oll_diff1 >= App.ADTrack.LeftLowVPT) + && (Oll_diff1 >= App.ADTrack.LeftLowVPT) + && (Oll_diff1 >= App.ADTrack.LeftLowVPT)) ) { BDC_DOWN(); @@ -191,12 +200,12 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (App.RightLowCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) && (App.RightLowCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) && (App.RightLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)) -// || ((Oru_diff1 >= App.ADTrack.RightUpVPT) -// && (Oru_diff2 >= App.ADTrack.RightUpVPT) -// && (Oru_diff3 >= App.ADTrack.RightUpVPT) -// && (Orl_diff1 <= -App.ADTrack.RightLowVPT) -// && (Orl_diff2 <= -App.ADTrack.RightLowVPT) -// && (Orl_diff3 <= -App.ADTrack.RightLowVPT)) + || ((Oru_diff1 >= App.ADTrack.RightUpVPT) + && (Oru_diff2 >= App.ADTrack.RightUpVPT) + && (Oru_diff3 >= App.ADTrack.RightUpVPT) + && (Orl_diff1 <= -App.ADTrack.RightLowVPT) + && (Orl_diff2 <= -App.ADTrack.RightLowVPT) + && (Orl_diff3 <= -App.ADTrack.RightLowVPT)) ) { BDC_UP(); @@ -211,12 +220,12 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (App.RightLowCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) && (App.RightLowCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) && (App.RightLowCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)) -// || ((Oru_diff1 <= -App.ADTrack.RightUpVPT) -// && (Oru_diff2 <= -App.ADTrack.RightUpVPT) -// && (Oru_diff3 <= -App.ADTrack.RightUpVPT) -// && (Orl_diff1 >= App.ADTrack.RightLowVPT) -// && (Orl_diff2 >= App.ADTrack.RightLowVPT) -// && (Orl_diff3 >= App.ADTrack.RightLowVPT)) + || ((Oru_diff1 <= -App.ADTrack.RightUpVPT) + && (Oru_diff2 <= -App.ADTrack.RightUpVPT) + && (Oru_diff3 <= -App.ADTrack.RightUpVPT) + && (Orl_diff1 >= App.ADTrack.RightLowVPT) + && (Orl_diff2 >= App.ADTrack.RightLowVPT) + && (Orl_diff3 >= App.ADTrack.RightLowVPT)) ) { BDC_DOWN(); @@ -256,7 +265,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ else if(App.RightRunFalg == true) { if(ru_slope < -App.ADTrack.Slope_VPT - //&& rl_slope > App.ADTrack.Slope_VPT + && rl_slope > App.ADTrack.Slope_VPT ) { BDC_STOP(); @@ -265,16 +274,15 @@ 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(Oru_diff1 > -(App.ADTrack.RightUpVPT / 2) -// //&& Orl_diff1 < (App.ADTrack.RightLowVPT / 2) -// ) -// { -// BDC_STOP(); -// App.StopFlag = true; -// App.DownFlag = false; -// App.UpFlag = false; -// App.StopCollDelay1mSCnt = STOP_TIME_MS; -// } + else if(Oru_diff1 > -(App.ADTrack.RightUpVPT / 2) + && Orl_diff1 < (App.ADTrack.RightLowVPT / 2)) + { + BDC_STOP(); + App.StopFlag = true; + App.DownFlag = false; + App.UpFlag = false; + App.StopCollDelay1mSCnt = STOP_TIME_MS; + } } } } @@ -306,7 +314,17 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ else if(App.RightRunFalg == true) { if(ru_slope > App.ADTrack.Slope_VPT - //&& rl_slope < -App.ADTrack.Slope_VPT + && rl_slope < -App.ADTrack.Slope_VPT + ) + { + BDC_STOP(); + App.StopFlag = true; + App.DownFlag = false; + App.UpFlag = false; + App.StopCollDelay1mSCnt = STOP_TIME_MS; + } + else if(Oru_diff1 < (App.ADTrack.RightUpVPT / 2) + && Orl_diff1 > -(App.ADTrack.RightLowVPT / 2) ) { BDC_STOP(); @@ -315,16 +333,6 @@ 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(Oru_diff1 < (App.ADTrack.RightUpVPT / 2) -// && Orl_diff1 > -(App.ADTrack.RightLowVPT / 2) -// ) -// { -// BDC_STOP(); -// App.StopFlag = true; -// App.DownFlag = false; -// App.UpFlag = false; -// App.StopCollDelay1mSCnt = STOP_TIME_MS; -// } } } } @@ -534,7 +542,7 @@ static void AppLoopHandler(void) App.StopFlag = true; App.EvenRunning = false; - App.ADTrack.Slope_VPT = 1500; + App.ADTrack.Slope_VPT = 2000; App.ADTrack.LeftUpVPT = 10000; App.ADTrack.LeftLowVPT = 10000; App.ADTrack.RightUpVPT = 10000;