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:
YuanHongbin
2025-09-15 15:15:26 +08:00
parent 7dc8f7c6d7
commit f296a8eb48
5 changed files with 165 additions and 130 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Project/HC32F460/source/User/Inc/main.h
+9 -1
View File
@@ -9,8 +9,15 @@
#define OSC_CNT 40
#define SLOPE_CNT 30
#define SLOPE_CNT 20
#define STOP_TIME_MS 500
#define RUN_TIME_MS 10
#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 struct{
int32_t Slope_VPT;
@@ -66,6 +73,7 @@ typedef struct {
uint32_t Read1mSDelayCnt;
uint32_t Uart1Delay1mSCnt;
uint32_t StopCollDelay1mSCnt;
uint32_t RunCollDelay1mSCnt;
uint32_t EvenRunDelay1mSCnt;
uint8_t FeedDogDlyCnt;
Project/HC32F460/source/User/Src/main.c
+123 -115
View File
@@ -147,18 +147,19 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (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)))
// || ((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))
)
{
//BDC_UP();
Motor_Control(MOTOR_CMD_FORWARD);
BDC_UP();
App.UpFlag = true;
App.StopFlag = false;
App.DownFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
}
else if(((App.LeftUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
&& (App.LeftUpCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT)
@@ -166,18 +167,19 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (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)))
// || ((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))
)
{
//BDC_DOWN();
Motor_Control(MOTOR_CMD_REVERSE);
BDC_DOWN();
App.DownFlag = true;
App.StopFlag = false;
App.UpFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
}
}
else if(LeftSlopeSum < RightSlopeSum && LeftDiffSum < RightDiffSum)
@@ -189,18 +191,19 @@ 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)
// && (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))
)
{
//BDC_UP();
Motor_Control(MOTOR_CMD_FORWARD);
BDC_UP();
App.UpFlag = true;
App.StopFlag = false;
App.DownFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
}
else if(((App.RightUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
&& (App.RightUpCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT)
@@ -208,117 +211,120 @@ 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)
// && (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))
)
{
//BDC_DOWN();
Motor_Control(MOTOR_CMD_REVERSE);
BDC_DOWN();
App.DownFlag = true;
App.StopFlag = false;
App.UpFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
}
}
}
}
else if(App.DownFlag == true)
{
if(App.LeftRunFalg == true)
if(App.RunCollDelay1mSCnt <= 0)
{
if(lu_slope < -App.ADTrack.Slope_VPT
&& ll_slope > App.ADTrack.Slope_VPT)
if(App.LeftRunFalg == true)
{
//BDC_STOP();
Motor_Control(MOTOR_CMD_STOP);
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.StopCollDelay1mSCnt = 1000;
if(lu_slope < -App.ADTrack.Slope_VPT
&& ll_slope > App.ADTrack.Slope_VPT)
{
BDC_STOP();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
else if(Olu_diff1 > -(App.ADTrack.LeftUpVPT / 2)
&& Oll_diff1 < (App.ADTrack.LeftLowVPT / 2))
{
BDC_STOP();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
}
else if(Olu_diff1 > -(App.ADTrack.LeftUpVPT / 2)
&& Oll_diff1 < (App.ADTrack.LeftLowVPT / 2))
else if(App.RightRunFalg == true)
{
//BDC_STOP();
Motor_Control(MOTOR_CMD_STOP);
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.StopCollDelay1mSCnt = 1000;
}
}
else if(App.RightRunFalg == true)
{
if(ru_slope < -App.ADTrack.Slope_VPT
&& rl_slope > App.ADTrack.Slope_VPT)
{
//BDC_STOP();
Motor_Control(MOTOR_CMD_STOP);
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.StopCollDelay1mSCnt = 1000;
}
else if(Oru_diff1 > -(App.ADTrack.RightUpVPT / 2)
&& Orl_diff1 < (App.ADTrack.RightLowVPT / 2))
{
//BDC_STOP();
Motor_Control(MOTOR_CMD_STOP);
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.StopCollDelay1mSCnt = 1000;
if(ru_slope < -App.ADTrack.Slope_VPT
//&& rl_slope > App.ADTrack.Slope_VPT
)
{
BDC_STOP();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
// else if(Oru_diff1 > -(App.ADTrack.RightUpVPT / 2)
// //&& Orl_diff1 < (App.ADTrack.RightLowVPT / 2)
// )
// {
// BDC_STOP();
// App.StopFlag = true;
// App.DownFlag = false;
// App.UpFlag = false;
// App.StopCollDelay1mSCnt = STOP_TIME_MS;
// }
}
}
}
else if(App.UpFlag == true)
{
if(App.LeftRunFalg == true)
if(App.RunCollDelay1mSCnt <= 0)
{
if(lu_slope > App.ADTrack.Slope_VPT
&& ll_slope < -App.ADTrack.Slope_VPT)
if(App.LeftRunFalg == true)
{
//BDC_STOP();
Motor_Control(MOTOR_CMD_STOP);
if(lu_slope > App.ADTrack.Slope_VPT
&& ll_slope < -App.ADTrack.Slope_VPT)
{
BDC_STOP();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.StopCollDelay1mSCnt = 1000;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
else if(Olu_diff1 < (App.ADTrack.LeftUpVPT / 2)
&& Oll_diff1 > -(App.ADTrack.LeftLowVPT / 2))
{
BDC_STOP();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
}
else if(Olu_diff1 < (App.ADTrack.LeftUpVPT / 2)
&& Oll_diff1 > -(App.ADTrack.LeftLowVPT / 2))
else if(App.RightRunFalg == true)
{
//BDC_STOP();
Motor_Control(MOTOR_CMD_STOP);
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.StopCollDelay1mSCnt = 1000;
}
}
else if(App.RightRunFalg == true)
{
if(ru_slope > App.ADTrack.Slope_VPT
&& rl_slope < -App.ADTrack.Slope_VPT)
{
//BDC_STOP();
Motor_Control(MOTOR_CMD_STOP);
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.StopCollDelay1mSCnt = 1000;
}
else if(Oru_diff1 < (App.ADTrack.RightUpVPT / 2)
&& Orl_diff1 > -(App.ADTrack.RightLowVPT / 2))
{
//BDC_STOP();
Motor_Control(MOTOR_CMD_STOP);
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.StopCollDelay1mSCnt = 1000;
if(ru_slope > App.ADTrack.Slope_VPT
//&& rl_slope < -App.ADTrack.Slope_VPT
)
{
BDC_STOP();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
// else if(Oru_diff1 < (App.ADTrack.RightUpVPT / 2)
// && Orl_diff1 > -(App.ADTrack.RightLowVPT / 2)
// )
// {
// BDC_STOP();
// App.StopFlag = true;
// App.DownFlag = false;
// App.UpFlag = false;
// App.StopCollDelay1mSCnt = STOP_TIME_MS;
// }
}
}
}
@@ -521,18 +527,18 @@ static void AppLoopHandler(void)
case APP_STATUS_WAIT:{
if(App.TD1mSDelayCnt <= 0)
{
{
App.WaitFlag = true;
App.UpFlag = false;
App.DownFlag = false;
App.StopFlag = true;
App.EvenRunning = false;
App.ADTrack.Slope_VPT = 1000;
App.ADTrack.LeftUpVPT = 20000;
App.ADTrack.LeftLowVPT = 20000;
App.ADTrack.RightUpVPT = 20000;
App.ADTrack.RightLowVPT = 20000;
App.ADTrack.Slope_VPT = 1500;
App.ADTrack.LeftUpVPT = 10000;
App.ADTrack.LeftLowVPT = 10000;
App.ADTrack.RightUpVPT = 10000;
App.ADTrack.RightLowVPT = 10000;
App.ADTrack.LeftUp_Org_AdZero = App.LeftUpCS1237_TrendArray[0];
App.ADTrack.LeftLow_Org_AdZero = App.LeftLowCS1237_TrendArray[0];
@@ -558,6 +564,8 @@ static void App1mSRoutine(void)
App.Uart1Delay1mSCnt--;
if(App.StopCollDelay1mSCnt > 0)
App.StopCollDelay1mSCnt--;
if(App.RunCollDelay1mSCnt > 0)
App.RunCollDelay1mSCnt--;
if(App.EvenRunning == true)
App.EvenRunDelay1mSCnt++;