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

双柱改良版调试完成

Browse Source
This commit is contained in:
DESKTOP-KQHDCM4\97555
2025-10-30 15:52:37 +08:00
parent 445e67f234
commit cde156a030
3 changed files with 85 additions and 111 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Project/HC32F460/MDK/HC32F460.uvoptx
+7 -22
View File
@@ -103,7 +103,7 @@
<bEvRecOn>1</bEvRecOn>
<bSchkAxf>0</bSchkAxf>
<bTchkAxf>0</bTchkAxf>
<nTsel>3</nTsel>
<nTsel>4</nTsel>
<sDll></sDll>
<sDllPa></sDllPa>
<sDlgDll></sDlgDll>
@@ -114,9 +114,14 @@
<tDlgDll></tDlgDll>
<tDlgPa></tDlgPa>
<tIfile></tIfile>
<pMon>BIN\CMSIS_AGDI.dll</pMon>
<pMon>Segger\JL2CM3.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>ARMRTXEVENTFLAGS</Key>
@@ -210,26 +215,6 @@
<WinNumber>1</WinNumber>
<ItemText>App.EmergencyDelay1mSCnt,0x0A</ItemText>
</Ww>
<Ww>
<count>12</count>
<WinNumber>1</WinNumber>
<ItemText>App.ADTrack.LeftLow_Org_AdZero,0x0A</ItemText>
</Ww>
<Ww>
<count>13</count>
<WinNumber>1</WinNumber>
<ItemText>Oll_diff1,0x0A</ItemText>
</Ww>
<Ww>
<count>14</count>
<WinNumber>1</WinNumber>
<ItemText>Orl_diff1,0x0A</ItemText>
</Ww>
<Ww>
<count>15</count>
<WinNumber>1</WinNumber>
<ItemText>-(App.ADTrack.Stop_VPT),0x0A</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>
<THDelay>0</THDelay>
Project/HC32F460/source/User/Inc/main.h
+10 -10
View File
@@ -12,24 +12,24 @@
#define SLOPE_VPT 1000
#define FOLLOW_VPT 20
#define SHIELD_VPT 2500
#define EME_STOP_VPT 10000
#define SHIELD_VPT 10000
#define EME_STOP_VPT 4000
#define STOP_TIME_MS 0
#define RUN_TIME_MS 0
#define EME_START_MS 3000
#define EME_STOP_MS 3000
#define EME_START_MS 1000
#define EME_STOP_MS 2000
#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_DOWN() (SET_TWIN_BIT1(),CLR_TWIN_BIT0())
#define BDC_STOP() (SET_TWIN_BIT0(),SET_TWIN_BIT1())
#define BDC_UP() (SET_TWIN_BIT0(),CLR_TWIN_BIT1())
#define BDC_DOWN() (SET_TWIN_BIT1(),CLR_TWIN_BIT0())
#define BDC_STOP() Motor_Control(MOTOR_CMD_STOP)
#define BDC_UP() Motor_Control(MOTOR_CMD_FORWARD)
#define BDC_DOWN() Motor_Control(MOTOR_CMD_REVERSE)
//#define BDC_STOP() Motor_Control(MOTOR_CMD_STOP)
//#define BDC_UP() Motor_Control(MOTOR_CMD_FORWARD)
//#define BDC_DOWN() Motor_Control(MOTOR_CMD_REVERSE)
/*控制等级*/
typedef enum {
Project/HC32F460/source/User/Src/main.c
+68 -79
View File
@@ -95,6 +95,19 @@ void GradeControl(ControlLevel_m level)
{
switch(level){
case LEVEL_1:{
App.ADTrack.Slope_VPT = SLOPE_VPT;
App.ADTrack.LeftUpVPT = 5000;
App.ADTrack.LeftLowVPT = 5000;
App.ADTrack.RightUpVPT = 5000;
App.ADTrack.RightLowVPT = 5000;
App.ADTrack.LeftUpShieldVPT = SHIELD_VPT;
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 5000*4/5;
break;}
case LEVEL_2:{
App.ADTrack.Slope_VPT = SLOPE_VPT;
App.ADTrack.LeftUpVPT = 10000;
App.ADTrack.LeftLowVPT = 10000;
@@ -104,10 +117,23 @@ void GradeControl(ControlLevel_m level)
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 10000*4/5;
break;}
case LEVEL_3:{
App.ADTrack.Slope_VPT = SLOPE_VPT;
App.ADTrack.LeftUpVPT = 15000;
App.ADTrack.LeftLowVPT = 15000;
App.ADTrack.RightUpVPT = 15000;
App.ADTrack.RightLowVPT = 15000;
App.ADTrack.LeftUpShieldVPT = SHIELD_VPT;
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 10000;
break;}
case LEVEL_2:{
case LEVEL_4:{
App.ADTrack.Slope_VPT = SLOPE_VPT;
App.ADTrack.LeftUpVPT = 20000;
App.ADTrack.LeftLowVPT = 20000;
@@ -117,46 +143,20 @@ void GradeControl(ControlLevel_m level)
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 20000;
break;}
case LEVEL_3:{
App.ADTrack.Slope_VPT = SLOPE_VPT;
App.ADTrack.LeftUpVPT = 30000;
App.ADTrack.LeftLowVPT = 30000;
App.ADTrack.RightUpVPT = 30000;
App.ADTrack.RightLowVPT = 30000;
App.ADTrack.LeftUpShieldVPT = SHIELD_VPT;
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 30000/2;
break;}
case LEVEL_4:{
App.ADTrack.Slope_VPT = SLOPE_VPT;
App.ADTrack.LeftUpVPT = 40000;
App.ADTrack.LeftLowVPT = 40000;
App.ADTrack.RightUpVPT = 40000;
App.ADTrack.RightLowVPT = 40000;
App.ADTrack.LeftUpShieldVPT = SHIELD_VPT;
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 40000/2;
App.ADTrack.Stop_VPT = 15000;
break;}
case LEVEL_5:{
App.ADTrack.Slope_VPT = SLOPE_VPT;
App.ADTrack.LeftUpVPT = 50000;
App.ADTrack.LeftLowVPT = 50000;
App.ADTrack.RightUpVPT = 50000;
App.ADTrack.RightLowVPT = 50000;
App.ADTrack.LeftUpVPT = 25000;
App.ADTrack.LeftLowVPT = 25000;
App.ADTrack.RightUpVPT = 25000;
App.ADTrack.RightLowVPT = 25000;
App.ADTrack.LeftUpShieldVPT = SHIELD_VPT;
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 50000/2;
App.ADTrack.Stop_VPT = 20000;
break;}
}
}
@@ -265,8 +265,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
if(abs(Oll_diff1) > abs(Orl_diff1))
{
App.LeftRunFalg = true;
// if((App.LeftLowCS1237_SlopeArray[0] > App.ADTrack.Slope_VPT/3))
// {
if((Oll_diff1+Orl_diff1) < -(App.ADTrack.LeftLowVPT/(App.Para.Layout.ControlLevel+1)))
{
BDC_UP();
@@ -275,11 +274,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.DownFlag = false;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
//App.EmergencyDelay1mSCnt = EME_START_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
}
// }
// else if((App.LeftLowCS1237_SlopeArray[0] < -App.ADTrack.Slope_VPT/3))
// {
if((Oll_diff1+Orl_diff1) > App.ADTrack.LeftLowVPT/(App.Para.Layout.ControlLevel+1))
{
BDC_DOWN();
@@ -288,15 +284,12 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.DownFlag = true;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
//App.EmergencyDelay1mSCnt = EME_START_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((Oll_diff1+Orl_diff1) < -(App.ADTrack.RightLowVPT/(App.Para.Layout.ControlLevel+1)))
{
BDC_UP();
@@ -305,11 +298,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.DownFlag = false;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
//App.EmergencyDelay1mSCnt = EME_START_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
}
// }
// else if((App.RightLowCS1237_SlopeArray[0] < -App.ADTrack.Slope_VPT/3))
// {
if((Oll_diff1+Orl_diff1) > App.ADTrack.RightLowVPT/(App.Para.Layout.ControlLevel+1))
{
BDC_DOWN();
@@ -318,9 +308,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.DownFlag = true;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
//App.EmergencyDelay1mSCnt = EME_START_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
}
// }
}
}
else if((LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum))
@@ -333,15 +322,15 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (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)))
(Olu_diff1 >= App.ADTrack.LeftUpVPT*5/6)
&& (Olu_diff2 >= App.ADTrack.LeftUpVPT*5/6)
&& (Olu_diff3 >= App.ADTrack.LeftUpVPT*5/6)
&& (Oll_diff1 <= -(App.ADTrack.LeftLowVPT*5/6))
&& (Oll_diff2 <= -(App.ADTrack.LeftLowVPT*5/6))
&& (Oll_diff3 <= -(App.ADTrack.LeftLowVPT*5/6))))
{
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();
@@ -350,7 +339,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.DownFlag = false;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
//App.EmergencyDelay1mSCnt = EME_START_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
}
}
else if(((App.LeftUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
@@ -368,7 +357,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (Oll_diff1 >= App.ADTrack.LeftLowVPT)))
{
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();
@@ -377,7 +366,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.StopFlag = false;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
//App.EmergencyDelay1mSCnt = EME_START_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
}
}
}
@@ -390,12 +379,12 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (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)))
|| ((Oru_diff1 >= App.ADTrack.RightUpVPT*5/6)
&& (Oru_diff2 >= App.ADTrack.RightUpVPT*5/6)
&& (Oru_diff3 >= App.ADTrack.RightUpVPT*5/6)
&& (Orl_diff1 <= -(App.ADTrack.RightLowVPT*5/6))
&& (Orl_diff2 <= -(App.ADTrack.RightLowVPT*5/6))
&& (Orl_diff3 <= -(App.ADTrack.RightLowVPT*5/6))))
{
if((abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT)
&& (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT)
@@ -407,7 +396,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.DownFlag = false;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
//App.EmergencyDelay1mSCnt = EME_START_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
}
}
else if(((App.RightUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
@@ -434,7 +423,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.UpFlag = false;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
//App.EmergencyDelay1mSCnt = EME_START_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
}
}
}
@@ -465,9 +454,9 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
}
else if((abs(Oll_diff1) + EME_STOP_VPT) < abs(Orl_diff1)
&& (abs(Oll_diff2) + EME_STOP_VPT) < abs(Orl_diff2)
&& (abs(Oll_diff3) + EME_STOP_VPT) < abs(Orl_diff3)
else if((abs(Oll_diff1) - EME_STOP_VPT) < abs(Orl_diff1)
&& (abs(Oll_diff2) - EME_STOP_VPT) < abs(Orl_diff2)
&& (abs(Oll_diff3) - EME_STOP_VPT) < abs(Orl_diff3)
&& App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
{
BDC_STOP();
@@ -499,9 +488,9 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
}
else if(abs(Oll_diff1) > (abs(Orl_diff1) + EME_STOP_VPT)
&& abs(Oll_diff2) > (abs(Orl_diff2) + EME_STOP_VPT)
&& abs(Oll_diff3) > (abs(Orl_diff3) + EME_STOP_VPT)
else if(abs(Oll_diff1) > (abs(Orl_diff1) - EME_STOP_VPT)
&& abs(Oll_diff2) > (abs(Orl_diff2) - EME_STOP_VPT)
&& abs(Oll_diff3) > (abs(Orl_diff3) - EME_STOP_VPT)
&& App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
{
BDC_STOP();
@@ -536,9 +525,9 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
}
else if((abs(Oll_diff1) + EME_STOP_VPT) < abs(Orl_diff1)
&& (abs(Oll_diff2) + EME_STOP_VPT) < abs(Orl_diff2)
&& (abs(Oll_diff3) + EME_STOP_VPT) < abs(Orl_diff3)
else if((abs(Oll_diff1) - EME_STOP_VPT) < abs(Orl_diff1)
&& (abs(Oll_diff2) - EME_STOP_VPT) < abs(Orl_diff2)
&& (abs(Oll_diff3) - EME_STOP_VPT) < abs(Orl_diff3)
&& App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
{
BDC_STOP();
@@ -570,9 +559,9 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.EmergencyDelay1mSCnt = EME_START_MS;
}
else if(abs(Oll_diff1) > (abs(Orl_diff1) + EME_STOP_VPT)
&& abs(Oll_diff2) > (abs(Orl_diff2) + EME_STOP_VPT)
&& abs(Oll_diff3) > (abs(Orl_diff3) + EME_STOP_VPT)
else if(abs(Oll_diff1) > (abs(Orl_diff1) - EME_STOP_VPT)
&& abs(Oll_diff2) > (abs(Orl_diff2) - EME_STOP_VPT)
&& abs(Oll_diff3) > (abs(Orl_diff3) - EME_STOP_VPT)
&& App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
{
BDC_STOP();