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>
<tExt>*.txt; *.h; *.inc; *.md</tExt>
<pExt>*.plm</pExt>
<CppX>*.cpp</CppX>
<CppX>*.cpp; *.cc; *.cxx</CppX>
<nMigrate>0</nMigrate>
</Extensions>
@@ -117,21 +117,6 @@
<pMon>BIN\CMSIS_AGDI.dll</pMon>
</DebugOpt>
<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>
<Number>0</Number>
<Key>ARMRTXEVENTFLAGS</Key>
@@ -152,45 +137,18 @@
<Key>DLGUARM</Key>
<Name></Name>
</SetRegEntry>
</TargetDriverDllRegistry>
<<<<<<< HEAD
<Breakpoint/>
=======
<Breakpoint>
<Bp>
<SetRegEntry>
<Number>0</Number>
<Type>0</Type>
<LineNumber>291</LineNumber>
<EnabledFlag>1</EnabledFlag>
<Address>4146</Address>
<ByteObject>0</ByteObject>
<HtxType>0</HtxType>
<ManyObjects>0</ManyObjects>
<SizeOfObject>0</SizeOfObject>
<BreakByAccess>0</BreakByAccess>
<BreakIfRCount>1</BreakIfRCount>
<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
<Key>CMSIS_AGDI</Key>
<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>
</SetRegEntry>
<SetRegEntry>
<Number>0</Number>
<Key>UL2CM3</Key>
<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>
</SetRegEntry>
</TargetDriverDllRegistry>
<Breakpoint/>
<WatchWindow1>
<Ww>
<count>0</count>
@@ -210,83 +168,61 @@
<Ww>
<count>3</count>
<WinNumber>1</WinNumber>
<ItemText>App.LeftUpCS1237_SlopeArray[0],0x0A</ItemText>
<ItemText>App.EmeFlag</ItemText>
</Ww>
<Ww>
<count>4</count>
<WinNumber>1</WinNumber>
<ItemText>App.LeftLowCS1237_SlopeArray[0],0x0A</ItemText>
<ItemText>App.ADTrack.LeftUp_Org_AdZero,0x0A</ItemText>
</Ww>
<Ww>
<count>5</count>
<WinNumber>1</WinNumber>
<ItemText>App.RightUpCS1237_SlopeArray[0],0x0A</ItemText>
<ItemText>App.ADTrack.LeftLow_Org_AdZero,0x0A</ItemText>
</Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>App.RightLowCS1237_SlopeArray[0],0x0A</ItemText>
<ItemText>App.ADTrack.RightUp_Org_AdZero,0x0A</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>App.ADTrack.LeftUp_Org_AdZero,0x0A</ItemText>
<ItemText>App.ADTrack.RightLow_Org_AdZero,0x0A</ItemText>
</Ww>
<Ww>
<count>8</count>
<WinNumber>1</WinNumber>
<ItemText>App.ADTrack.LeftLow_Org_AdZero,0x0A</ItemText>
<ItemText>App.LeftRunFalg</ItemText>
</Ww>
<Ww>
<count>9</count>
<WinNumber>1</WinNumber>
<ItemText>App.ADTrack.RightUp_Org_AdZero,0x0A</ItemText>
<ItemText>App.RightRunFalg</ItemText>
</Ww>
<Ww>
<count>10</count>
<WinNumber>1</WinNumber>
<ItemText>App.ADTrack.RightLow_Org_AdZero,0x0A</ItemText>
<ItemText>App.CentreRunFalg</ItemText>
</Ww>
<Ww>
<count>11</count>
<WinNumber>1</WinNumber>
<ItemText>App.LeftRunFalg,0x0A</ItemText>
<ItemText>LeftLowDiff,0x0A</ItemText>
</Ww>
<Ww>
<count>12</count>
<WinNumber>1</WinNumber>
<ItemText>App.RightRunFalg,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>
<ItemText>RightLowDiff,0x0A</ItemText>
</Ww>
</WatchWindow1>
<MemoryWindow1>
<Mm>
<WinNumber>1</WinNumber>
<SubType>0</SubType>
<ItemText>0x1FFFA910</ItemText>
<AccSizeX>0</AccSizeX>
</Mm>
</MemoryWindow1>
<Tracepoint>
<THDelay>0</THDelay>
</Tracepoint>
<DebugFlag>
<trace>0</trace>
<periodic>1</periodic>
<<<<<<< HEAD
<aLwin>1</aLwin>
=======
<aLwin>0</aLwin>
>>>>>>> Motor_Test
<aCover>0</aCover>
<aSer1>0</aSer1>
<aSer2>0</aSer2>
@@ -324,10 +260,10 @@
<pMultCmdsp></pMultCmdsp>
<DebugDescription>
<Enable>1</Enable>
<EnableFlashSeq>0</EnableFlashSeq>
<EnableFlashSeq>1</EnableFlashSeq>
<EnableLog>0</EnableLog>
<Protocol>2</Protocol>
<DbgClock>5000000</DbgClock>
<DbgClock>1000000</DbgClock>
</DebugDescription>
</TargetOption>
</Target>
@@ -427,72 +363,7 @@
<tIfile></tIfile>
<pMon></pMon>
</DebugOpt>
<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>
<Breakpoint/>
<Tracepoint>
<THDelay>0</THDelay>
</Tracepoint>
@@ -1152,7 +1023,7 @@
<Group>
<GroupName>CS1237</GroupName>
<tvExp>1</tvExp>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
@@ -1208,7 +1079,7 @@
<Group>
<GroupName>dc_motor</GroupName>
<tvExp>1</tvExp>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<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 ACCEL_STEP 10 // 加速步进值
#define DECEL_STEP 10 // 减速步进值
<<<<<<< HEAD
#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 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
+232 -183
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)
{
if(App.StopCollDelay1mSCnt <= 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]);
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]);
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))
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.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(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/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.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)
}
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)))
{
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;
}
}
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(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();
}
/*****************************************************************************************