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

双柱双电机S型传感器独立分支所有基本功能完成

Browse Source
This commit is contained in:
DESKTOP-KQHDCM4\97555
2025-10-21 18:28:49 +08:00
parent 0094de177e
commit c2976eff6b
4 changed files with 258 additions and 339 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Project/HC32F460/source/User/Inc/main.h
+8 -5
View File
@@ -5,7 +5,7 @@
#define USE_FILE_SYSTEM 1
#define SDCARD_BLOCK_MAX (10000)
#define SDCARD_BLOCK_MAX (10000)
#define OSC_CNT 40
#define SLOPE_CNT 30
@@ -13,15 +13,18 @@
#define SLOPE_VPT 1000
#define FOLLOW_VPT 20
#define SHIELD_VPT 2500
#define EME_STOP_VPT 10000
#define STOP_TIME_MS 0
#define STOP_TIME_MS 500
#define RUN_TIME_MS 0
#define EME_START_MS 500
#define EME_STOP_MS 3000
#define TRAVEL_TIME_MS 30000
#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() (SET_TWIN_BIT1(),CLR_TWIN_BIT0())
/*控制等级*/
@@ -99,13 +102,13 @@ typedef struct {
uint32_t Uart1Delay1mSCnt;
uint32_t StopCollDelay1mSCnt;
uint32_t RunCollDelay1mSCnt;
uint32_t EvenRunDelay1mSCnt;
uint32_t EmergencyDelay1mSCnt;
uint8_t FeedDogDlyCnt;
bool UpFlag;//上升标志位
bool DownFlag;//下降标志位
bool StopFlag;//停止标志位
bool EvenRunning;//平稳运行标志位
bool EmeFlag;//应急标志位
bool LeftRunFalg;
bool RightRunFalg;
bool CentreRunFalg;
Project/HC32F460/source/User/Src/main.c
+223 -174
View File
@@ -203,184 +203,221 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
PeakValleyCheck ru_K = check_peaks_valleys(App.RightUpCS1237_SlopeArray, SLOPE_CNT);
PeakValleyCheck rl_K = check_peaks_valleys(App.RightLowCS1237_SlopeArray, SLOPE_CNT);
if(App.StopFlag == true)
int32_t LeftSlopeSum = abs(App.LeftUpCS1237_SlopeArray[0]) + abs(App.LeftLowCS1237_SlopeArray[0]);
int32_t RightSlopeSum = abs(App.RightUpCS1237_SlopeArray[0]) + abs(App.RightLowCS1237_SlopeArray[0]);
int32_t LeftLowDiff = abs(Oll_diff1);
int32_t RightLowDiff = abs(Orl_diff1);
int32_t LeftDiffSum = abs(lu_diff[0]);
int32_t RightDiffSum = abs(ru_diff[0]);
if(App.EmeFlag == true)
{
if(App.StopCollDelay1mSCnt <= 0)
if(App.DownFlag == true && App.EmergencyDelay1mSCnt > 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 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;
App.CentreRunFalg = false;
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((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))
BDC_UP();
}
else if(App.UpFlag == true && App.EmergencyDelay1mSCnt > 0)
{
BDC_DOWN();
}
else if(App.EmergencyDelay1mSCnt <= 0)
{
BDC_STOP();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
}
else if(App.StopFlag == true && App.StopCollDelay1mSCnt <= 0)
{
App.LeftRunFalg = false;
App.RightRunFalg = false;
App.CentreRunFalg = false;
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.LeftUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT)
&& (App.LeftUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)
&& (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)))
{
if((abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT)
&& (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT)
)
// if((App.LeftLowCS1237_SlopeArray[0] > App.ADTrack.Slope_VPT/3))
// {
if(Oll_diff1 < -App.ADTrack.LeftLowVPT/3)
{
BDC_UP();
App.UpFlag = true;
App.StopFlag = false;
App.DownFlag = false;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
}
}
else if(((App.LeftUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
&& (App.LeftUpCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)
&& (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)))
{
if((abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT)
&& (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT)
)
// }
// else if((App.LeftLowCS1237_SlopeArray[0] < -App.ADTrack.Slope_VPT/3))
// {
if(Oll_diff1 > App.ADTrack.LeftLowVPT/3)
{
BDC_DOWN();
App.UpFlag = false;
App.DownFlag = true;
App.StopFlag = false;
App.DownFlag = true;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
}
}
// }
}
else if(LeftSlopeSum < RightSlopeSum && LeftDiffSum < RightDiffSum)
else if(abs(Oll_diff1) < abs(Orl_diff1))
{
App.RightRunFalg = true;
if(((App.RightUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT)
&& (App.RightUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT)
&& (App.RightUpCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)
&& (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)))
if((App.RightLowCS1237_SlopeArray[0] > App.ADTrack.Slope_VPT/3))
{
if((abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT)
&& (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT)
)
if(Orl_diff1 < -App.ADTrack.RightLowVPT/3)
{
BDC_UP();
App.UpFlag = true;
App.StopFlag = false;
App.DownFlag = false;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
}
}
else if(((App.RightUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
&& (App.RightUpCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT)
&& (App.RightUpCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)
&& (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))
else if((App.RightLowCS1237_SlopeArray[0] < -App.ADTrack.Slope_VPT/3))
{
if(Orl_diff1 > App.ADTrack.RightLowVPT/3)
{
BDC_DOWN();
App.UpFlag = false;
App.StopFlag = false;
App.DownFlag = true;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
}
}
}
}
else if((LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum))
{
App.LeftRunFalg = true;
if(((App.LeftUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT)
&& (App.LeftUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)
&& (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)))
{
if((abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT)
// && (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT)
)
{
if((abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT)
&& (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT)
)
{
BDC_DOWN();
App.DownFlag = true;
App.StopFlag = false;
App.UpFlag = false;
}
BDC_UP();
App.UpFlag = true;
App.StopFlag = false;
App.DownFlag = false;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
}
}
else if(((App.LeftUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
&& (App.LeftUpCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)
&& (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)))
{
if((abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT)
// && (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT)
)
{
BDC_DOWN();
App.UpFlag = false;
App.DownFlag = true;
App.StopFlag = false;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
}
}
}
else if(LeftSlopeSum < RightSlopeSum && LeftDiffSum < RightDiffSum)
{
App.RightRunFalg = true;
if(((App.RightUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT)
&& (App.RightUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT)
&& (App.RightUpCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)
&& (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)))
{
if((abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT)
&& (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT)
)
{
BDC_UP();
App.UpFlag = true;
App.StopFlag = false;
App.DownFlag = false;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
}
}
else if(((App.RightUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
&& (App.RightUpCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT)
&& (App.RightUpCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)
&& (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))
)
{
if((abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT)
&& (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT)
)
{
BDC_DOWN();
App.DownFlag = true;
App.StopFlag = false;
App.UpFlag = false;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
}
}
}
@@ -396,6 +433,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
else if(((App.CentreRunFalg == false ) && (Oll_diff1 < App.ADTrack.Stop_VPT))
@@ -405,8 +443,15 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
else if((LeftLowDiff + EME_STOP_VPT) < RightLowDiff && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
{
BDC_STOP();
App.EmeFlag = true;
App.EmergencyDelay1mSCnt = EME_STOP_MS;
}
}
else if(App.RightRunFalg == true)
{
@@ -417,6 +462,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
else if(((App.CentreRunFalg == false ) && (Orl_diff1 < App.ADTrack.Stop_VPT))
@@ -426,8 +472,15 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
else if(LeftLowDiff > (RightLowDiff + EME_STOP_VPT) && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
{
BDC_STOP();
App.EmeFlag = true;
App.EmergencyDelay1mSCnt = EME_STOP_MS;
}
}
}
else if(App.UpFlag == true && App.RunCollDelay1mSCnt <= 0)
@@ -441,6 +494,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
else if(((App.CentreRunFalg == false ) && (Oll_diff1 > -App.ADTrack.Stop_VPT))
@@ -450,8 +504,15 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
else if((LeftLowDiff + EME_STOP_VPT) < RightLowDiff && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
{
BDC_STOP();
App.EmeFlag = true;
App.EmergencyDelay1mSCnt = EME_STOP_MS;
}
}
else if(App.RightRunFalg == true)
{
@@ -462,6 +523,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
else if(((App.CentreRunFalg == false ) && (Orl_diff1 > -App.ADTrack.Stop_VPT))
@@ -471,8 +533,15 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
else if(LeftLowDiff > (RightLowDiff + EME_STOP_VPT) && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
{
BDC_STOP();
App.EmeFlag = true;
App.EmergencyDelay1mSCnt = EME_STOP_MS;
}
}
}
@@ -515,25 +584,6 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.ADTrack.RightUp_Org_AdZero = ru_ad[0];
App.ADTrack.RightLow_Org_AdZero = rl_ad[0];
}
else if(App.StopFlag == false)
{
App.EvenRunning = true;
App.EvenRunDelay1mSCnt = 0;
}
}
else
{
App.EvenRunning = false;
App.EvenRunDelay1mSCnt = 0;
}
if(App.EvenRunDelay1mSCnt >= 2000)
{
App.EvenRunning = false;
App.EvenRunDelay1mSCnt = 0;
App.ADTrack.LeftUp_Org_AdZero = lu_ad[0];
App.ADTrack.LeftLow_Org_AdZero = ll_ad[0];
App.ADTrack.RightUp_Org_AdZero = ru_ad[0];
App.ADTrack.RightLow_Org_AdZero = rl_ad[0];
}
}
#endif
@@ -767,8 +817,8 @@ static void AppLoopHandler(void)
App.WaitFlag = true;
App.UpFlag = false;
App.DownFlag = false;
App.EmeFlag = false;
App.StopFlag = true;
App.EvenRunning = false;
App.ADTrack.LeftUp_Org_AdZero = App.LeftUpCS1237_TrendArray[0];
App.ADTrack.LeftLow_Org_AdZero = App.LeftLowCS1237_TrendArray[0];
@@ -798,9 +848,8 @@ static void App1mSRoutine(void)
App.StopCollDelay1mSCnt--;
if(App.RunCollDelay1mSCnt > 0)
App.RunCollDelay1mSCnt--;
if(App.EvenRunning == true)
App.EvenRunDelay1mSCnt++;
if(App.EmergencyDelay1mSCnt > 0)
App.EmergencyDelay1mSCnt--;
App.FeedDogDlyCnt++;
}
void FeedDogHandler(void)
@@ -854,7 +903,7 @@ void SysTick_IrqHandler(void)
RIGHTUP_CS12371mSRoutine();
RIGHTLOW_CS12371mSRoutine();
Dcmotor1msRoutine();
Adc1mSRoutine();
Adc1mSRoutine();
}
/*****************************************************************************************