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

双柱双电机S型传感器版本更新

Browse Source 
1.优化了屏幕中间控制用力不均匀的问题
2.优化了控制算法
This commit is contained in:
DESKTOP-KQHDCM4\97555
2025-10-17 13:59:16 +08:00
parent a6f4d0fb17
commit 60b35ce978
3 changed files with 62 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Project/HC32F460/source/User/Src/main.c
+61 -31
View File
@@ -217,37 +217,67 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.LeftRunFalg = false;
App.RightRunFalg = false;
App.CentreRunFalg = false;
if(((ll_ad[0] > App.ADTrack.LeftLow_Org_AdZero)
&& (rl_ad[0] > App.ADTrack.RightLow_Org_AdZero))
|| ((ll_ad[0] < App.ADTrack.LeftLow_Org_AdZero)
&& (rl_ad[0] < App.ADTrack.RightLow_Org_AdZero)))
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(((Oll_diff1 + Orl_diff1) < -(App.ADTrack.LeftLowVPT + App.ADTrack.RightLowVPT)/2)
&& (((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) < -App.ADTrack.Slope_VPT/4)))
{
BDC_UP();
App.UpFlag = true;
App.StopFlag = false;
App.DownFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
}
else if(((Oll_diff1 + Orl_diff1) > (App.ADTrack.LeftLowVPT + App.ADTrack.RightLowVPT)/2)
&& (((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT/4)))
{
BDC_DOWN();
App.UpFlag = false;
App.StopFlag = false;
App.DownFlag = true;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
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))
@@ -360,7 +390,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
if(App.LeftRunFalg == true)
{
if(((App.CentreRunFalg == false ) && (ll_slope > App.ADTrack.Slope_VPT))
|| ((App.CentreRunFalg == true ) && (App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) < -App.ADTrack.Slope_VPT/3))
|| ((App.CentreRunFalg == true ) && (App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) < -App.ADTrack.Slope_VPT))
{
BDC_STOP();
App.StopFlag = true;
@@ -380,8 +410,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
}
else if(App.RightRunFalg == true)
{
if(((App.CentreRunFalg == false ) && (ll_slope > App.ADTrack.Slope_VPT))
|| ((App.CentreRunFalg == true ) && (App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) < -App.ADTrack.Slope_VPT/3))
if(((App.CentreRunFalg == false ) && (rl_slope > App.ADTrack.Slope_VPT))
|| ((App.CentreRunFalg == true ) && (App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) < -App.ADTrack.Slope_VPT))
{
BDC_STOP();
App.StopFlag = true;
@@ -405,7 +435,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
if(App.LeftRunFalg == true)
{
if(((App.CentreRunFalg == false ) && (ll_slope < -App.ADTrack.Slope_VPT))
|| ((App.CentreRunFalg == true ) && (App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT/3))
|| ((App.CentreRunFalg == true ) && (App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT))
{
BDC_STOP();
App.StopFlag = true;
@@ -426,7 +456,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
else if(App.RightRunFalg == true)
{
if(((App.CentreRunFalg == false ) && (rl_slope < -App.ADTrack.Slope_VPT))
|| ((App.CentreRunFalg == true ) && (App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT/3))
|| ((App.CentreRunFalg == true ) && (App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT))
{
BDC_STOP();
App.StopFlag = true;
@@ -538,10 +568,10 @@ static void AppInit(void)
RIGHTUP_CS1237_Init();
RIGHTLOW_CS1237_Init();
App.Status = APP_STATUS_WAIT;
App.TD1mSDelayCnt = 1000;
// App.Status = APP_STATUS_UP1;
// App.TD1mSDelayCnt = TRAVEL_TIME_MS;
// App.Status = APP_STATUS_WAIT;
// App.TD1mSDelayCnt = 1000;
App.Status = APP_STATUS_UP1;
App.TD1mSDelayCnt = TRAVEL_TIME_MS;
App.RStatus = READ_STATUS_START;
GradeControl(App.Para.Layout.ControlLevel);//等级控制