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/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();