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/source/User/Src/bsp.c
+3
View File
@@ -1347,6 +1347,8 @@ void BSP_Init(void)
SystemClockConfig();
SystemCoreClockUpdate();
//Xtal32_ClockConfig();
GPIO_Config();
ExtiCh10_Init();
ExtiCh01_Init();
@@ -1364,6 +1366,7 @@ void BSP_Init(void)
//Adc1_Config();
//Dma_Config();
//FeedDog();
SysTick_Init(1000u);
SysTick_Delay(1000);
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.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 10000;
break;}
case LEVEL_2:{
@@ -99,6 +100,7 @@ void GradeControl(ControlLevel_m level)
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 20000;
break;}
case LEVEL_3:{
@@ -111,6 +113,7 @@ void GradeControl(ControlLevel_m level)
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 30000/2;
break;}
case LEVEL_4:{
@@ -123,6 +126,7 @@ void GradeControl(ControlLevel_m level)
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 40000/2;
break;}
case LEVEL_5:{
@@ -135,6 +139,7 @@ void GradeControl(ControlLevel_m level)
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 50000/2;
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 RightSlopeSum = abs(App.RightUpCS1237_SlopeArray[0]) + abs(App.RightLowCS1237_SlopeArray[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 LeftDiffSum = abs(lu_diff[0]) + abs(ll_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.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;
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[1] >= 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_diff3 >= App.ADTrack.LeftUpVPT)
&& (Oll_diff1 <= -App.ADTrack.LeftLowVPT)
&& (Oll_diff2 <= -App.ADTrack.LeftLowVPT)
&& (Oll_diff3 <= -App.ADTrack.LeftLowVPT)))
{
if((abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT)
&& (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)
)
{
BDC_UP();
App.UpFlag = true;
App.StopFlag = false;
App.DownFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
}
}
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[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)
&& (Olu_diff1 <= -App.ADTrack.LeftUpVPT)
&& (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)))
{
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();
App.UpFlag = false;
App.DownFlag = true;
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)))
{
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();
App.UpFlag = true;
App.StopFlag = false;
App.DownFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
}
}
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)
&& (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT))
&& (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT)
)
{
BDC_DOWN();
App.DownFlag = true;
App.StopFlag = false;
App.UpFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
}
}
}
}
}
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(lu_slope > App.ADTrack.Slope_VPT
&& ll_slope < -App.ADTrack.Slope_VPT
&& App.RunCollDelay1mSCnt <= 0)
if(ll_slope > App.ADTrack.Slope_VPT)
{
BDC_STOP();
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.StopCollDelay1mSCnt = STOP_TIME_MS;
}
else if(Olu_diff1 < (App.ADTrack.LeftUpVPT / 2)
&& Oll_diff1 > -(App.ADTrack.LeftLowVPT / 2))
else if(Oll_diff1 < App.ADTrack.Stop_VPT)
{
BDC_STOP();
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)
{
if(ru_slope > App.ADTrack.Slope_VPT
&& rl_slope < -App.ADTrack.Slope_VPT
&& App.RunCollDelay1mSCnt <= 0)
if(rl_slope > App.ADTrack.Slope_VPT)
{
BDC_STOP();
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.StopCollDelay1mSCnt = STOP_TIME_MS;
}
else if(Oru_diff1 < (App.ADTrack.RightUpVPT / 2)
&& Orl_diff1 > -(App.ADTrack.RightLowVPT / 2)
)
else if(Orl_diff1 < App.ADTrack.Stop_VPT)
{
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();
App.StopFlag = true;
@@ -500,8 +528,10 @@ static void AppInit(void)
RIGHTUP_CS1237_Init();
RIGHTLOW_CS1237_Init();
App.Status = APP_STATUS_DOWN_2S;
App.TD1mSDelayCnt = 10000;
App.Status = APP_STATUS_WAIT;
App.Status = APP_STATUS_UP1;
App.TD1mSDelayCnt = TRAVEL_TIME_MS;
//App.TD1mSDelayCnt = 1000;
App.RStatus = READ_STATUS_START;
GradeControl(App.Para.Layout.ControlLevel);//等级控制
@@ -620,24 +650,75 @@ static void AppLoopHandler(void)
App.Status = APP_STATUS_IDLE;
break;}
case APP_STATUS_UP_2S:{
BDC_UP();
if(App.TD1mSDelayCnt <= 0)
case APP_STATUS_UP1:{
if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-500))
{
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();
}
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;}
case APP_STATUS_DOWN_2S:{
BDC_DOWN();
if(App.TD1mSDelayCnt <= 0)
case APP_STATUS_DOWN:{
if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-500))
{
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();
}
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;}
case APP_STATUS_WAIT:{
@@ -658,7 +739,9 @@ static void AppLoopHandler(void)
App.ADTrack.LeftLow_Org_AdZero,
App.ADTrack.RightUp_Org_AdZero,
App.ADTrack.RightLow_Org_AdZero);
App.Status = APP_STATUS_IDLE;
App.TD1mSDelayCnt = 0;
}
break;}
}
@@ -673,8 +756,6 @@ static void App1mSRoutine(void)
App.Uart1Delay1mSCnt--;
if(App.StopCollDelay1mSCnt > 0)
App.StopCollDelay1mSCnt--;
if(App.RunCollDelay1mSCnt > 0)
App.RunCollDelay1mSCnt--;
if(App.EvenRunning == true)
App.EvenRunDelay1mSCnt++;