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/MDK/HC32F460.uvoptx
+27 -156
View File
@@ -12,7 +12,7 @@
<lExt>*.lib</lExt> <lExt>*.lib</lExt>
<tExt>*.txt; *.h; *.inc; *.md</tExt> <tExt>*.txt; *.h; *.inc; *.md</tExt>
<pExt>*.plm</pExt> <pExt>*.plm</pExt>
<CppX>*.cpp</CppX> <CppX>*.cpp; *.cc; *.cxx</CppX>
<nMigrate>0</nMigrate> <nMigrate>0</nMigrate>
</Extensions> </Extensions>
@@ -117,21 +117,6 @@
<pMon>BIN\CMSIS_AGDI.dll</pMon> <pMon>BIN\CMSIS_AGDI.dll</pMon>
</DebugOpt> </DebugOpt>
<TargetDriverDllRegistry> <TargetDriverDllRegistry>
<SetRegEntry>
<Number>0</Number>
<Key>JL2CM3</Key>
<Name>-U4294967295 -O78 -S2 -ZTIFSpeedSel5000 -A0 -C0 -JU1 -JI127.0.0.1 -JP0 -RST0 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -TB1 -TFE0 -FO15 -FD1FFF8000 -FC1000 -FN2 -FF0HC32F460_512K.FLM -FS00 -FL080000 -FP0($$Device:HC32F460JETA$FlashARM\HC32F460_512K.FLM) -FF1HC32F460_otp.FLM -FS13000C00 -FL13FC -FP1($$Device:HC32F460JETA$FlashARM\HC32F460_otp.FLM)</Name>
</SetRegEntry>
<SetRegEntry>
<Number>0</Number>
<Key>UL2CM3</Key>
<Name>UL2CM3(-S0 -C0 -P0 ) -FN2 -FC1000 -FD1FFF8000 -FF0HC32F460_512K -FF1HC32F460_otp -FL080000 -FL13FC -FS00 -FS13000C00 -FP0($$Device:HC32F460JETA$FlashARM\HC32F460_512K.FLM) -FP1($$Device:HC32F460JETA$FlashARM\HC32F460_otp.FLM)</Name>
</SetRegEntry>
<SetRegEntry>
<Number>0</Number>
<Key>CMSIS_AGDI</Key>
<Name>-X"Any" -UAny -O206 -S9 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD1FFF8000 -FC1000 -FN2 -FF0HC32F460_512K.FLM -FS00 -FL080000 -FP0($$Device:HC32F460JETA$FlashARM\HC32F460_512K.FLM) -FF1HC32F460_otp.FLM -FS13000C00 -FL13FC -FP1($$Device:HC32F460JETA$FlashARM\HC32F460_otp.FLM)</Name>
</SetRegEntry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>ARMRTXEVENTFLAGS</Key> <Key>ARMRTXEVENTFLAGS</Key>
@@ -152,45 +137,18 @@
<Key>DLGUARM</Key> <Key>DLGUARM</Key>
<Name></Name> <Name></Name>
</SetRegEntry> </SetRegEntry>
</TargetDriverDllRegistry> <SetRegEntry>
<<<<<<< HEAD
<Breakpoint/>
=======
<Breakpoint>
<Bp>
<Number>0</Number> <Number>0</Number>
<Type>0</Type> <Key>CMSIS_AGDI</Key>
<LineNumber>291</LineNumber> <Name>-X"Any" -UAny -O206 -S0 -C0 -P00000000 -N00("") -D00(00000000) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD1FFF8000 -FC1000 -FN2 -FF0HC32F460_512K.FLM -FS00 -FL080000 -FP0($$Device:HC32F460JETA$FlashARM\HC32F460_512K.FLM) -FF1HC32F460_otp.FLM -FS13000C00 -FL13FC -FP1($$Device:HC32F460JETA$FlashARM\HC32F460_otp.FLM)</Name>
<EnabledFlag>1</EnabledFlag> </SetRegEntry>
<Address>4146</Address> <SetRegEntry>
<ByteObject>0</ByteObject> <Number>0</Number>
<HtxType>0</HtxType> <Key>UL2CM3</Key>
<ManyObjects>0</ManyObjects> <Name>UL2CM3(-S0 -C0 -P0 -FD1FFF8000 -FC1000 -FN2 -FF0HC32F460_512K -FS00 -FL080000 -FF1HC32F460_otp -FS13000C00 -FL13FC -FP0($$Device:HC32F460JETA$FlashARM\HC32F460_512K.FLM) -FP1($$Device:HC32F460JETA$FlashARM\HC32F460_otp.FLM))</Name>
<SizeOfObject>0</SizeOfObject> </SetRegEntry>
<BreakByAccess>0</BreakByAccess> </TargetDriverDllRegistry>
<BreakIfRCount>1</BreakIfRCount> <Breakpoint/>
<Filename>..\source\Module\dc_motor\dc_motor.c</Filename>
<ExecCommand></ExecCommand>
<Expression>\\BombSensor\../source/Module/dc_motor/dc_motor.c\291</Expression>
</Bp>
<Bp>
<Number>1</Number>
<Type>0</Type>
<LineNumber>343</LineNumber>
<EnabledFlag>1</EnabledFlag>
<Address>0</Address>
<ByteObject>0</ByteObject>
<HtxType>0</HtxType>
<ManyObjects>0</ManyObjects>
<SizeOfObject>0</SizeOfObject>
<BreakByAccess>0</BreakByAccess>
<BreakIfRCount>0</BreakIfRCount>
<Filename>..\source\Module\dc_motor\dc_motor.c</Filename>
<ExecCommand></ExecCommand>
<Expression></Expression>
</Bp>
</Breakpoint>
>>>>>>> Motor_Test
<WatchWindow1> <WatchWindow1>
<Ww> <Ww>
<count>0</count> <count>0</count>
@@ -210,83 +168,61 @@
<Ww> <Ww>
<count>3</count> <count>3</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>App.LeftUpCS1237_SlopeArray[0],0x0A</ItemText> <ItemText>App.EmeFlag</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>4</count> <count>4</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>App.LeftLowCS1237_SlopeArray[0],0x0A</ItemText> <ItemText>App.ADTrack.LeftUp_Org_AdZero,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>5</count> <count>5</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>App.RightUpCS1237_SlopeArray[0],0x0A</ItemText> <ItemText>App.ADTrack.LeftLow_Org_AdZero,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>6</count> <count>6</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>App.RightLowCS1237_SlopeArray[0],0x0A</ItemText> <ItemText>App.ADTrack.RightUp_Org_AdZero,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>7</count> <count>7</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>App.ADTrack.LeftUp_Org_AdZero,0x0A</ItemText> <ItemText>App.ADTrack.RightLow_Org_AdZero,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>8</count> <count>8</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>App.ADTrack.LeftLow_Org_AdZero,0x0A</ItemText> <ItemText>App.LeftRunFalg</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>9</count> <count>9</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>App.ADTrack.RightUp_Org_AdZero,0x0A</ItemText> <ItemText>App.RightRunFalg</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>10</count> <count>10</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>App.ADTrack.RightLow_Org_AdZero,0x0A</ItemText> <ItemText>App.CentreRunFalg</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>11</count> <count>11</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>App.LeftRunFalg,0x0A</ItemText> <ItemText>LeftLowDiff,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>12</count> <count>12</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>App.RightRunFalg,0x0A</ItemText> <ItemText>RightLowDiff,0x0A</ItemText>
</Ww>
<Ww>
<count>13</count>
<WinNumber>1</WinNumber>
<ItemText>App.CentreRunFalg,0x0A</ItemText>
</Ww>
<Ww>
<count>18</count>
<WinNumber>1</WinNumber>
<ItemText>g_motor_data</ItemText>
</Ww> </Ww>
</WatchWindow1> </WatchWindow1>
<MemoryWindow1>
<Mm>
<WinNumber>1</WinNumber>
<SubType>0</SubType>
<ItemText>0x1FFFA910</ItemText>
<AccSizeX>0</AccSizeX>
</Mm>
</MemoryWindow1>
<Tracepoint> <Tracepoint>
<THDelay>0</THDelay> <THDelay>0</THDelay>
</Tracepoint> </Tracepoint>
<DebugFlag> <DebugFlag>
<trace>0</trace> <trace>0</trace>
<periodic>1</periodic> <periodic>1</periodic>
<<<<<<< HEAD
<aLwin>1</aLwin> <aLwin>1</aLwin>
=======
<aLwin>0</aLwin>
>>>>>>> Motor_Test
<aCover>0</aCover> <aCover>0</aCover>
<aSer1>0</aSer1> <aSer1>0</aSer1>
<aSer2>0</aSer2> <aSer2>0</aSer2>
@@ -324,10 +260,10 @@
<pMultCmdsp></pMultCmdsp> <pMultCmdsp></pMultCmdsp>
<DebugDescription> <DebugDescription>
<Enable>1</Enable> <Enable>1</Enable>
<EnableFlashSeq>0</EnableFlashSeq> <EnableFlashSeq>1</EnableFlashSeq>
<EnableLog>0</EnableLog> <EnableLog>0</EnableLog>
<Protocol>2</Protocol> <Protocol>2</Protocol>
<DbgClock>5000000</DbgClock> <DbgClock>1000000</DbgClock>
</DebugDescription> </DebugDescription>
</TargetOption> </TargetOption>
</Target> </Target>
@@ -427,72 +363,7 @@
<tIfile></tIfile> <tIfile></tIfile>
<pMon></pMon> <pMon></pMon>
</DebugOpt> </DebugOpt>
<Breakpoint> <Breakpoint/>
<Bp>
<Number>0</Number>
<Type>0</Type>
<LineNumber>163</LineNumber>
<EnabledFlag>1</EnabledFlag>
<Address>0</Address>
<ByteObject>0</ByteObject>
<HtxType>0</HtxType>
<ManyObjects>0</ManyObjects>
<SizeOfObject>0</SizeOfObject>
<BreakByAccess>0</BreakByAccess>
<BreakIfRCount>0</BreakIfRCount>
<Filename>..\source\User\Src\main.c</Filename>
<ExecCommand></ExecCommand>
<Expression></Expression>
</Bp>
<Bp>
<Number>1</Number>
<Type>0</Type>
<LineNumber>185</LineNumber>
<EnabledFlag>1</EnabledFlag>
<Address>0</Address>
<ByteObject>0</ByteObject>
<HtxType>0</HtxType>
<ManyObjects>0</ManyObjects>
<SizeOfObject>0</SizeOfObject>
<BreakByAccess>0</BreakByAccess>
<BreakIfRCount>0</BreakIfRCount>
<Filename>..\source\User\Src\main.c</Filename>
<ExecCommand></ExecCommand>
<Expression></Expression>
</Bp>
<Bp>
<Number>2</Number>
<Type>0</Type>
<LineNumber>211</LineNumber>
<EnabledFlag>1</EnabledFlag>
<Address>0</Address>
<ByteObject>0</ByteObject>
<HtxType>0</HtxType>
<ManyObjects>0</ManyObjects>
<SizeOfObject>0</SizeOfObject>
<BreakByAccess>0</BreakByAccess>
<BreakIfRCount>0</BreakIfRCount>
<Filename>..\source\User\Src\main.c</Filename>
<ExecCommand></ExecCommand>
<Expression></Expression>
</Bp>
<Bp>
<Number>3</Number>
<Type>0</Type>
<LineNumber>235</LineNumber>
<EnabledFlag>1</EnabledFlag>
<Address>0</Address>
<ByteObject>0</ByteObject>
<HtxType>0</HtxType>
<ManyObjects>0</ManyObjects>
<SizeOfObject>0</SizeOfObject>
<BreakByAccess>0</BreakByAccess>
<BreakIfRCount>0</BreakIfRCount>
<Filename>..\source\User\Src\main.c</Filename>
<ExecCommand></ExecCommand>
<Expression></Expression>
</Bp>
</Breakpoint>
<Tracepoint> <Tracepoint>
<THDelay>0</THDelay> <THDelay>0</THDelay>
</Tracepoint> </Tracepoint>
@@ -1152,7 +1023,7 @@
<Group> <Group>
<GroupName>CS1237</GroupName> <GroupName>CS1237</GroupName>
<tvExp>1</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
@@ -1208,7 +1079,7 @@
<Group> <Group>
<GroupName>dc_motor</GroupName> <GroupName>dc_motor</GroupName>
<tvExp>1</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
Project/HC32F460/source/Module/dc_motor/dc_motor.h
-4
View File
@@ -28,11 +28,7 @@
#define MAX_SPEED 480 // 最大速度(PWM占空比) #define MAX_SPEED 480 // 最大速度(PWM占空比)
#define ACCEL_STEP 10 // 加速步进值 #define ACCEL_STEP 10 // 加速步进值
#define DECEL_STEP 10 // 减速步进值 #define DECEL_STEP 10 // 减速步进值
<<<<<<< HEAD
#define ACCEL_INTERVAL 10 // 加减速间隔(ms) #define ACCEL_INTERVAL 10 // 加减速间隔(ms)
=======
#define ACCEL_INTERVAL 2 // 加减速间隔(ms)
>>>>>>> Motor_Test
Project/HC32F460/source/User/Inc/main.h
+6 -3
View File
@@ -13,9 +13,12 @@
#define SLOPE_VPT 1000 #define SLOPE_VPT 1000
#define FOLLOW_VPT 20 #define FOLLOW_VPT 20
#define SHIELD_VPT 2500 #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 RUN_TIME_MS 0
#define EME_START_MS 500
#define EME_STOP_MS 3000
#define TRAVEL_TIME_MS 30000 #define TRAVEL_TIME_MS 30000
@@ -99,13 +102,13 @@ typedef struct {
uint32_t Uart1Delay1mSCnt; uint32_t Uart1Delay1mSCnt;
uint32_t StopCollDelay1mSCnt; uint32_t StopCollDelay1mSCnt;
uint32_t RunCollDelay1mSCnt; uint32_t RunCollDelay1mSCnt;
uint32_t EvenRunDelay1mSCnt; uint32_t EmergencyDelay1mSCnt;
uint8_t FeedDogDlyCnt; uint8_t FeedDogDlyCnt;
bool UpFlag;//上升标志位 bool UpFlag;//上升标志位
bool DownFlag;//下降标志位 bool DownFlag;//下降标志位
bool StopFlag;//停止标志位 bool StopFlag;//停止标志位
bool EvenRunning;//平稳运行标志位 bool EmeFlag;//应急标志位
bool LeftRunFalg; bool LeftRunFalg;
bool RightRunFalg; bool RightRunFalg;
bool CentreRunFalg; bool CentreRunFalg;
Project/HC32F460/source/User/Src/main.c
+95 -46
View File
@@ -203,17 +203,35 @@ 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);
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]);
if(App.StopCollDelay1mSCnt <= 0) int32_t LeftLowDiff = abs(Oll_diff1);
{ int32_t RightLowDiff = abs(Orl_diff1);
uint32_t LeftSlopeSum = abs(App.LeftUpCS1237_SlopeArray[0]) + abs(App.LeftLowCS1237_SlopeArray[0]); int32_t LeftDiffSum = abs(lu_diff[0]);
uint32_t RightSlopeSum = abs(App.RightUpCS1237_SlopeArray[0]) + abs(App.RightLowCS1237_SlopeArray[0]); int32_t RightDiffSum = abs(ru_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]);
if(App.EmeFlag == true)
{
if(App.DownFlag == true && App.EmergencyDelay1mSCnt > 0)
{
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.LeftRunFalg = false;
App.RightRunFalg = false; App.RightRunFalg = false;
App.CentreRunFalg = false; App.CentreRunFalg = false;
@@ -230,52 +248,60 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
if(abs(Oll_diff1) > abs(Orl_diff1)) if(abs(Oll_diff1) > abs(Orl_diff1))
{ {
App.LeftRunFalg = true; App.LeftRunFalg = true;
if((App.LeftLowCS1237_SlopeArray[0] > App.ADTrack.Slope_VPT/3)) // if((App.LeftLowCS1237_SlopeArray[0] > App.ADTrack.Slope_VPT/3))
{ // {
if(Oll_diff1 < -App.ADTrack.LeftLowVPT/2) if(Oll_diff1 < -App.ADTrack.LeftLowVPT/3)
{ {
BDC_UP(); BDC_UP();
App.UpFlag = true; App.UpFlag = true;
App.StopFlag = false; App.StopFlag = false;
App.DownFlag = false; App.DownFlag = false;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
} }
} // }
else if((App.LeftLowCS1237_SlopeArray[0] < -App.ADTrack.Slope_VPT/3)) // else if((App.LeftLowCS1237_SlopeArray[0] < -App.ADTrack.Slope_VPT/3))
{ // {
if(Oll_diff1 > App.ADTrack.LeftLowVPT/2) if(Oll_diff1 > App.ADTrack.LeftLowVPT/3)
{ {
BDC_DOWN(); BDC_DOWN();
App.UpFlag = false; App.UpFlag = false;
App.StopFlag = false; App.StopFlag = false;
App.DownFlag = true; App.DownFlag = true;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
} }
} // }
} }
else if(abs(Oll_diff1) < abs(Orl_diff1)) else if(abs(Oll_diff1) < abs(Orl_diff1))
{ {
App.RightRunFalg = true; App.RightRunFalg = true;
if((App.RightLowCS1237_SlopeArray[0] > App.ADTrack.Slope_VPT/3)) if((App.RightLowCS1237_SlopeArray[0] > App.ADTrack.Slope_VPT/3))
{ {
if(Orl_diff1 < -App.ADTrack.RightLowVPT/2) if(Orl_diff1 < -App.ADTrack.RightLowVPT/3)
{ {
BDC_UP(); BDC_UP();
App.UpFlag = true; App.UpFlag = true;
App.StopFlag = false; App.StopFlag = false;
App.DownFlag = false; App.DownFlag = false;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
} }
} }
else if((App.RightLowCS1237_SlopeArray[0] < -App.ADTrack.Slope_VPT/3)) else if((App.RightLowCS1237_SlopeArray[0] < -App.ADTrack.Slope_VPT/3))
{ {
if(Orl_diff1 > App.ADTrack.RightLowVPT/2) if(Orl_diff1 > App.ADTrack.RightLowVPT/3)
{ {
BDC_DOWN(); BDC_DOWN();
App.UpFlag = false; App.UpFlag = false;
App.StopFlag = false; App.StopFlag = false;
App.DownFlag = true; App.DownFlag = true;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; App.RunCollDelay1mSCnt = RUN_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
} }
} }
} }
@@ -298,13 +324,16 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (Oll_diff3 <= -App.ADTrack.LeftLowVPT))) && (Oll_diff3 <= -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_UP(); BDC_UP();
App.UpFlag = true; App.UpFlag = true;
App.StopFlag = false; App.StopFlag = false;
App.DownFlag = 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) else if(((App.LeftUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
@@ -322,13 +351,16 @@ 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.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
} }
} }
} }
@@ -356,6 +388,9 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.UpFlag = true; App.UpFlag = true;
App.StopFlag = false; App.StopFlag = false;
App.DownFlag = 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) else if(((App.RightUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
@@ -380,7 +415,9 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.DownFlag = true; App.DownFlag = true;
App.StopFlag = false; App.StopFlag = false;
App.UpFlag = 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.StopFlag = true;
App.DownFlag = false; App.DownFlag = false;
App.UpFlag = false; App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS; App.StopCollDelay1mSCnt = STOP_TIME_MS;
} }
else if(((App.CentreRunFalg == false ) && (Oll_diff1 < App.ADTrack.Stop_VPT)) 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.StopFlag = true;
App.DownFlag = false; App.DownFlag = false;
App.UpFlag = false; App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS; 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) 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.StopFlag = true;
App.DownFlag = false; App.DownFlag = false;
App.UpFlag = false; App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS; App.StopCollDelay1mSCnt = STOP_TIME_MS;
} }
else if(((App.CentreRunFalg == false ) && (Orl_diff1 < App.ADTrack.Stop_VPT)) 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.StopFlag = true;
App.DownFlag = false; App.DownFlag = false;
App.UpFlag = false; App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS; 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) 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.StopFlag = true;
App.DownFlag = false; App.DownFlag = false;
App.UpFlag = false; App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS; App.StopCollDelay1mSCnt = STOP_TIME_MS;
} }
else if(((App.CentreRunFalg == false ) && (Oll_diff1 > -App.ADTrack.Stop_VPT)) 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.StopFlag = true;
App.DownFlag = false; App.DownFlag = false;
App.UpFlag = false; App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS; 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) 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.StopFlag = true;
App.DownFlag = false; App.DownFlag = false;
App.UpFlag = false; App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS; App.StopCollDelay1mSCnt = STOP_TIME_MS;
} }
else if(((App.CentreRunFalg == false ) && (Orl_diff1 > -App.ADTrack.Stop_VPT)) 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.StopFlag = true;
App.DownFlag = false; App.DownFlag = false;
App.UpFlag = false; App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS; 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.RightUp_Org_AdZero = ru_ad[0];
App.ADTrack.RightLow_Org_AdZero = rl_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 #endif
@@ -767,8 +817,8 @@ static void AppLoopHandler(void)
App.WaitFlag = true; App.WaitFlag = true;
App.UpFlag = false; App.UpFlag = false;
App.DownFlag = false; App.DownFlag = false;
App.EmeFlag = false;
App.StopFlag = true; App.StopFlag = true;
App.EvenRunning = false;
App.ADTrack.LeftUp_Org_AdZero = App.LeftUpCS1237_TrendArray[0]; App.ADTrack.LeftUp_Org_AdZero = App.LeftUpCS1237_TrendArray[0];
App.ADTrack.LeftLow_Org_AdZero = App.LeftLowCS1237_TrendArray[0]; App.ADTrack.LeftLow_Org_AdZero = App.LeftLowCS1237_TrendArray[0];
@@ -798,9 +848,8 @@ static void App1mSRoutine(void)
App.StopCollDelay1mSCnt--; App.StopCollDelay1mSCnt--;
if(App.RunCollDelay1mSCnt > 0) if(App.RunCollDelay1mSCnt > 0)
App.RunCollDelay1mSCnt--; App.RunCollDelay1mSCnt--;
if(App.EvenRunning == true) if(App.EmergencyDelay1mSCnt > 0)
App.EvenRunDelay1mSCnt++; App.EmergencyDelay1mSCnt--;
App.FeedDogDlyCnt++; App.FeedDogDlyCnt++;
} }
void FeedDogHandler(void) void FeedDogHandler(void)