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-14 16:25:05 +08:00
parent ad5bf12ca0
commit a0f42649f4
3 changed files with 299 additions and 113 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Project/HC32F460/source/User/Src/main.c
+250 -108
View File
@@ -70,144 +70,284 @@ void RIGHTLOW_CS1237AinADataProcessCallBack(int32_t AD)
App.RightLowCS1237_TrendArray[0] = AD;
App.RightLowCS1237ReadEndFlag = true;
}
#if 0
void SupportControlHandle(int32_t *Ain1_ad, int32_t *Ain2_ad)
#if 1
void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_t *rl_ad)
{
int32_t A1_diff[OSC_CNT], A2_diff[OSC_CNT], O1_diff1, O1_diff2, O1_diff3, O2_diff1, O2_diff2, O2_diff3;
O1_diff1 = Ain1_ad[0] - App.ADTrack.Ain1_Org_AdZero;
O1_diff2 = Ain1_ad[1] - App.ADTrack.Ain1_Org_AdZero;
O1_diff3 = Ain1_ad[2] - App.ADTrack.Ain1_Org_AdZero;
O2_diff1 = Ain2_ad[0] - App.ADTrack.Ain2_Org_AdZero;
O2_diff2 = Ain2_ad[1] - App.ADTrack.Ain2_Org_AdZero;
O2_diff3 = Ain2_ad[2] - App.ADTrack.Ain2_Org_AdZero;
int32_t lu_diff[OSC_CNT], Olu_diff1, Olu_diff2, Olu_diff3;
int32_t ll_diff[OSC_CNT], Oll_diff1, Oll_diff2, Oll_diff3;
int32_t ru_diff[OSC_CNT], Oru_diff1, Oru_diff2, Oru_diff3;
int32_t rl_diff[OSC_CNT], Orl_diff1, Orl_diff2, Orl_diff3;
Olu_diff1 = lu_ad[0] - App.ADTrack.LeftUp_Org_AdZero;
Olu_diff2 = lu_ad[1] - App.ADTrack.LeftUp_Org_AdZero;
Olu_diff3 = lu_ad[2] - App.ADTrack.LeftUp_Org_AdZero;
Oll_diff1 = ll_ad[0] - App.ADTrack.LeftLow_Org_AdZero;
Oll_diff2 = ll_ad[1] - App.ADTrack.LeftLow_Org_AdZero;
Oll_diff3 = ll_ad[2] - App.ADTrack.LeftLow_Org_AdZero;
Oru_diff1 = ru_ad[0] - App.ADTrack.RightUp_Org_AdZero;
Oru_diff2 = ru_ad[1] - App.ADTrack.RightUp_Org_AdZero;
Oru_diff3 = ru_ad[2] - App.ADTrack.RightUp_Org_AdZero;
Orl_diff1 = rl_ad[0] - App.ADTrack.RightLow_Org_AdZero;
Orl_diff2 = rl_ad[1] - App.ADTrack.RightLow_Org_AdZero;
Orl_diff3 = rl_ad[2] - App.ADTrack.RightLow_Org_AdZero;
for(int i = 0; i < OSC_CNT; i++)
{
A1_diff[i] = Ain1_ad[i] - Ain1_ad[OSC_CNT - 1];
A2_diff[i] = Ain2_ad[i] - Ain2_ad[OSC_CNT - 1];
lu_diff[i] = lu_ad[i] - lu_ad[OSC_CNT - 1];
ll_diff[i] = ll_ad[i] - ll_ad[OSC_CNT - 1];
ru_diff[i] = ru_ad[i] - ru_ad[OSC_CNT - 1];
rl_diff[i] = rl_ad[i] - rl_ad[OSC_CNT - 1];
}
TrendResult result_A1 = analyze_trend(A1_diff, OSC_CNT);
TrendResult result_A2 = analyze_trend(A2_diff, OSC_CNT);
TrendResult result_lu = analyze_trend(lu_diff, OSC_CNT);
TrendResult result_ll = analyze_trend(ll_diff, OSC_CNT);
TrendResult result_ru = analyze_trend(ru_diff, OSC_CNT);
TrendResult result_rl = analyze_trend(rl_diff, OSC_CNT);
int32_t A1_slope = result_A1.slope;
int32_t A2_slope = result_A2.slope;
//show2(A2_slope);
int32_t lu_slope = result_lu.slope;
int32_t ll_slope = result_ll.slope;
int32_t ru_slope = result_ru.slope;
int32_t rl_slope = result_rl.slope;
//show(A2_slope);
for(int i = SLOPE_CNT-1; i > 0; i--)
{
App.LeftUpCS1237_SlopeArray[i] = App.LeftUpCS1237_SlopeArray[i - 1];
App.LeftLowCS1237_SlopeArray[i] = App.LeftLowCS1237_SlopeArray[i - 1];
App.RightUpCS1237_SlopeArray[i] = App.RightUpCS1237_SlopeArray[i - 1];
App.RightLowCS1237_SlopeArray[i] = App.RightLowCS1237_SlopeArray[i - 1];
}
App.LeftUpCS1237_SlopeArray[0] = A1_slope;
App.LeftLowCS1237_SlopeArray[0] = A2_slope;
App.LeftUpCS1237_SlopeArray[0] = lu_slope;
App.LeftLowCS1237_SlopeArray[0] = ll_slope;
App.RightUpCS1237_SlopeArray[0] = ru_slope;
App.RightLowCS1237_SlopeArray[0] = rl_slope;
PeakValleyCheck result1_K = check_peaks_valleys(App.LeftUpCS1237_SlopeArray, SLOPE_CNT);
PeakValleyCheck result2_K = check_peaks_valleys(App.LeftLowCS1237_SlopeArray, SLOPE_CNT);
PeakValleyCheck lu_K = check_peaks_valleys(App.LeftUpCS1237_SlopeArray, SLOPE_CNT);
PeakValleyCheck ll_K = check_peaks_valleys(App.LeftLowCS1237_SlopeArray, SLOPE_CNT);
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)
{
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))
|| ((O1_diff1 >= App.ADTrack.Ain1VPT)
&& (O1_diff2 >= App.ADTrack.Ain1VPT)
&& (O1_diff3 >= App.ADTrack.Ain1VPT)
&& (O2_diff1 <= -App.ADTrack.Ain2VPT)
&& (O2_diff2 <= -App.ADTrack.Ain2VPT)
&& (O2_diff3 <= -App.ADTrack.Ain2VPT)))
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_ad[0]) + abs(ll_ad[0]);
uint32_t RightDiffSum = abs(ru_ad[0]) + abs(rl_ad[0]);
App.LeftRunFalg = false;
App.RightRunFalg = false;
if(LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum)
{
//BDC_UP();
App.UpFlag = true;
App.StopFlag = false;
App.DownFlag = false;
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)))
{
//BDC_UP();
App.UpFlag = true;
App.StopFlag = false;
App.DownFlag = false;
}
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)))
{
//BDC_DOWN();
App.DownFlag = true;
App.StopFlag = false;
App.UpFlag = false;
}
}
else if(((App.Ain1_SlopeArray[0] >= App.ADTrack.Slope_VPT)
&& (App.Ain1_SlopeArray[1] >= App.ADTrack.Slope_VPT)
&& (App.Ain1_SlopeArray[2] >= App.ADTrack.Slope_VPT)
&& (App.Ain2_SlopeArray[0] <= -App.ADTrack.Slope_VPT)
&& (App.Ain2_SlopeArray[1] <= -App.ADTrack.Slope_VPT)
&& (App.Ain2_SlopeArray[2] <= -App.ADTrack.Slope_VPT))
|| ((O1_diff1 <= -App.ADTrack.Ain1VPT)
&& (O1_diff2 <= -App.ADTrack.Ain1VPT)
&& (O1_diff3 <= -App.ADTrack.Ain1VPT)
&& (O2_diff1 >= App.ADTrack.Ain2VPT)
&& (O2_diff2 >= App.ADTrack.Ain2VPT)
&& (O2_diff3 >= App.ADTrack.Ain2VPT)))
else if(LeftSlopeSum < RightSlopeSum && LeftDiffSum < RightDiffSum)
{
//BDC_DOWN();
App.DownFlag = true;
App.StopFlag = false;
App.UpFlag = false;
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)))
{
//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)))
{
//BDC_DOWN();
App.DownFlag = true;
App.StopFlag = false;
App.UpFlag = false;
}
}
}
}
else if(App.DownFlag == true)
{
if(A1_slope < -App.ADTrack.Slope_VPT
&& A2_slope > App.ADTrack.Slope_VPT)
{
//BDC_STOP();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.StopCollDelay1mSCnt = 1000;
}
else if(O1_diff1 > -(App.ADTrack.Ain1VPT / 2)
&& O2_diff1 < (App.ADTrack.Ain2VPT / 2))
{
//BDC_STOP();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.StopCollDelay1mSCnt = 1000;
}
}
else if(App.UpFlag == true)
{
if(A1_slope > App.ADTrack.Slope_VPT
&& A2_slope < -App.ADTrack.Slope_VPT)
if(App.LeftRunFalg == true)
{
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;
}
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 = 1000;
}
}
else if(O1_diff1 < (App.ADTrack.Ain1VPT / 2)
&& O2_diff1 > -(App.ADTrack.Ain2VPT / 2))
else if(App.RightRunFalg == true)
{
//BDC_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 = 1000;
}
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 = 1000;
}
}
}
else if(App.UpFlag == true)
{
if(App.LeftRunFalg == true)
{
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;
}
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 = 1000;
}
}
else if(App.RightRunFalg == true)
{
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 = 1000;
}
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 = 1000;
}
}
}
if (result1_K.peaks_positive
&& result1_K.valleys_negative
&& A1_diff[0] < App.ADTrack.Slope_VPT / 2
&& A1_diff[0] > -App.ADTrack.Slope_VPT / 2
&& A1_diff[1] < App.ADTrack.Slope_VPT / 2
&& A1_diff[1] > -App.ADTrack.Slope_VPT / 2
&& A1_diff[2] < App.ADTrack.Slope_VPT / 2
&& A1_diff[2] > -App.ADTrack.Slope_VPT / 2
&& result2_K.peaks_positive
&& result2_K.valleys_negative
&& A2_diff[0] < App.ADTrack.Slope_VPT / 2
&& A2_diff[0] > -App.ADTrack.Slope_VPT / 2
&& A2_diff[1] < App.ADTrack.Slope_VPT / 2
&& A2_diff[1] > -App.ADTrack.Slope_VPT / 2
&& A2_diff[2] < App.ADTrack.Slope_VPT / 2
&& A2_diff[2] > -App.ADTrack.Slope_VPT / 2)
if (lu_K.peaks_positive
&& lu_K.valleys_negative
&& lu_diff[0] < App.ADTrack.Slope_VPT / 2
&& lu_diff[0] > -App.ADTrack.Slope_VPT / 2
&& lu_diff[1] < App.ADTrack.Slope_VPT / 2
&& lu_diff[1] > -App.ADTrack.Slope_VPT / 2
&& lu_diff[2] < App.ADTrack.Slope_VPT / 2
&& lu_diff[2] > -App.ADTrack.Slope_VPT / 2
&& ll_K.peaks_positive
&& ll_K.valleys_negative
&& ll_diff[0] < App.ADTrack.Slope_VPT / 2
&& ll_diff[0] > -App.ADTrack.Slope_VPT / 2
&& ll_diff[1] < App.ADTrack.Slope_VPT / 2
&& ll_diff[1] > -App.ADTrack.Slope_VPT / 2
&& ll_diff[2] < App.ADTrack.Slope_VPT / 2
&& ll_diff[2] > -App.ADTrack.Slope_VPT / 2
&& ru_K.peaks_positive
&& ru_K.valleys_negative
&& ru_diff[0] < App.ADTrack.Slope_VPT / 2
&& ru_diff[0] > -App.ADTrack.Slope_VPT / 2
&& ru_diff[1] < App.ADTrack.Slope_VPT / 2
&& ru_diff[1] > -App.ADTrack.Slope_VPT / 2
&& ru_diff[2] < App.ADTrack.Slope_VPT / 2
&& ru_diff[2] > -App.ADTrack.Slope_VPT / 2
&& rl_K.peaks_positive
&& rl_K.valleys_negative
&& rl_diff[0] < App.ADTrack.Slope_VPT / 2
&& rl_diff[0] > -App.ADTrack.Slope_VPT / 2
&& rl_diff[1] < App.ADTrack.Slope_VPT / 2
&& rl_diff[1] > -App.ADTrack.Slope_VPT / 2
&& rl_diff[2] < App.ADTrack.Slope_VPT / 2
&& rl_diff[2] > -App.ADTrack.Slope_VPT / 2)
{
if(App.StopFlag == true && App.StopCollDelay1mSCnt <= 0){
App.ADTrack.Ain1_Org_AdZero = Ain1_ad[0];
App.ADTrack.Ain2_Org_AdZero = Ain2_ad[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];
}
else if(App.StopFlag == false)
{
@@ -224,8 +364,10 @@ void SupportControlHandle(int32_t *Ain1_ad, int32_t *Ain2_ad)
{
App.EvenRunning = false;
App.EvenRunDelay1mSCnt = 0;
App.ADTrack.Ain1_Org_AdZero = Ain1_ad[0];
App.ADTrack.Ain2_Org_AdZero = Ain2_ad[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
@@ -250,6 +392,8 @@ static void AppInit(void)
App.Status = APP_STATUS_WAIT;
App.TD1mSDelayCnt = 2000;
App.RStatus = READ_STATUS_START;
App.WaitFlag = false;
}
static void ReadLoopHandler(void)
{
@@ -337,13 +481,10 @@ static void ReadLoopHandler(void)
if(App.LeftUpCS1237ReadEndFlag == true &&
App.LeftLowCS1237ReadEndFlag == true &&
App.RightUpCS1237ReadEndFlag == true &&
App.RightLowCS1237ReadEndFlag == true)
App.RightLowCS1237ReadEndFlag == true &&
App.WaitFlag == true)
{
App.LeftUpCS1237ReadEndFlag = false;
App.LeftUpCS1237ReadEndFlag = false;
App.RightUpCS1237ReadEndFlag = false;
App.RightLowCS1237ReadEndFlag = false;
//SupportControlHandle(App.Ain1_TrendArray, App.Ain2_TrendArray);
SupportControlHandle(App.LeftUpCS1237_TrendArray, App.LeftLowCS1237_TrendArray, App.RightUpCS1237_TrendArray, App.RightLowCS1237_TrendArray);
}
App.RStatus = READ_STATUS_START;
break;}
@@ -368,6 +509,7 @@ static void AppLoopHandler(void)
case APP_STATUS_WAIT:{
if(App.TD1mSDelayCnt <= 0)
{
App.WaitFlag = true;
App.UpFlag = false;
App.DownFlag = false;
App.StopFlag = true;