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

复样后的双柱支架调试

Browse Source 
1、修复了晃动后支架乱跳的bug
2、修复了从底部到顶部后控制力变差的bug
3、控制方式改为以上面两个传感器为主
4、控制完成后设备还未校零，马上进行侧面推拉会产生误控
This commit is contained in:
DESKTOP-KQHDCM4\97555
2026-03-13 20:00:20 +08:00
parent 4b1eb44562
commit 0e05817f63
4 changed files with 302 additions and 148 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Project/HC32F460/MDK/HC32F460.uvoptx
+57 -2
View File
@@ -103,7 +103,7 @@
<bEvRecOn>1</bEvRecOn> <bEvRecOn>1</bEvRecOn>
<bSchkAxf>0</bSchkAxf> <bSchkAxf>0</bSchkAxf>
<bTchkAxf>0</bTchkAxf> <bTchkAxf>0</bTchkAxf>
<nTsel>4</nTsel> <nTsel>3</nTsel>
<sDll></sDll> <sDll></sDll>
<sDllPa></sDllPa> <sDllPa></sDllPa>
<sDlgDll></sDlgDll> <sDlgDll></sDlgDll>
@@ -114,7 +114,7 @@
<tDlgDll></tDlgDll> <tDlgDll></tDlgDll>
<tDlgPa></tDlgPa> <tDlgPa></tDlgPa>
<tIfile></tIfile> <tIfile></tIfile>
<pMon>Segger\JL2CM3.dll</pMon> <pMon>BIN\CMSIS_AGDI.dll</pMon>
</DebugOpt> </DebugOpt>
<TargetDriverDllRegistry> <TargetDriverDllRegistry>
<SetRegEntry> <SetRegEntry>
@@ -225,6 +225,61 @@
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>App.EmergencyDelay1mSCnt,0x0A</ItemText> <ItemText>App.EmergencyDelay1mSCnt,0x0A</ItemText>
</Ww> </Ww>
<Ww>
<count>14</count>
<WinNumber>1</WinNumber>
<ItemText>App.ADTrack.Slope_VPT,0x0A</ItemText>
</Ww>
<Ww>
<count>15</count>
<WinNumber>1</WinNumber>
<ItemText>App.RightUpCS1237_SlopeArray[0],0x0A</ItemText>
</Ww>
<Ww>
<count>16</count>
<WinNumber>1</WinNumber>
<ItemText>App.ADTrack.Stop_VPT,0x0A</ItemText>
</Ww>
<Ww>
<count>17</count>
<WinNumber>1</WinNumber>
<ItemText>Olu_diff1,0x0A</ItemText>
</Ww>
<Ww>
<count>18</count>
<WinNumber>1</WinNumber>
<ItemText>Oru_diff1,0x0A</ItemText>
</Ww>
<Ww>
<count>19</count>
<WinNumber>1</WinNumber>
<ItemText>Olu_diff2,0x0A</ItemText>
</Ww>
<Ww>
<count>20</count>
<WinNumber>1</WinNumber>
<ItemText>Oru_diff2,0x0A</ItemText>
</Ww>
<Ww>
<count>21</count>
<WinNumber>1</WinNumber>
<ItemText>Olu_diff3,0x0A</ItemText>
</Ww>
<Ww>
<count>22</count>
<WinNumber>1</WinNumber>
<ItemText>Oru_diff3,0x0A</ItemText>
</Ww>
<Ww>
<count>23</count>
<WinNumber>1</WinNumber>
<ItemText>App.LeftUpCS1237_SlopeArray[0],0x0A</ItemText>
</Ww>
<Ww>
<count>24</count>
<WinNumber>1</WinNumber>
<ItemText>App.RightUpCS1237_SlopeArray[0],0x0A</ItemText>
</Ww>
</WatchWindow1> </WatchWindow1>
<Tracepoint> <Tracepoint>
<THDelay>0</THDelay> <THDelay>0</THDelay>
Project/HC32F460/MDK/HC32F460.uvprojx
+1
View File
@@ -10,6 +10,7 @@
<TargetName>Debug</TargetName> <TargetName>Debug</TargetName>
<ToolsetNumber>0x4</ToolsetNumber> <ToolsetNumber>0x4</ToolsetNumber>
<ToolsetName>ARM-ADS</ToolsetName> <ToolsetName>ARM-ADS</ToolsetName>
<pArmCC>5060960::V5.06 update 7 (build 960)::.\ARMCC</pArmCC>
<pCCUsed>5060960::V5.06 update 7 (build 960)::.\ARMCC</pCCUsed> <pCCUsed>5060960::V5.06 update 7 (build 960)::.\ARMCC</pCCUsed>
<uAC6>0</uAC6> <uAC6>0</uAC6>
<TargetOption> <TargetOption>
Project/HC32F460/source/User/Inc/main.h
+4 -1
View File
@@ -11,12 +11,13 @@
#define SLOPE_CNT 20 #define SLOPE_CNT 20
#define SLOPE_VPT 1000 #define SLOPE_VPT 1000
#define PRESS_VPT 10
#define FOLLOW_VPT 20 #define FOLLOW_VPT 20
#define SHIELD_VPT 5000 #define SHIELD_VPT 5000
#define EME_STOP_VPT 4000 #define EME_STOP_VPT 4000
#define CENTRE_VPT 2000 #define CENTRE_VPT 2000
#define STOP_TIME_MS 1000 #define STOP_TIME_MS 500
#define RUN_TIME_MS 0 #define RUN_TIME_MS 0
#define EME_START_MS 0 #define EME_START_MS 0
#define EME_STOP_MS 2000 #define EME_STOP_MS 2000
@@ -112,6 +113,8 @@ typedef struct {
uint32_t SlopeCollDelay1mSCnt; uint32_t SlopeCollDelay1mSCnt;
uint8_t FeedDogDlyCnt; uint8_t FeedDogDlyCnt;
bool CaliZeroFlag;//校零标志位
bool SideFlag;//侧压标志位
bool UpFlag;//上升标志位 bool UpFlag;//上升标志位
bool DownFlag;//下降标志位 bool DownFlag;//下降标志位
bool StopFlag;//停止标志位 bool StopFlag;//停止标志位
Project/HC32F460/source/User/Src/main.c
+198 -103
View File
@@ -220,12 +220,16 @@ 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 ru_K = check_peaks_valleys(App.RightUpCS1237_SlopeArray, SLOPE_CNT);
PeakValleyCheck rl_K = check_peaks_valleys(App.RightLowCS1237_SlopeArray, SLOPE_CNT); PeakValleyCheck rl_K = check_peaks_valleys(App.RightLowCS1237_SlopeArray, SLOPE_CNT);
int32_t LeftSlopeSum = abs(App.LeftUpCS1237_SlopeArray[0]) + abs(App.LeftLowCS1237_SlopeArray[0]); int32_t LeftSlopeSum = abs(App.LeftUpCS1237_SlopeArray[0]) + abs(App.LeftLowCS1237_SlopeArray[0]) + abs(App.LeftUpCS1237_SlopeArray[1]) + abs(App.LeftLowCS1237_SlopeArray[1]) + abs(App.LeftUpCS1237_SlopeArray[2]) + abs(App.LeftLowCS1237_SlopeArray[2]);
int32_t RightSlopeSum = abs(App.RightUpCS1237_SlopeArray[0]) + abs(App.RightLowCS1237_SlopeArray[0]); int32_t RightSlopeSum = abs(App.RightUpCS1237_SlopeArray[0]) + abs(App.RightLowCS1237_SlopeArray[0]) + abs(App.RightUpCS1237_SlopeArray[1]) + abs(App.RightLowCS1237_SlopeArray[1]) + abs(App.RightUpCS1237_SlopeArray[2]) + abs(App.RightLowCS1237_SlopeArray[2]);
int32_t LeftDiff = abs(Oll_diff1) + abs(Olu_diff1); int32_t LeftDiffSum = abs(lu_diff[0]) + abs(lu_diff[1]) + abs(lu_diff[2]);
int32_t RightDiff = abs(Orl_diff1) + abs(Oru_diff1); int32_t RightDiffSum = abs(ru_diff[0]) + abs(ru_diff[1]) + abs(ru_diff[2]);
int32_t LeftDiffSum = abs(lu_diff[0]) + abs(ll_diff[0]); int32_t LeftDiff = abs(Oll_diff1) + abs(Olu_diff1) + abs(Oll_diff2) + abs(Olu_diff2) + abs(Oll_diff3) + abs(Olu_diff3);
int32_t RightDiffSum = abs(ru_diff[0]) + abs(rl_diff[0]); int32_t RightDiff = abs(Orl_diff1) + abs(Oru_diff1) + abs(Orl_diff2) + abs(Oru_diff2) + abs(Orl_diff3) + abs(Oru_diff3);
int32_t LuSideDiff = lu_ad[0] - lu_ad[5];
int32_t LlSideDiff = ll_ad[0] - ll_ad[5];
int32_t RuSideDiff = ru_ad[0] - ru_ad[5];
int32_t RlSideDiff = rl_ad[0] - rl_ad[5];
if(App.EmeFlag == true) if(App.EmeFlag == true)
{ {
@@ -282,18 +286,24 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
if(LeftDiff > RightDiff) if(LeftDiff > RightDiff)
{ {
App.LeftRunFalg = true; App.LeftRunFalg = true;
if((Oll_diff1+Orl_diff1) < -(App.ADTrack.Stop_VPT)) if((Olu_diff1+Oru_diff1) < -(App.ADTrack.Stop_VPT)
&& ((Olu_diff2+Oru_diff2) < -(App.ADTrack.Stop_VPT))
&& ((Olu_diff3+Oru_diff3) < -(App.ADTrack.Stop_VPT)))
{ {
BDC_UP(); BDC_UP();
App.CaliZeroFlag = false;
App.UpFlag = true; App.UpFlag = true;
App.StopFlag = false; App.StopFlag = false;
App.DownFlag = false; App.DownFlag = false;
App.EmeFlag = false; App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
} }
if((Oll_diff1+Orl_diff1) > (App.ADTrack.Stop_VPT)) if((Olu_diff1+Oru_diff1) > (App.ADTrack.Stop_VPT)
&& (Olu_diff2+Oru_diff2) > (App.ADTrack.Stop_VPT)
&& (Olu_diff3+Oru_diff3) > (App.ADTrack.Stop_VPT))
{ {
BDC_DOWN(); BDC_DOWN();
App.CaliZeroFlag = false;
App.UpFlag = false; App.UpFlag = false;
App.StopFlag = false; App.StopFlag = false;
App.DownFlag = true; App.DownFlag = true;
@@ -304,18 +314,24 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
else if(RightDiff > LeftDiff) else if(RightDiff > LeftDiff)
{ {
App.RightRunFalg = true; App.RightRunFalg = true;
if((Oll_diff1+Orl_diff1) < -(App.ADTrack.Stop_VPT)) if((Olu_diff1+Oru_diff1) < -(App.ADTrack.Stop_VPT)
&& (Olu_diff2+Oru_diff2) < -(App.ADTrack.Stop_VPT)
&& (Olu_diff3+Oru_diff3) < -(App.ADTrack.Stop_VPT))
{ {
BDC_UP(); BDC_UP();
App.CaliZeroFlag = false;
App.UpFlag = true; App.UpFlag = true;
App.StopFlag = false; App.StopFlag = false;
App.DownFlag = false; App.DownFlag = false;
App.EmeFlag = false; App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
} }
if((Oll_diff1+Orl_diff1) > (App.ADTrack.Stop_VPT)) if((Olu_diff1+Oru_diff1) > (App.ADTrack.Stop_VPT)
&& (Olu_diff2+Oru_diff2) > (App.ADTrack.Stop_VPT)
&& (Olu_diff3+Oru_diff3) > (App.ADTrack.Stop_VPT))
{ {
BDC_DOWN(); BDC_DOWN();
App.CaliZeroFlag = false;
App.UpFlag = false; App.UpFlag = false;
App.StopFlag = false; App.StopFlag = false;
App.DownFlag = true; App.DownFlag = true;
@@ -324,90 +340,182 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
} }
} }
} }
else if((LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum))////判断左边操作 else if((LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum))//判断左边操作
{ {
App.LeftRunFalg = true; App.LeftRunFalg = true;
if(((App.SlopeCollDelay1mSCnt <= 0) if((
&& (App.LeftUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) //判断正面按压 // (App.SlopeCollDelay1mSCnt <= 0)
&& (App.LeftUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) // && (App.LeftUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) //通过斜率判断上下拉方向
&& (App.LeftUpCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT) // && (App.LeftUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) // && (App.LeftUpCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) // && (App.LeftLowCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)) // && (App.LeftLowCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT)
|| ((Oll_diff1 <= -(App.ADTrack.LeftLowVPT*5/6)) // && (App.LeftLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)) || (
&& (Oll_diff2 <= -(App.ADTrack.LeftLowVPT*5/6)) (Olu_diff1 >= (App.ADTrack.LeftUpVPT*5/6)) //通过差值判断上下拉方向
&& (Oll_diff3 <= -(App.ADTrack.LeftLowVPT*5/6))
&& (Olu_diff1 >= (App.ADTrack.LeftUpVPT*5/6))
&& (Olu_diff2 >= (App.ADTrack.LeftUpVPT*5/6)) && (Olu_diff2 >= (App.ADTrack.LeftUpVPT*5/6))
&& (Olu_diff3 >= (App.ADTrack.LeftUpVPT*5/6)))) && (Olu_diff3 >= (App.ADTrack.LeftUpVPT*5/6))))
{ {
if((abs(Oll_diff1) > abs(Orl_diff1) + App.ADTrack.LeftLowShieldVPT)//判断侧面推拉 if((App.LeftUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
&& (App.LeftUpCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] >= PRESS_VPT))
{
if(App.CaliZeroFlag == true)//判断是否校零
{
if((abs(Oll_diff1) > abs(Orl_diff1) + App.ADTrack.LeftLowShieldVPT)//判断是否不是侧面推拉
&& (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT) && (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT)
&& (abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT) && (abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT)
&& (abs(Olu_diff1) > abs(Oru_diff1) + App.ADTrack.LeftUpShieldVPT) && (abs(Olu_diff1) > abs(Oru_diff1) + App.ADTrack.LeftUpShieldVPT))
)
{ {
BDC_UP(); BDC_DOWN();
App.UpFlag = true; App.CaliZeroFlag = false;
App.UpFlag = false;
App.DownFlag = true;
App.StopFlag = false; App.StopFlag = false;
App.DownFlag = false;
App.EmeFlag = false; App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
} }
} }
else if(((App.SlopeCollDelay1mSCnt <= 0) else
&& (App.LeftUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) //判断上下拉方向 {
&& (App.LeftUpCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) BDC_DOWN();
&& (App.LeftUpCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT) App.CaliZeroFlag = false;
&& (App.LeftLowCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) App.UpFlag = false;
&& (App.LeftLowCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) App.DownFlag = true;
&& (App.LeftLowCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)) App.StopFlag = false;
|| ((Oll_diff1 >= (App.ADTrack.LeftLowVPT*5/6)) App.EmeFlag = false;
&& (Oll_diff2 >= (App.ADTrack.LeftLowVPT*5/6)) App.RunCollDelay1mSCnt = RUN_TIME_MS;
&& (Oll_diff3 >= (App.ADTrack.LeftLowVPT*5/6)) }
&& (Olu_diff1 <= -(App.ADTrack.LeftUpVPT*5/6)) }
}
else if((
// (App.SlopeCollDelay1mSCnt <= 0)
// && (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*5/6))//通过差值判断上下拉方向
&& (Olu_diff2 <= -(App.ADTrack.LeftUpVPT*5/6)) && (Olu_diff2 <= -(App.ADTrack.LeftUpVPT*5/6))
&& (Olu_diff3 <= -(App.ADTrack.LeftUpVPT*5/6)))) && (Olu_diff3 <= -(App.ADTrack.LeftUpVPT*5/6))))
{ {
if((abs(Oll_diff1) > abs(Orl_diff1) + App.ADTrack.LeftLowShieldVPT)//判断侧面推拉 if((App.LeftUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
&& (App.LeftUpCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] <= -PRESS_VPT))
{
if(App.CaliZeroFlag == true)//判断是否校零
{
if((abs(Oll_diff1) > abs(Orl_diff1) + App.ADTrack.LeftLowShieldVPT)//判断是否不是侧面推拉
&& (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT) && (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT)
&& (abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT) && (abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT)
&& (abs(Olu_diff1) > abs(Oru_diff1) + App.ADTrack.LeftUpShieldVPT) && (abs(Olu_diff1) > abs(Oru_diff1) + App.ADTrack.LeftUpShieldVPT))
)
{ {
BDC_DOWN(); BDC_UP();
App.UpFlag = false; App.CaliZeroFlag = false;
App.DownFlag = true; App.UpFlag = true;
App.DownFlag = false;
App.StopFlag = false;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
}
}
else
{
BDC_UP();
App.CaliZeroFlag = false;
App.UpFlag = true;
App.DownFlag = false;
App.StopFlag = false; App.StopFlag = false;
App.EmeFlag = false; App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
} }
} }
} }
else if(LeftSlopeSum < RightSlopeSum && LeftDiffSum < RightDiffSum)//上下拉屏幕右边 }
else if(LeftSlopeSum < RightSlopeSum && LeftDiffSum < RightDiffSum)//判断右边操作
{ {
App.RightRunFalg = true; App.RightRunFalg = true;
if(((App.SlopeCollDelay1mSCnt <= 0) if((
&& (App.RightUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) //判断上下拉方向 // (App.SlopeCollDelay1mSCnt <= 0)
&& (App.RightUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) // && (App.RightUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) //通过斜率判断上下拉方向
&& (App.RightUpCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT) // && (App.RightUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT)
&& (App.RightLowCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) // && (App.RightUpCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)
&& (App.RightLowCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) // && (App.RightLowCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
&& (App.RightLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)) // && (App.RightLowCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT)
|| ((Orl_diff1 <= -(App.ADTrack.RightLowVPT*5/6)) // && (App.RightLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)) || (
&& (Orl_diff2 <= -(App.ADTrack.RightLowVPT*5/6)) (Oru_diff1 >= (App.ADTrack.RightUpVPT*5/6)) //通过差值判断上下拉方向
&& (Orl_diff3 <= -(App.ADTrack.RightLowVPT*5/6))
&& (Oru_diff1 >= (App.ADTrack.RightUpVPT*5/6))
&& (Oru_diff2 >= (App.ADTrack.RightUpVPT*5/6)) && (Oru_diff2 >= (App.ADTrack.RightUpVPT*5/6))
&& (Oru_diff3 >= (App.ADTrack.RightUpVPT*5/6)))) && (Oru_diff3 >= (App.ADTrack.RightUpVPT*5/6))))
{ {
if((abs(Orl_diff1) > abs(Oll_diff1) + App.ADTrack.RightLowShieldVPT)//判断侧面推拉 if((App.RightUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
&& (App.RightUpCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[0] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] >= PRESS_VPT))
{
if(App.CaliZeroFlag == true)//判断是否校零
{
if((abs(Orl_diff1) > abs(Oll_diff1) + App.ADTrack.RightLowShieldVPT)//判断是否不是侧面推拉
&& (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT) && (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)
&& (abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT) && (abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT))
) {
BDC_DOWN();
App.CaliZeroFlag = false;
App.DownFlag = true;
App.StopFlag = false;
App.UpFlag = false;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
}
}
else
{
BDC_DOWN();
App.CaliZeroFlag = false;
App.DownFlag = true;
App.StopFlag = false;
App.UpFlag = false;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
}
}
}
else if((
// (App.SlopeCollDelay1mSCnt <= 0)
// && (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*5/6)) //通过差值判断上下拉方向
&& (Oru_diff2 <= -(App.ADTrack.RightUpVPT*5/6))
&& (Oru_diff3 <= -(App.ADTrack.RightUpVPT*5/6))))
{
if((App.RightUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
&& (App.RightUpCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[0] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] <= -PRESS_VPT))
{
if(App.CaliZeroFlag == true)//判断是否校零
{
if((abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT)//判断是否不是侧面推拉
&& (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT)
&& (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT)
&& (abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT))
{ {
BDC_UP(); BDC_UP();
App.CaliZeroFlag = false;
App.UpFlag = true; App.UpFlag = true;
App.StopFlag = false; App.StopFlag = false;
App.DownFlag = false; App.DownFlag = false;
@@ -415,41 +523,25 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
} }
} }
else if(((App.SlopeCollDelay1mSCnt <= 0) else
&& (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))
|| ((Orl_diff1 >= (App.ADTrack.RightLowVPT*5/6))
&& (Orl_diff2 >= (App.ADTrack.RightLowVPT*5/6))
&& (Orl_diff3 >= (App.ADTrack.RightLowVPT*5/6))
&& (Oru_diff1 <= -(App.ADTrack.RightUpVPT*5/6))
&& (Oru_diff2 <= -(App.ADTrack.RightUpVPT*5/6))
&& (Oru_diff3 <= -(App.ADTrack.RightUpVPT*5/6))))
{ {
if((abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT)//判断侧面推拉 BDC_UP();
&& (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT) App.CaliZeroFlag = false;
&& (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT) App.UpFlag = true;
&& (abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT)
)
{
BDC_DOWN();
App.DownFlag = true;
App.StopFlag = false; App.StopFlag = false;
App.UpFlag = false; App.DownFlag = false;
App.EmeFlag = false; App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
} }
} }
} }
} }
}
else if(App.DownFlag == true && App.RunCollDelay1mSCnt <= 0) else if(App.DownFlag == true && App.RunCollDelay1mSCnt <= 0)
{ {
if(App.LeftRunFalg == true) if(App.LeftRunFalg == true)
{ {
if(((App.CentreRunFalg == false ) && (ll_slope > App.ADTrack.Slope_VPT)) if(((App.CentreRunFalg == false ) && (lu_slope > App.ADTrack.Slope_VPT))
|| ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0))) || ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0)))
{ {
BDC_STOP(); BDC_STOP();
@@ -461,11 +553,11 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.SlopeCollDelay1mSCnt = SLOPE_START_MS; App.SlopeCollDelay1mSCnt = SLOPE_START_MS;
App.EmergencyDelay1mSCnt = EME_START_MS; App.EmergencyDelay1mSCnt = EME_START_MS;
} }
else if(((App.CentreRunFalg == false ) && (Oll_diff1 < App.ADTrack.Stop_VPT)) else if(((App.CentreRunFalg == false ) && (Olu_diff1 < App.ADTrack.Stop_VPT) && (Olu_diff2 < App.ADTrack.Stop_VPT) && (Olu_diff3 < App.ADTrack.Stop_VPT))
|| ((App.CentreRunFalg == true) || ((App.CentreRunFalg == true)
&& ((Oll_diff1 + Orl_diff1) < (App.ADTrack.Stop_VPT*2/3)) && ((Olu_diff1 + Oru_diff1) < (App.ADTrack.Stop_VPT*2/3))
&& ((Oll_diff2 + Orl_diff2) < (App.ADTrack.Stop_VPT*2/3)) && ((Olu_diff2 + Oru_diff2) < (App.ADTrack.Stop_VPT*2/3))
&& ((Oll_diff3 + Orl_diff3) < (App.ADTrack.Stop_VPT*2/3)) && ((Olu_diff3 + Oru_diff3) < (App.ADTrack.Stop_VPT*2/3))
)) ))
{ {
BDC_STOP(); BDC_STOP();
@@ -492,8 +584,8 @@ 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(((App.CentreRunFalg == false ) && (rl_slope > App.ADTrack.Slope_VPT)) if(((App.CentreRunFalg == false ) && (ru_slope > App.ADTrack.Slope_VPT))
|| ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) < -App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0))) || ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0)))
{ {
BDC_STOP(); BDC_STOP();
App.StopFlag = true; App.StopFlag = true;
@@ -504,11 +596,11 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.SlopeCollDelay1mSCnt = SLOPE_START_MS; App.SlopeCollDelay1mSCnt = SLOPE_START_MS;
App.EmergencyDelay1mSCnt = EME_START_MS; App.EmergencyDelay1mSCnt = EME_START_MS;
} }
else if(((App.CentreRunFalg == false ) && (Orl_diff1 < App.ADTrack.Stop_VPT)) else if(((App.CentreRunFalg == false ) && (Oru_diff1 < App.ADTrack.Stop_VPT) && (Oru_diff2 < App.ADTrack.Stop_VPT) && (Oru_diff3 < App.ADTrack.Stop_VPT))
|| ((App.CentreRunFalg == true) || ((App.CentreRunFalg == true)
&& ((Oll_diff1 + Orl_diff1) < (App.ADTrack.Stop_VPT*2/3)) && ((Olu_diff1 + Oru_diff1) < (App.ADTrack.Stop_VPT*2/3))
&& ((Oll_diff2 + Orl_diff2) < (App.ADTrack.Stop_VPT*2/3)) && ((Olu_diff2 + Oru_diff2) < (App.ADTrack.Stop_VPT*2/3))
&& ((Oll_diff3 + Orl_diff3) < (App.ADTrack.Stop_VPT*2/3)) && ((Olu_diff3 + Oru_diff3) < (App.ADTrack.Stop_VPT*2/3))
)) ))
{ {
BDC_STOP(); BDC_STOP();
@@ -538,7 +630,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
{ {
if(App.LeftRunFalg == true) if(App.LeftRunFalg == true)
{ {
if(((App.CentreRunFalg == false ) && (ll_slope < -App.ADTrack.Slope_VPT)) if(((App.CentreRunFalg == false ) && (lu_slope < -App.ADTrack.Slope_VPT))
|| ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0))) || ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0)))
{ {
BDC_STOP(); BDC_STOP();
@@ -550,11 +642,11 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.SlopeCollDelay1mSCnt = SLOPE_START_MS; App.SlopeCollDelay1mSCnt = SLOPE_START_MS;
App.EmergencyDelay1mSCnt = EME_START_MS; App.EmergencyDelay1mSCnt = EME_START_MS;
} }
else if(((App.CentreRunFalg == false ) && (Oll_diff1 > -App.ADTrack.Stop_VPT)) else if(((App.CentreRunFalg == false ) && (Olu_diff1 > -App.ADTrack.Stop_VPT) && (Olu_diff2 > -App.ADTrack.Stop_VPT) && (Olu_diff3 > -App.ADTrack.Stop_VPT))
|| ((App.CentreRunFalg == true) || ((App.CentreRunFalg == true)
&& ((Oll_diff1 + Orl_diff1) > -(App.ADTrack.Stop_VPT*2/3)) && ((Olu_diff1 + Oru_diff1) > -(App.ADTrack.Stop_VPT*2/3))
&& ((Oll_diff2 + Orl_diff2) > -(App.ADTrack.Stop_VPT*2/3)) && ((Olu_diff2 + Oru_diff2) > -(App.ADTrack.Stop_VPT*2/3))
&& ((Oll_diff3 + Orl_diff3) > -(App.ADTrack.Stop_VPT*2/3)) && ((Olu_diff3 + Oru_diff3) > -(App.ADTrack.Stop_VPT*2/3))
)) ))
{ {
BDC_STOP(); BDC_STOP();
@@ -581,7 +673,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(((App.CentreRunFalg == false ) && (rl_slope < -App.ADTrack.Slope_VPT)) if(((App.CentreRunFalg == false ) && (ru_slope < -App.ADTrack.Slope_VPT))
|| ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0))) || ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0)))
{ {
BDC_STOP(); BDC_STOP();
@@ -593,11 +685,11 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.SlopeCollDelay1mSCnt = SLOPE_START_MS; App.SlopeCollDelay1mSCnt = SLOPE_START_MS;
App.EmergencyDelay1mSCnt = EME_START_MS; App.EmergencyDelay1mSCnt = EME_START_MS;
} }
else if(((App.CentreRunFalg == false ) && (Orl_diff1 > -App.ADTrack.Stop_VPT)) else if(((App.CentreRunFalg == false ) && (Oru_diff1 > -App.ADTrack.Stop_VPT) && (Oru_diff2 > -App.ADTrack.Stop_VPT) && (Oru_diff3 > -App.ADTrack.Stop_VPT))
|| ((App.CentreRunFalg == true) || ((App.CentreRunFalg == true)
&& ((Oll_diff1 + Orl_diff1) > -(App.ADTrack.Stop_VPT*2/3)) && ((Olu_diff1 + Oru_diff1) > -(App.ADTrack.Stop_VPT*2/3))
&& ((Oll_diff2 + Orl_diff2) > -(App.ADTrack.Stop_VPT*2/3)) && ((Olu_diff2 + Oru_diff2) > -(App.ADTrack.Stop_VPT*2/3))
&& ((Oll_diff3 + Orl_diff3) > -(App.ADTrack.Stop_VPT*2/3)) && ((Olu_diff3 + Oru_diff3) > -(App.ADTrack.Stop_VPT*2/3))
)) ))
{ {
BDC_STOP(); BDC_STOP();
@@ -663,6 +755,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.ADTrack.LeftLow_Org_AdZero = ll_ad[0]; App.ADTrack.LeftLow_Org_AdZero = ll_ad[0];
App.ADTrack.RightUp_Org_AdZero = ru_ad[0]; App.ADTrack.RightUp_Org_AdZero = ru_ad[0];
App.ADTrack.RightLow_Org_AdZero = rl_ad[0]; App.ADTrack.RightLow_Org_AdZero = rl_ad[0];
App.CaliZeroFlag = true;
} }
} }
} }
@@ -897,6 +990,8 @@ static void AppLoopHandler(void)
if(App.TD1mSDelayCnt <= 0) if(App.TD1mSDelayCnt <= 0)
{ {
App.WaitFlag = true; App.WaitFlag = true;
App.SideFlag = false;
App.CaliZeroFlag = false;
App.UpFlag = false; App.UpFlag = false;
App.DownFlag = false; App.DownFlag = false;
App.EmeFlag = false; App.EmeFlag = false;