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);//µÈ¼¶¿ØÖÆ