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
+8 -5
View File
@@ -5,7 +5,7 @@
#define USE_FILE_SYSTEM 1 #define USE_FILE_SYSTEM 1
#define SDCARD_BLOCK_MAX (10000) #define SDCARD_BLOCK_MAX (10000)
#define OSC_CNT 40 #define OSC_CNT 40
#define SLOPE_CNT 30 #define SLOPE_CNT 30
@@ -13,15 +13,18 @@
#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
#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() (SET_TWIN_BIT1(),CLR_TWIN_BIT0()) #define BDC_DOWN() (SET_TWIN_BIT1(),CLR_TWIN_BIT0())
/*控制等级*/ /*控制等级*/
@@ -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
+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 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]);
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]); BDC_UP();
uint32_t RightSlopeSum = abs(App.RightUpCS1237_SlopeArray[0]) + abs(App.RightLowCS1237_SlopeArray[0]); }
// uint32_t LeftDiffSum = abs(lu_diff[0]) + abs(ll_diff[0]); else if(App.UpFlag == true && App.EmergencyDelay1mSCnt > 0)
// uint32_t RightDiffSum = abs(ru_diff[0]) + abs(rl_diff[0]); {
uint32_t LeftDiffSum = abs(ll_diff[0]); BDC_DOWN();
uint32_t RightDiffSum = abs(rl_diff[0]); }
else if(App.EmergencyDelay1mSCnt <= 0)
App.LeftRunFalg = false; {
App.RightRunFalg = false; BDC_STOP();
App.CentreRunFalg = false; App.StopFlag = true;
if(((lu_ad[0] < App.ADTrack.LeftUp_Org_AdZero-1000) App.DownFlag = false;
&& (ll_ad[0] > App.ADTrack.LeftLow_Org_AdZero+1000) App.UpFlag = false;
&& (ru_ad[0] < App.ADTrack.RightUp_Org_AdZero-1000) App.EmeFlag = false;
&& (rl_ad[0] > App.ADTrack.RightLow_Org_AdZero+1000)) App.StopCollDelay1mSCnt = STOP_TIME_MS;
|| ((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) else if(App.StopFlag == true && App.StopCollDelay1mSCnt <= 0)
&& (rl_ad[0] < App.ADTrack.RightLow_Org_AdZero-1000))) {
{ App.LeftRunFalg = false;
App.CentreRunFalg = true; App.RightRunFalg = false;
if(abs(Oll_diff1) > abs(Orl_diff1)) App.CentreRunFalg = false;
{ if(((lu_ad[0] < App.ADTrack.LeftUp_Org_AdZero-1000)
App.LeftRunFalg = true; && (ll_ad[0] > App.ADTrack.LeftLow_Org_AdZero+1000)
if((App.LeftLowCS1237_SlopeArray[0] > App.ADTrack.Slope_VPT/3)) && (ru_ad[0] < App.ADTrack.RightUp_Org_AdZero-1000)
{ && (rl_ad[0] > App.ADTrack.RightLow_Org_AdZero+1000))
if(Oll_diff1 < -App.ADTrack.LeftLowVPT/2) || ((lu_ad[0] > App.ADTrack.LeftUp_Org_AdZero+1000)
{ && (ll_ad[0] < App.ADTrack.LeftLow_Org_AdZero-1000)
BDC_UP(); && (ru_ad[0] > App.ADTrack.RightUp_Org_AdZero+1000)
App.UpFlag = true; && (rl_ad[0] < App.ADTrack.RightLow_Org_AdZero-1000)))
App.StopFlag = false; {
App.DownFlag = false; App.CentreRunFalg = true;
App.RunCollDelay1mSCnt = RUN_TIME_MS; if(abs(Oll_diff1) > abs(Orl_diff1))
}
}
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))
{ {
App.LeftRunFalg = true; App.LeftRunFalg = true;
if(((App.LeftUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) // if((App.LeftLowCS1237_SlopeArray[0] > App.ADTrack.Slope_VPT/3))
&& (App.LeftUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) // {
&& (App.LeftUpCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT) if(Oll_diff1 < -App.ADTrack.LeftLowVPT/3)
&& (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)
)
{ {
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.LeftLowCS1237_SlopeArray[0] < -App.ADTrack.Slope_VPT/3))
&& (App.LeftUpCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) // {
&& (App.LeftUpCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT) if(Oll_diff1 > App.ADTrack.LeftLowVPT/3)
&& (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(); BDC_DOWN();
App.UpFlag = false; App.UpFlag = false;
App.DownFlag = true;
App.StopFlag = 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) else if(abs(Oll_diff1) < abs(Orl_diff1))
{ {
App.RightRunFalg = true; App.RightRunFalg = true;
if(((App.RightUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) if((App.RightLowCS1237_SlopeArray[0] > App.ADTrack.Slope_VPT/3))
&& (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) if(Orl_diff1 < -App.ADTrack.RightLowVPT/3)
&& (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.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.RightLowCS1237_SlopeArray[0] < -App.ADTrack.Slope_VPT/3))
&& (App.RightUpCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) {
&& (App.RightUpCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT) if(Orl_diff1 > App.ADTrack.RightLowVPT/3)
&& (App.RightLowCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) {
&& (App.RightLowCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) BDC_DOWN();
&& (App.RightLowCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)) App.UpFlag = false;
|| ((Oru_diff1 <= -App.ADTrack.RightUpVPT) App.StopFlag = false;
&& (Oru_diff2 <= -App.ADTrack.RightUpVPT) App.DownFlag = true;
&& (Oru_diff3 <= -App.ADTrack.RightUpVPT) App.EmeFlag = false;
&& (Orl_diff1 >= App.ADTrack.RightLowVPT) App.RunCollDelay1mSCnt = RUN_TIME_MS;
&& (Orl_diff2 >= App.ADTrack.RightLowVPT) App.EmergencyDelay1mSCnt = EME_START_MS;
&& (Orl_diff3 >= App.ADTrack.RightLowVPT)) }
}
}
}
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) BDC_UP();
&& (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT) App.UpFlag = true;
) App.StopFlag = false;
{ App.DownFlag = false;
BDC_DOWN(); App.EmeFlag = false;
App.DownFlag = true; App.RunCollDelay1mSCnt = RUN_TIME_MS;
App.StopFlag = false; App.EmergencyDelay1mSCnt = EME_START_MS;
App.UpFlag = false; }
} }
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.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)
@@ -854,7 +903,7 @@ void SysTick_IrqHandler(void)
RIGHTUP_CS12371mSRoutine(); RIGHTUP_CS12371mSRoutine();
RIGHTLOW_CS12371mSRoutine(); RIGHTLOW_CS12371mSRoutine();
Dcmotor1msRoutine(); Dcmotor1msRoutine();
Adc1mSRoutine(); Adc1mSRoutine();
} }
/***************************************************************************************** /*****************************************************************************************