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/MDK/HC32F460.uvoptx
+43 -3
View File
@@ -153,17 +153,57 @@
<Ww> <Ww>
<count>0</count> <count>0</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>LEFTLOW_CS1237Var.CS1237Delay1mSCnt,0x0A</ItemText> <ItemText>App.WaitFlag</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>1</count> <count>1</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>App.FeedDogDlyCnt,0x0A</ItemText> <ItemText>App.UpFlag</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>2</count> <count>2</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>App.Status,0x0A</ItemText> <ItemText>App.DownFlag</ItemText>
</Ww>
<Ww>
<count>3</count>
<WinNumber>1</WinNumber>
<ItemText>App.StopFlag</ItemText>
</Ww>
<Ww>
<count>4</count>
<WinNumber>1</WinNumber>
<ItemText>App.EvenRunning</ItemText>
</Ww>
<Ww>
<count>5</count>
<WinNumber>1</WinNumber>
<ItemText>App.LeftRunFalg</ItemText>
</Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>App.RightRunFalg</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>App.ADTrack.LeftUp_Org_AdZero,0x0A</ItemText>
</Ww>
<Ww>
<count>8</count>
<WinNumber>1</WinNumber>
<ItemText>App.ADTrack.LeftLow_Org_AdZero,0x0A</ItemText>
</Ww>
<Ww>
<count>9</count>
<WinNumber>1</WinNumber>
<ItemText>App.ADTrack.RightUp_Org_AdZero,0x0A</ItemText>
</Ww>
<Ww>
<count>10</count>
<WinNumber>1</WinNumber>
<ItemText>App.ADTrack.RightLow_Org_AdZero,0x0A</ItemText>
</Ww> </Ww>
</WatchWindow1> </WatchWindow1>
<Tracepoint> <Tracepoint>
Project/HC32F460/source/User/Inc/main.h
+6 -2
View File
@@ -20,9 +20,9 @@ typedef struct{
int32_t RightLowVPT;//右下设备阈值 int32_t RightLowVPT;//右下设备阈值
int32_t LeftUp_Org_AdZero;//左上设备原始AD零点 int32_t LeftUp_Org_AdZero;//左上设备原始AD零点
int32_t LeftLow_Org_AdZero;//左上设备原始AD零点 int32_t LeftLow_Org_AdZero;//左下设备原始AD零点
int32_t RightUp_Org_AdZero;//右上设备原始AD零点 int32_t RightUp_Org_AdZero;//右上设备原始AD零点
int32_t RightLow_Org_AdZero;//右上设备原始AD零点 int32_t RightLow_Org_AdZero;//右下设备原始AD零点
}ADTrack_t; }ADTrack_t;
/*数据读取状态机*/ /*数据读取状态机*/
@@ -73,6 +73,8 @@ typedef struct {
bool DownFlag;//下降标志位 bool DownFlag;//下降标志位
bool StopFlag;//停止标志位 bool StopFlag;//停止标志位
bool EvenRunning;//平稳运行标志位 bool EvenRunning;//平稳运行标志位
bool LeftRunFalg;
bool RightRunFalg;
bool LeftUpCS1237ReadEndFlag; bool LeftUpCS1237ReadEndFlag;
bool LeftLowCS1237ReadEndFlag; bool LeftLowCS1237ReadEndFlag;
@@ -88,6 +90,8 @@ typedef struct {
int32_t LeftLowCS1237_SlopeArray[SLOPE_CNT]; int32_t LeftLowCS1237_SlopeArray[SLOPE_CNT];
int32_t RightUpCS1237_SlopeArray[SLOPE_CNT]; int32_t RightUpCS1237_SlopeArray[SLOPE_CNT];
int32_t RightLowCS1237_SlopeArray[SLOPE_CNT]; int32_t RightLowCS1237_SlopeArray[SLOPE_CNT];
bool WaitFlag;
}AppDetect_t; }AppDetect_t;
extern AppDetect_t App; extern AppDetect_t App;
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.RightLowCS1237_TrendArray[0] = AD;
App.RightLowCS1237ReadEndFlag = true; App.RightLowCS1237ReadEndFlag = true;
} }
#if 0 #if 1
void SupportControlHandle(int32_t *Ain1_ad, int32_t *Ain2_ad) 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; int32_t lu_diff[OSC_CNT], Olu_diff1, Olu_diff2, Olu_diff3;
O1_diff1 = Ain1_ad[0] - App.ADTrack.Ain1_Org_AdZero; int32_t ll_diff[OSC_CNT], Oll_diff1, Oll_diff2, Oll_diff3;
O1_diff2 = Ain1_ad[1] - App.ADTrack.Ain1_Org_AdZero; int32_t ru_diff[OSC_CNT], Oru_diff1, Oru_diff2, Oru_diff3;
O1_diff3 = Ain1_ad[2] - App.ADTrack.Ain1_Org_AdZero; int32_t rl_diff[OSC_CNT], Orl_diff1, Orl_diff2, Orl_diff3;
O2_diff1 = Ain2_ad[0] - App.ADTrack.Ain2_Org_AdZero;
O2_diff2 = Ain2_ad[1] - App.ADTrack.Ain2_Org_AdZero; Olu_diff1 = lu_ad[0] - App.ADTrack.LeftUp_Org_AdZero;
O2_diff3 = Ain2_ad[2] - App.ADTrack.Ain2_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++) for(int i = 0; i < OSC_CNT; i++)
{ {
A1_diff[i] = Ain1_ad[i] - Ain1_ad[OSC_CNT - 1]; lu_diff[i] = lu_ad[i] - lu_ad[OSC_CNT - 1];
A2_diff[i] = Ain2_ad[i] - Ain2_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_lu = analyze_trend(lu_diff, OSC_CNT);
TrendResult result_A2 = analyze_trend(A2_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 lu_slope = result_lu.slope;
int32_t A2_slope = result_A2.slope; int32_t ll_slope = result_ll.slope;
//show2(A2_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--) for(int i = SLOPE_CNT-1; i > 0; i--)
{ {
App.LeftUpCS1237_SlopeArray[i] = App.LeftUpCS1237_SlopeArray[i - 1]; App.LeftUpCS1237_SlopeArray[i] = App.LeftUpCS1237_SlopeArray[i - 1];
App.LeftLowCS1237_SlopeArray[i] = App.LeftLowCS1237_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.LeftUpCS1237_SlopeArray[0] = lu_slope;
App.LeftLowCS1237_SlopeArray[0] = A2_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 lu_K = check_peaks_valleys(App.LeftUpCS1237_SlopeArray, SLOPE_CNT);
PeakValleyCheck result2_K = check_peaks_valleys(App.LeftLowCS1237_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.StopFlag == true)
{ {
if(App.StopCollDelay1mSCnt <= 0) if(App.StopCollDelay1mSCnt <= 0)
{ {
if(((App.LeftUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) uint32_t LeftSlopeSum = abs(App.LeftUpCS1237_SlopeArray[0]) + abs(App.LeftLowCS1237_SlopeArray[0]);
&& (App.LeftUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) uint32_t RightSlopeSum = abs(App.RightUpCS1237_SlopeArray[0]) + abs(App.RightLowCS1237_SlopeArray[0]);
&& (App.LeftUpCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT) uint32_t LeftDiffSum = abs(lu_ad[0]) + abs(ll_ad[0]);
&& (App.LeftLowCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) uint32_t RightDiffSum = abs(ru_ad[0]) + abs(rl_ad[0]);
&& (App.LeftLowCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)) App.LeftRunFalg = false;
|| ((O1_diff1 >= App.ADTrack.Ain1VPT) App.RightRunFalg = false;
&& (O1_diff2 >= App.ADTrack.Ain1VPT) if(LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum)
&& (O1_diff3 >= App.ADTrack.Ain1VPT)
&& (O2_diff1 <= -App.ADTrack.Ain2VPT)
&& (O2_diff2 <= -App.ADTrack.Ain2VPT)
&& (O2_diff3 <= -App.ADTrack.Ain2VPT)))
{ {
//BDC_UP(); App.LeftRunFalg = true;
App.UpFlag = true; if(((App.LeftUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT)
App.StopFlag = false; && (App.LeftUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT)
App.DownFlag = false; && (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) else if(LeftSlopeSum < RightSlopeSum && LeftDiffSum < RightDiffSum)
&& (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)))
{ {
//BDC_DOWN(); App.RightRunFalg = true;
App.DownFlag = true; if(((App.RightUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT)
App.StopFlag = false; && (App.RightUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT)
App.UpFlag = false; && (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) else if(App.DownFlag == true)
{ {
if(A1_slope < -App.ADTrack.Slope_VPT if(App.LeftRunFalg == true)
&& 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(lu_slope < -App.ADTrack.Slope_VPT
&& ll_slope > App.ADTrack.Slope_VPT)
{
//BDC_STOP(); //BDC_STOP();
App.StopFlag = true; App.StopFlag = true;
App.DownFlag = false; App.DownFlag = false;
App.UpFlag = false; App.UpFlag = false;
App.StopCollDelay1mSCnt = 1000; 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) else if(App.RightRunFalg == true)
&& O2_diff1 > -(App.ADTrack.Ain2VPT / 2))
{ {
//BDC_STOP(); if(ru_slope < -App.ADTrack.Slope_VPT
App.StopFlag = true; && rl_slope > App.ADTrack.Slope_VPT)
App.DownFlag = false; {
App.UpFlag = false; //BDC_STOP();
App.StopCollDelay1mSCnt = 1000; 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 if (lu_K.peaks_positive
&& result1_K.valleys_negative && lu_K.valleys_negative
&& A1_diff[0] < App.ADTrack.Slope_VPT / 2 && lu_diff[0] < App.ADTrack.Slope_VPT / 2
&& A1_diff[0] > -App.ADTrack.Slope_VPT / 2 && lu_diff[0] > -App.ADTrack.Slope_VPT / 2
&& A1_diff[1] < App.ADTrack.Slope_VPT / 2 && lu_diff[1] < App.ADTrack.Slope_VPT / 2
&& A1_diff[1] > -App.ADTrack.Slope_VPT / 2 && lu_diff[1] > -App.ADTrack.Slope_VPT / 2
&& A1_diff[2] < App.ADTrack.Slope_VPT / 2 && lu_diff[2] < App.ADTrack.Slope_VPT / 2
&& A1_diff[2] > -App.ADTrack.Slope_VPT / 2 && lu_diff[2] > -App.ADTrack.Slope_VPT / 2
&& result2_K.peaks_positive && ll_K.peaks_positive
&& result2_K.valleys_negative && ll_K.valleys_negative
&& A2_diff[0] < App.ADTrack.Slope_VPT / 2 && ll_diff[0] < App.ADTrack.Slope_VPT / 2
&& A2_diff[0] > -App.ADTrack.Slope_VPT / 2 && ll_diff[0] > -App.ADTrack.Slope_VPT / 2
&& A2_diff[1] < App.ADTrack.Slope_VPT / 2 && ll_diff[1] < App.ADTrack.Slope_VPT / 2
&& A2_diff[1] > -App.ADTrack.Slope_VPT / 2 && ll_diff[1] > -App.ADTrack.Slope_VPT / 2
&& A2_diff[2] < App.ADTrack.Slope_VPT / 2 && ll_diff[2] < App.ADTrack.Slope_VPT / 2
&& A2_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){ if(App.StopFlag == true && App.StopCollDelay1mSCnt <= 0){
App.ADTrack.Ain1_Org_AdZero = Ain1_ad[0]; App.ADTrack.LeftUp_Org_AdZero = lu_ad[0];
App.ADTrack.Ain2_Org_AdZero = Ain2_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) else if(App.StopFlag == false)
{ {
@@ -224,8 +364,10 @@ void SupportControlHandle(int32_t *Ain1_ad, int32_t *Ain2_ad)
{ {
App.EvenRunning = false; App.EvenRunning = false;
App.EvenRunDelay1mSCnt = 0; App.EvenRunDelay1mSCnt = 0;
App.ADTrack.Ain1_Org_AdZero = Ain1_ad[0]; App.ADTrack.LeftUp_Org_AdZero = lu_ad[0];
App.ADTrack.Ain2_Org_AdZero = Ain2_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 #endif
@@ -250,6 +392,8 @@ static void AppInit(void)
App.Status = APP_STATUS_WAIT; App.Status = APP_STATUS_WAIT;
App.TD1mSDelayCnt = 2000; App.TD1mSDelayCnt = 2000;
App.RStatus = READ_STATUS_START; App.RStatus = READ_STATUS_START;
App.WaitFlag = false;
} }
static void ReadLoopHandler(void) static void ReadLoopHandler(void)
{ {
@@ -337,13 +481,10 @@ static void ReadLoopHandler(void)
if(App.LeftUpCS1237ReadEndFlag == true && if(App.LeftUpCS1237ReadEndFlag == true &&
App.LeftLowCS1237ReadEndFlag == true && App.LeftLowCS1237ReadEndFlag == true &&
App.RightUpCS1237ReadEndFlag == true && App.RightUpCS1237ReadEndFlag == true &&
App.RightLowCS1237ReadEndFlag == true) App.RightLowCS1237ReadEndFlag == true &&
App.WaitFlag == true)
{ {
App.LeftUpCS1237ReadEndFlag = false; SupportControlHandle(App.LeftUpCS1237_TrendArray, App.LeftLowCS1237_TrendArray, App.RightUpCS1237_TrendArray, App.RightLowCS1237_TrendArray);
App.LeftUpCS1237ReadEndFlag = false;
App.RightUpCS1237ReadEndFlag = false;
App.RightLowCS1237ReadEndFlag = false;
//SupportControlHandle(App.Ain1_TrendArray, App.Ain2_TrendArray);
} }
App.RStatus = READ_STATUS_START; App.RStatus = READ_STATUS_START;
break;} break;}
@@ -368,6 +509,7 @@ static void AppLoopHandler(void)
case APP_STATUS_WAIT:{ case APP_STATUS_WAIT:{
if(App.TD1mSDelayCnt <= 0) if(App.TD1mSDelayCnt <= 0)
{ {
App.WaitFlag = true;
App.UpFlag = false; App.UpFlag = false;
App.DownFlag = false; App.DownFlag = false;
App.StopFlag = true; App.StopFlag = true;