YuanHongbin/Acc_Support_29V_HV2.x
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

2025-10-15 下班备份

Browse Source
This commit is contained in:
DESKTOP-KQHDCM4\97555
2025-10-15 18:46:13 +08:00
parent 92bb9bf934
commit 3b1cce2275
4 changed files with 195 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Project/HC32F460/MDK/HC32F460.uvoptx
+15
View File
@@ -210,6 +210,21 @@
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>App.ADTrack.RightLow_Org_AdZero,0x0A</ItemText> <ItemText>App.ADTrack.RightLow_Org_AdZero,0x0A</ItemText>
</Ww> </Ww>
<Ww>
<count>11</count>
<WinNumber>1</WinNumber>
<ItemText>App.LeftRunFalg,0x0A</ItemText>
</Ww>
<Ww>
<count>12</count>
<WinNumber>1</WinNumber>
<ItemText>App.RightRunFalg,0x0A</ItemText>
</Ww>
<Ww>
<count>13</count>
<WinNumber>1</WinNumber>
<ItemText>App.CentreRunFalg,0x0A</ItemText>
</Ww>
</WatchWindow1> </WatchWindow1>
<MemoryWindow1> <MemoryWindow1>
<Mm> <Mm>
Project/HC32F460/source/User/Inc/main.h
+10 -5
View File
@@ -12,14 +12,17 @@
#define SLOPE_VPT 1000 #define SLOPE_VPT 1000
#define FOLLOW_VPT 20 #define FOLLOW_VPT 20
#define SHIELD_VPT 5000 #define SHIELD_VPT 2500
#define STOP_TIME_MS 500 #define STOP_TIME_MS 500
#define RUN_TIME_MS 0 #define RUN_TIME_MS 0
#define TRAVEL_TIME_MS 30000
#define BDC_STOP() (SET_TWIN_BIT0(),SET_TWIN_BIT1()) #define BDC_STOP() (SET_TWIN_BIT0(),SET_TWIN_BIT1())
#define BDC_UP() (SET_TWIN_BIT0(),CLR_TWIN_BIT1()) #define BDC_UP() (SET_TWIN_BIT0(),CLR_TWIN_BIT1())
#define BDC_DOWN() (CLR_TWIN_BIT0(),SET_TWIN_BIT1()) #define BDC_DOWN() (SET_TWIN_BIT1(),CLR_TWIN_BIT0())
/*控制等级*/ /*控制等级*/
typedef enum { typedef enum {
@@ -33,6 +36,7 @@ typedef enum {
/*设备阈值*/ /*设备阈值*/
typedef struct{ typedef struct{
int32_t Slope_VPT; int32_t Slope_VPT;
int32_t Stop_VPT;
int32_t LeftUpVPT;//左上设备阈值 int32_t LeftUpVPT;//左上设备阈值
int32_t LeftLowVPT;//左下设备阈值 int32_t LeftLowVPT;//左下设备阈值
@@ -70,8 +74,9 @@ typedef enum {
APP_STATUS_IDLE, APP_STATUS_IDLE,
APP_STATUS_ON, APP_STATUS_ON,
APP_STATUS_OFF, APP_STATUS_OFF,
APP_STATUS_UP_2S, APP_STATUS_UP1,
APP_STATUS_DOWN_2S, APP_STATUS_DOWN,
APP_STATUS_UP2,
APP_STATUS_WAIT, APP_STATUS_WAIT,
}AppStatus_m; }AppStatus_m;
@@ -93,7 +98,6 @@ typedef struct {
uint32_t Read1mSDelayCnt; uint32_t Read1mSDelayCnt;
uint32_t Uart1Delay1mSCnt; uint32_t Uart1Delay1mSCnt;
uint32_t StopCollDelay1mSCnt; uint32_t StopCollDelay1mSCnt;
uint32_t RunCollDelay1mSCnt;
uint32_t EvenRunDelay1mSCnt; uint32_t EvenRunDelay1mSCnt;
uint8_t FeedDogDlyCnt; uint8_t FeedDogDlyCnt;
@@ -103,6 +107,7 @@ typedef struct {
bool EvenRunning;//平稳运行标志位 bool EvenRunning;//平稳运行标志位
bool LeftRunFalg; bool LeftRunFalg;
bool RightRunFalg; bool RightRunFalg;
bool CentreRunFalg;
bool LeftUpCS1237ReadEndFlag; bool LeftUpCS1237ReadEndFlag;
bool LeftLowCS1237ReadEndFlag; bool LeftLowCS1237ReadEndFlag;
Project/HC32F460/source/User/Src/bsp.c
+3
View File
@@ -1347,6 +1347,8 @@ void BSP_Init(void)
SystemClockConfig(); SystemClockConfig();
SystemCoreClockUpdate(); SystemCoreClockUpdate();
//Xtal32_ClockConfig(); //Xtal32_ClockConfig();
GPIO_Config(); GPIO_Config();
ExtiCh10_Init(); ExtiCh10_Init();
ExtiCh01_Init(); ExtiCh01_Init();
@@ -1364,6 +1366,7 @@ void BSP_Init(void)
//Adc1_Config(); //Adc1_Config();
//Dma_Config(); //Dma_Config();
//FeedDog(); //FeedDog();
SysTick_Init(1000u); SysTick_Init(1000u);
SysTick_Delay(1000); SysTick_Delay(1000);
CLK_GetClockFreq(&Freq); CLK_GetClockFreq(&Freq);
Project/HC32F460/source/User/Src/main.c
+167 -86
View File
@@ -87,6 +87,7 @@ void GradeControl(ControlLevel_m level)
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT; App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT; App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT; App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 10000;
break;} break;}
case LEVEL_2:{ case LEVEL_2:{
@@ -99,6 +100,7 @@ void GradeControl(ControlLevel_m level)
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT; App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT; App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT; App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 20000;
break;} break;}
case LEVEL_3:{ case LEVEL_3:{
@@ -111,6 +113,7 @@ void GradeControl(ControlLevel_m level)
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT; App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT; App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT; App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 30000/2;
break;} break;}
case LEVEL_4:{ case LEVEL_4:{
@@ -123,6 +126,7 @@ void GradeControl(ControlLevel_m level)
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT; App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT; App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT; App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 40000/2;
break;} break;}
case LEVEL_5:{ case LEVEL_5:{
@@ -135,6 +139,7 @@ void GradeControl(ControlLevel_m level)
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT; App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT; App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT; App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 50000/2;
break;} break;}
} }
} }
@@ -204,12 +209,46 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
{ {
uint32_t LeftSlopeSum = abs(App.LeftUpCS1237_SlopeArray[0]) + abs(App.LeftLowCS1237_SlopeArray[0]); uint32_t LeftSlopeSum = abs(App.LeftUpCS1237_SlopeArray[0]) + abs(App.LeftLowCS1237_SlopeArray[0]);
uint32_t RightSlopeSum = abs(App.RightUpCS1237_SlopeArray[0]) + abs(App.RightLowCS1237_SlopeArray[0]); uint32_t RightSlopeSum = abs(App.RightUpCS1237_SlopeArray[0]) + abs(App.RightLowCS1237_SlopeArray[0]);
uint32_t LeftDiffSum = abs(lu_diff[0]) + abs(ll_diff[0]); // uint32_t LeftDiffSum = abs(lu_diff[0]) + abs(ll_diff[0]);
uint32_t RightDiffSum = abs(ru_diff[0]) + abs(rl_diff[0]); // uint32_t RightDiffSum = abs(ru_diff[0]) + abs(rl_diff[0]);
uint32_t LeftDiffSum = abs(ll_diff[0]);
uint32_t RightDiffSum = abs(rl_diff[0]);
App.LeftRunFalg = false; App.LeftRunFalg = false;
App.RightRunFalg = false; App.RightRunFalg = false;
if(LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum) 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)))
{
App.CentreRunFalg = true;
if(abs(Oll_diff1) > abs(Orl_diff1))
{
App.LeftRunFalg = true;
}
else if(abs(Oll_diff1) < abs(Orl_diff1))
{
App.RightRunFalg = true;
}
if(Oll_diff1 < -App.ADTrack.LeftLowVPT
|| Orl_diff1 < -App.ADTrack.RightLowVPT)
{
BDC_UP();
App.UpFlag = true;
App.StopFlag = false;
App.DownFlag = false;
}
else if(Oll_diff1 > App.ADTrack.LeftLowVPT
|| Orl_diff1 > App.ADTrack.RightLowVPT)
{
BDC_DOWN();
App.UpFlag = false;
App.StopFlag = false;
App.DownFlag = true;
}
}
else if((LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum))
{ {
App.LeftRunFalg = true; App.LeftRunFalg = true;
if(((App.LeftUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) if(((App.LeftUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT)
@@ -218,21 +257,22 @@ 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[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))
|| ((Olu_diff1 >= App.ADTrack.LeftUpVPT) || (
(Olu_diff1 >= App.ADTrack.LeftUpVPT)
&& (Olu_diff2 >= App.ADTrack.LeftUpVPT) && (Olu_diff2 >= App.ADTrack.LeftUpVPT)
&& (Olu_diff3 >= App.ADTrack.LeftUpVPT) && (Olu_diff3 >= App.ADTrack.LeftUpVPT)
&& (Oll_diff1 <= -App.ADTrack.LeftLowVPT) && (Oll_diff1 <= -App.ADTrack.LeftLowVPT)
&& (Oll_diff2 <= -App.ADTrack.LeftLowVPT) && (Oll_diff2 <= -App.ADTrack.LeftLowVPT)
&& (Oll_diff3 <= -App.ADTrack.LeftLowVPT))) && (Oll_diff3 <= -App.ADTrack.LeftLowVPT)))
{ {
if((abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT) if((abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT)
&& (abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT)) && (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT)
)
{ {
BDC_UP(); BDC_UP();
App.UpFlag = true; App.UpFlag = true;
App.StopFlag = false; App.StopFlag = false;
App.DownFlag = false; App.DownFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
} }
} }
else if(((App.LeftUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) else if(((App.LeftUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
@@ -241,7 +281,8 @@ 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[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))
|| ((Olu_diff1 <= -App.ADTrack.LeftUpVPT) || (
(Olu_diff1 <= -App.ADTrack.LeftUpVPT)
&& (Olu_diff1 <= -App.ADTrack.LeftUpVPT) && (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)
@@ -249,13 +290,13 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (Oll_diff1 >= App.ADTrack.LeftLowVPT))) && (Oll_diff1 >= App.ADTrack.LeftLowVPT)))
{ {
if((abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT) if((abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT)
&& (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT)) && (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT)
)
{ {
BDC_DOWN(); BDC_DOWN();
App.UpFlag = false; App.UpFlag = false;
App.DownFlag = true; App.DownFlag = true;
App.StopFlag = false; App.StopFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
} }
} }
} }
@@ -276,13 +317,13 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (Orl_diff3 <= -App.ADTrack.RightLowVPT))) && (Orl_diff3 <= -App.ADTrack.RightLowVPT)))
{ {
if((abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT) if((abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT)
&& (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT)) && (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT)
)
{ {
BDC_UP(); BDC_UP();
App.UpFlag = true; App.UpFlag = true;
App.StopFlag = false; App.StopFlag = false;
App.DownFlag = false; App.DownFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
} }
} }
else if(((App.RightUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) else if(((App.RightUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
@@ -300,72 +341,23 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
) )
{ {
if((abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT) if((abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT)
&& (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT)) && (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT)
)
{ {
BDC_DOWN(); BDC_DOWN();
App.DownFlag = true; App.DownFlag = true;
App.StopFlag = false; App.StopFlag = false;
App.UpFlag = false; App.UpFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
} }
} }
} }
} }
} }
else if(App.DownFlag == true) else if(App.DownFlag == true)
{
if(App.LeftRunFalg == true)
{
if(lu_slope < -App.ADTrack.Slope_VPT
&& ll_slope > App.ADTrack.Slope_VPT
&& App.RunCollDelay1mSCnt <= 0)
{
BDC_STOP();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
else if(Olu_diff1 > -(App.ADTrack.LeftUpVPT / 2)
&& Oll_diff1 < (App.ADTrack.LeftLowVPT / 2))
{
BDC_STOP();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
}
else if(App.RightRunFalg == true)
{
if(ru_slope < -App.ADTrack.Slope_VPT
&& rl_slope > App.ADTrack.Slope_VPT
&& App.RunCollDelay1mSCnt <= 0)
{
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;
}
}
}
else if(App.UpFlag == true)
{ {
if(App.LeftRunFalg == true) if(App.LeftRunFalg == true)
{ {
if(lu_slope > App.ADTrack.Slope_VPT if(ll_slope > App.ADTrack.Slope_VPT)
&& ll_slope < -App.ADTrack.Slope_VPT
&& App.RunCollDelay1mSCnt <= 0)
{ {
BDC_STOP(); BDC_STOP();
App.StopFlag = true; App.StopFlag = true;
@@ -373,8 +365,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.UpFlag = false; App.UpFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS; App.StopCollDelay1mSCnt = STOP_TIME_MS;
} }
else if(Olu_diff1 < (App.ADTrack.LeftUpVPT / 2) else if(Oll_diff1 < App.ADTrack.Stop_VPT)
&& Oll_diff1 > -(App.ADTrack.LeftLowVPT / 2))
{ {
BDC_STOP(); BDC_STOP();
App.StopFlag = true; App.StopFlag = true;
@@ -385,9 +376,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
} }
else if(App.RightRunFalg == true) else if(App.RightRunFalg == true)
{ {
if(ru_slope > App.ADTrack.Slope_VPT if(rl_slope > App.ADTrack.Slope_VPT)
&& rl_slope < -App.ADTrack.Slope_VPT
&& App.RunCollDelay1mSCnt <= 0)
{ {
BDC_STOP(); BDC_STOP();
App.StopFlag = true; App.StopFlag = true;
@@ -395,9 +384,48 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.UpFlag = false; App.UpFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS; App.StopCollDelay1mSCnt = STOP_TIME_MS;
} }
else if(Oru_diff1 < (App.ADTrack.RightUpVPT / 2) else if(Orl_diff1 < App.ADTrack.Stop_VPT)
&& Orl_diff1 > -(App.ADTrack.RightLowVPT / 2) {
) BDC_STOP();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
}
}
else if(App.UpFlag == true)
{
if(App.LeftRunFalg == true)
{
if(ll_slope < -App.ADTrack.Slope_VPT)
{
BDC_STOP();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
else if(Oll_diff1 > -App.ADTrack.Stop_VPT)
{
BDC_STOP();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
}
else if(App.RightRunFalg == true)
{
if(rl_slope < -App.ADTrack.Slope_VPT)
{
BDC_STOP();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
else if(Orl_diff1 > -App.ADTrack.Stop_VPT)
{ {
BDC_STOP(); BDC_STOP();
App.StopFlag = true; App.StopFlag = true;
@@ -500,8 +528,10 @@ static void AppInit(void)
RIGHTUP_CS1237_Init(); RIGHTUP_CS1237_Init();
RIGHTLOW_CS1237_Init(); RIGHTLOW_CS1237_Init();
App.Status = APP_STATUS_DOWN_2S; App.Status = APP_STATUS_WAIT;
App.TD1mSDelayCnt = 10000; App.Status = APP_STATUS_UP1;
App.TD1mSDelayCnt = TRAVEL_TIME_MS;
//App.TD1mSDelayCnt = 1000;
App.RStatus = READ_STATUS_START; App.RStatus = READ_STATUS_START;
GradeControl(App.Para.Layout.ControlLevel);//等级控制 GradeControl(App.Para.Layout.ControlLevel);//等级控制
@@ -620,24 +650,75 @@ static void AppLoopHandler(void)
App.Status = APP_STATUS_IDLE; App.Status = APP_STATUS_IDLE;
break;} break;}
case APP_STATUS_UP_2S:{ case APP_STATUS_UP1:{
BDC_UP(); if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-500))
if(App.TD1mSDelayCnt <= 0) {
BDC_UP();
}
else if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-2500) && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-500))
{ {
App.TD1mSDelayCnt = 2000;
App.Status = APP_STATUS_DOWN_2S;
BDC_STOP(); BDC_STOP();
} }
else if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-25000) && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-3000))
{
BDC_UP();
}
else if(App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-25000))
{
BDC_STOP();
App.Status = APP_STATUS_DOWN;
App.TD1mSDelayCnt = TRAVEL_TIME_MS;
}
break;} break;}
case APP_STATUS_DOWN_2S:{ case APP_STATUS_DOWN:{
BDC_DOWN(); if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-500))
if(App.TD1mSDelayCnt <= 0) {
BDC_DOWN();
}
else if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-1500) && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-500))
{ {
App.TD1mSDelayCnt = 2000;
App.Status = APP_STATUS_WAIT;
BDC_STOP(); BDC_STOP();
} }
else if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-2500) && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-1500))
{
BDC_DOWN();
}
else if(App.TD1mSDelayCnt >= (TRAVEL_TIME_MS-26100) && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-26000))
{
BDC_STOP();
}
else if(App.TD1mSDelayCnt > 0 && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-27100))
{
BDC_DOWN();
}
else if(App.TD1mSDelayCnt <= 0)
{
BDC_STOP();
App.Status = APP_STATUS_UP2;
App.TD1mSDelayCnt = TRAVEL_TIME_MS;
}
break;}
case APP_STATUS_UP2:{
if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-500))
{
BDC_UP();
}
else if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-2500) && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-500))
{
BDC_STOP();
}
else if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-25000) && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-3000))
{
BDC_UP();
}
else if(App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-25000))
{
BDC_STOP();
App.Status = APP_STATUS_WAIT;
App.TD1mSDelayCnt = 1000;
}
break;} break;}
case APP_STATUS_WAIT:{ case APP_STATUS_WAIT:{
@@ -658,7 +739,9 @@ static void AppLoopHandler(void)
App.ADTrack.LeftLow_Org_AdZero, App.ADTrack.LeftLow_Org_AdZero,
App.ADTrack.RightUp_Org_AdZero, App.ADTrack.RightUp_Org_AdZero,
App.ADTrack.RightLow_Org_AdZero); App.ADTrack.RightLow_Org_AdZero);
App.Status = APP_STATUS_IDLE; App.Status = APP_STATUS_IDLE;
App.TD1mSDelayCnt = 0;
} }
break;} break;}
} }
@@ -673,8 +756,6 @@ static void App1mSRoutine(void)
App.Uart1Delay1mSCnt--; App.Uart1Delay1mSCnt--;
if(App.StopCollDelay1mSCnt > 0) if(App.StopCollDelay1mSCnt > 0)
App.StopCollDelay1mSCnt--; App.StopCollDelay1mSCnt--;
if(App.RunCollDelay1mSCnt > 0)
App.RunCollDelay1mSCnt--;
if(App.EvenRunning == true) if(App.EvenRunning == true)
App.EvenRunDelay1mSCnt++; App.EvenRunDelay1mSCnt++;