YuanHongbin/DoubleColumn_Trolley
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
53897050bcaf1f1217fba86d688049a221b92971
DoubleColumn_Trolley/Project/HC32F460/source/User/Src/main.c
T

1483 lines
50 KiB
C
Raw Blame History

#include <stdlib.h>
#include <math.h>
#include "main.h"
#include "Encryption.h"
#include "Debug.h"
#include "Algorithm.h"
#include "dc_motor.h"
#include "CS1237_LEFTUP.h"
#include "CS1237_LEFTLOW.h"
#include "CS1237_RIGHTUP.h"
#include "CS1237_RIGHTLOW.h"
#include "BeepAndLed.h"
#define MAIN_DBG_LOG(...) DBG_LOG(__VA_ARGS__)
#define MAIN_DBG_ARRAY(ARRAY, SIZE) DBG_ARRAY(ARRAY,SIZE)
#define SOFTWARE_VERSION 11 //软件版本
#define HARDWARE_VERSION 20 //硬件版本
AppDetect_t App;
static void show(uint8_t ch,int32_t ad)
{
if(App.Para.Layout.DBGWaveOut == true)
{
uint8_t bao[7];
bao[0] = 0x5A;
bao[1] = ch;
bao[2] = ad & 0xFF;
bao[3] = ad >> 8 & 0xFF;
bao[4] = ad >> 16 & 0xFF;
bao[5] = ad >> 24 & 0xFF;
bao[6] = 0xA5;
DebugUartSend(bao,7);
}
}
void LEFTUP_CS1237AinADataProcessCallBack(int32_t AD)
{
// if(abs(AD - App.LeftUpCS1237_TrendArray[0]) > 2000000)
// {
// AD = App.LeftUpCS1237_TrendArray[0];
// }
if(App.MotorPara.CaliFlag == true)
{
AD = AD + App.MotorPara.LU_HeightStress[(App.MotorPara.Current_height)-725];
for(int i = OSC_CNT-1; i > 0; i--)
{
App.LeftUpCS1237_TrendArray[i] = App.LeftUpCS1237_TrendArray[i - 1];
}
App.LeftUpCS1237_TrendArray[0] = AD;
}
else{
for(int i = OSC_CNT-1; i > 0; i--)
{
App.LeftUpCS1237_TrendArray[i] = App.LeftUpCS1237_TrendArray[i - 1];
}
App.LeftUpCS1237_TrendArray[0] = AD;
}
show(1,AD);
App.LeftUpCS1237ReadEndFlag = true;
}
void LEFTLOW_CS1237AinADataProcessCallBack(int32_t AD)
{
// if(abs(AD - App.LeftLowCS1237_TrendArray[0]) > 2000000)
// {
// AD = App.LeftLowCS1237_TrendArray[0];
// }
if(App.MotorPara.CaliFlag == true)
{
AD = AD + App.MotorPara.LL_HeightStress[(App.MotorPara.Current_height)-725];
for(int i = OSC_CNT-1; i > 0; i--)
{
App.LeftLowCS1237_TrendArray[i] = App.LeftLowCS1237_TrendArray[i - 1];
}
App.LeftLowCS1237_TrendArray[0] = AD;
}
else
{
// for(int i = OSC_CNT-1; i > 0; i--)
// {
// App.LeftLowCS1237_TrendArray[i] = App.LeftLowCS1237_TrendArray[i - 1];
// }
App.LeftLowCS1237_TrendArray[0] = AD;
}
show(2,AD);
App.LeftLowCS1237ReadEndFlag = true;
}
void RIGHTUP_CS1237AinADataProcessCallBack(int32_t AD)
{
// if(abs(AD - App.RightUpCS1237_TrendArray[0]) > 2000000)
// {
// AD = App.RightUpCS1237_TrendArray[0];
// }
if(App.MotorPara.CaliFlag == true)
{
AD = AD + App.MotorPara.RU_HeightStress[(App.MotorPara.Current_height)-725];
for(int i = OSC_CNT-1; i > 0; i--)
{
App.RightUpCS1237_TrendArray[i] = App.RightUpCS1237_TrendArray[i - 1];
}
App.RightUpCS1237_TrendArray[0] = AD;
}
else
{
// for(int i = OSC_CNT-1; i > 0; i--)
// {
// App.RightUpCS1237_TrendArray[i] = App.RightUpCS1237_TrendArray[i - 1];
// }
App.RightUpCS1237_TrendArray[0] = AD;
}
show(3,AD);
App.RightUpCS1237ReadEndFlag = true;
}
void RIGHTLOW_CS1237AinADataProcessCallBack(int32_t AD)
{
// if(abs(AD - App.RightLowCS1237_TrendArray[0]) > 2000000)
// {
// AD = App.RightLowCS1237_TrendArray[0];
// }
if(App.MotorPara.CaliFlag == true)
{
AD = AD + App.MotorPara.RL_HeightStress[(App.MotorPara.Current_height)-725];
for(int i = OSC_CNT-1; i > 0; i--)
{
App.RightLowCS1237_TrendArray[i] = App.RightLowCS1237_TrendArray[i - 1];
}
App.RightLowCS1237_TrendArray[0] = AD;
}
else
{
// for(int i = OSC_CNT-1; i > 0; i--)
// {
// App.RightLowCS1237_TrendArray[i] = App.RightLowCS1237_TrendArray[i - 1];
// }
App.RightLowCS1237_TrendArray[0] = AD;
}
show(4,AD);
App.RightLowCS1237ReadEndFlag = true;
}
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.Stop_VPT = 4000;
break;}
case LEVEL_2:{
App.ADTrack.Slope_VPT = SLOPE_VPT;
App.ADTrack.LeftUpVPT = 10000;
App.ADTrack.LeftLowVPT = 10000;
App.ADTrack.RightUpVPT = 10000;
App.ADTrack.RightLowVPT = 10000;
App.ADTrack.Stop_VPT = 8000;
break;}
case LEVEL_3:{
App.ADTrack.Slope_VPT = SLOPE_VPT;
App.ADTrack.LeftUpVPT = 18000;
App.ADTrack.LeftLowVPT = 18000;
App.ADTrack.RightUpVPT = 18000;
App.ADTrack.RightLowVPT = 18000;
App.ADTrack.Stop_VPT = 15000;
break;}
case LEVEL_4:{
App.ADTrack.Slope_VPT = SLOPE_VPT;
App.ADTrack.LeftUpVPT = 25000;
App.ADTrack.LeftLowVPT = 25000;
App.ADTrack.RightUpVPT = 25000;
App.ADTrack.RightLowVPT = 25000;
App.ADTrack.Stop_VPT = 19000;
break;}
case LEVEL_5:{
App.ADTrack.Slope_VPT = SLOPE_VPT;
App.ADTrack.LeftUpVPT = 30000;
App.ADTrack.LeftLowVPT = 30000;
App.ADTrack.RightUpVPT = 30000;
App.ADTrack.RightLowVPT = 30000;
App.ADTrack.Stop_VPT = 25000;
break;}
}
}
#if(USE_AGC == 1)
void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_t *rl_ad)
{
int32_t lu_diff[OSC_CNT], Olu_diff1, Olu_diff2, Olu_diff3, Olu_diff4, Olu_diff5, Olu_diff6;
int32_t ll_diff[OSC_CNT], Oll_diff1, Oll_diff2, Oll_diff3, Oll_diff4, Oll_diff5, Oll_diff6;
int32_t ru_diff[OSC_CNT], Oru_diff1, Oru_diff2, Oru_diff3, Oru_diff4, Oru_diff5, Oru_diff6;
int32_t rl_diff[OSC_CNT], Orl_diff1, Orl_diff2, Orl_diff3, Orl_diff4 ,Orl_diff5, Orl_diff6;
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;
Olu_diff4 = lu_ad[3] - App.ADTrack.LeftUp_Org_AdZero;
Olu_diff5 = lu_ad[4] - App.ADTrack.LeftUp_Org_AdZero;
Olu_diff6 = lu_ad[5] - 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;
Oll_diff4 = ll_ad[3] - App.ADTrack.LeftLow_Org_AdZero;
Oll_diff5 = ll_ad[4] - App.ADTrack.LeftLow_Org_AdZero;
Oll_diff6 = ll_ad[5] - 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;
Oru_diff4 = ru_ad[3] - App.ADTrack.RightUp_Org_AdZero;
Oru_diff5 = ru_ad[4] - App.ADTrack.RightUp_Org_AdZero;
Oru_diff6 = ru_ad[5] - 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;
Orl_diff4 = rl_ad[3] - App.ADTrack.RightLow_Org_AdZero;
Orl_diff5 = rl_ad[4] - App.ADTrack.RightLow_Org_AdZero;
Orl_diff6 = rl_ad[5] - App.ADTrack.RightLow_Org_AdZero;
for(int i = 0; i < OSC_CNT; i++)
{
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_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 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(1, lu_slope);
// show(2, ll_slope);
// show(3, ru_slope);
// show(4, rl_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] = lu_slope;
App.LeftLowCS1237_SlopeArray[0] = ll_slope;
App.RightUpCS1237_SlopeArray[0] = ru_slope;
App.RightLowCS1237_SlopeArray[0] = rl_slope;
// 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);
int32_t LeftSlopeSum = abs(App.LeftUpCS1237_SlopeArray[0]) + abs(App.LeftLowCS1237_SlopeArray[0]) + abs(App.LeftUpCS1237_SlopeArray[1]) + abs(App.LeftLowCS1237_SlopeArray[1]) + abs(App.LeftUpCS1237_SlopeArray[2]) + abs(App.LeftLowCS1237_SlopeArray[2]);
int32_t RightSlopeSum = abs(App.RightUpCS1237_SlopeArray[0]) + abs(App.RightLowCS1237_SlopeArray[0]) + abs(App.RightUpCS1237_SlopeArray[1]) + abs(App.RightLowCS1237_SlopeArray[1]) + abs(App.RightUpCS1237_SlopeArray[2]) + abs(App.RightLowCS1237_SlopeArray[2]);
int32_t LeftDiffSum = abs(lu_diff[0]) + abs(lu_diff[1]) + abs(lu_diff[2]) + abs(ll_diff[0]) + abs(ll_diff[1]) + abs(ll_diff[2]);
int32_t RightDiffSum = abs(ru_diff[0]) + abs(ru_diff[1]) + abs(ru_diff[2]) + abs(rl_diff[0]) + abs(rl_diff[1]) + abs(rl_diff[2]);
int32_t LuSideDiff = lu_ad[0] - lu_ad[5];
int32_t LlSideDiff = ll_ad[0] - ll_ad[5];
int32_t RuSideDiff = ru_ad[0] - ru_ad[5];
int32_t RlSideDiff = rl_ad[0] - rl_ad[5];
if(App.EmeFlag == true)
{
if(App.DownFlag == true && App.EmergencyDelay1mSCnt > 0)
{
BDC_UP();
}
else if(App.UpFlag == true && App.EmergencyDelay1mSCnt > 0)
{
BDC_DOWN();
}
else if(App.EmergencyDelay1mSCnt <= 0)
{
BDC_STOP();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.SlopeCollDelay1mSCnt = SLOPE_START_MS;
}
}
else if(App.StopFlag == true && App.StopCollDelay1mSCnt <= 0)
{
App.LeftRunFalg = false;
App.RightRunFalg = false;
App.CentreRunFalg = false;
if(((lu_ad[0] < App.ADTrack.LeftUp_Org_AdZero-App.ddd)//判断中间操作
&& (ru_ad[0] < App.ADTrack.RightUp_Org_AdZero-App.ddd)
&& (lu_ad[1] < App.ADTrack.LeftUp_Org_AdZero-App.ddd)
&& (ru_ad[1] < App.ADTrack.RightUp_Org_AdZero-App.ddd)
&& (lu_ad[2] < App.ADTrack.LeftUp_Org_AdZero-App.ddd)
&& (ru_ad[2] < App.ADTrack.RightUp_Org_AdZero-App.ddd)
&& (ll_ad[0] > App.ADTrack.LeftLow_Org_AdZero+App.ddd)//判断中间操作
&& (rl_ad[0] > App.ADTrack.RightLow_Org_AdZero+App.ddd)
&& (ll_ad[1] > App.ADTrack.LeftLow_Org_AdZero+App.ddd)
&& (rl_ad[1] > App.ADTrack.RightLow_Org_AdZero+App.ddd)
&& (ll_ad[2] > App.ADTrack.LeftLow_Org_AdZero+App.ddd)
&& (rl_ad[2] > App.ADTrack.RightLow_Org_AdZero+App.ddd))
|| ((lu_ad[0] > App.ADTrack.LeftUp_Org_AdZero+App.ddd)//判断中间操作
&& (ru_ad[0] > App.ADTrack.RightUp_Org_AdZero+App.ddd)
&& (lu_ad[1] > App.ADTrack.LeftUp_Org_AdZero+App.ddd)
&& (ru_ad[1] > App.ADTrack.RightUp_Org_AdZero+App.ddd)
&& (lu_ad[2] > App.ADTrack.LeftUp_Org_AdZero+App.ddd)
&& (ru_ad[2] > App.ADTrack.RightUp_Org_AdZero+App.ddd)
&& (ll_ad[0] < App.ADTrack.LeftLow_Org_AdZero-App.ddd)//判断中间操作
&& (rl_ad[0] < App.ADTrack.RightLow_Org_AdZero-App.ddd)
&& (ll_ad[1] < App.ADTrack.LeftLow_Org_AdZero-App.ddd)
&& (rl_ad[1] < App.ADTrack.RightLow_Org_AdZero-App.ddd)
&& (ll_ad[2] < App.ADTrack.LeftLow_Org_AdZero-App.ddd)
&& (rl_ad[2] < App.ADTrack.RightLow_Org_AdZero-App.ddd))
)
{
App.CentreRunFalg = true;
App.CentCollDelay1mSCnt = CENT_TIME_MS;
if(abs(Oll_diff1) + 100 > abs(Orl_diff1)
&& abs(Oll_diff2) + 100 > abs(Orl_diff2)
&& abs(Oll_diff3) + 100 > abs(Orl_diff3)
&& abs(Olu_diff1) + 100 > abs(Oru_diff1)
&& abs(Olu_diff2) + 100 > abs(Oru_diff2)
&& abs(Olu_diff3) + 100 > abs(Oru_diff3)
)//判断偏左还是偏右
{
App.LeftRunFalg = true;
if(((Olu_diff1+Oru_diff1) > (App.ADTrack.Stop_VPT))//判断上拉
&& ((Olu_diff2+Oru_diff2) > (App.ADTrack.Stop_VPT))
&& ((Olu_diff3+Oru_diff3) > (App.ADTrack.Stop_VPT))
&& ((Oll_diff1+Orl_diff1) < -(App.ADTrack.Stop_VPT))
&& ((Oll_diff2+Orl_diff2) < -(App.ADTrack.Stop_VPT))
&& ((Oll_diff3+Orl_diff3) < -(App.ADTrack.Stop_VPT))
)
{
if((App.RightUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
&& (App.RightUpCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[0] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
&& (App.LeftUpCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] >= PRESS_VPT))
{
if(App.MotorPara.Current_height < 1195)
{
BDC_UP();
App.CaliZeroFlag = false;
App.UpFlag = true;
App.StopFlag = false;
App.DownFlag = false;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
}
}
}
else if(((Olu_diff1+Oru_diff1) < -(App.ADTrack.Stop_VPT))//判断下拉
&& ((Olu_diff2+Oru_diff2) < -(App.ADTrack.Stop_VPT))
&& ((Olu_diff3+Oru_diff3) < -(App.ADTrack.Stop_VPT))
&& ((Oll_diff1+Orl_diff1) > (App.ADTrack.Stop_VPT))
&& ((Oll_diff2+Orl_diff2) > (App.ADTrack.Stop_VPT))
&& ((Oll_diff3+Orl_diff3) > (App.ADTrack.Stop_VPT))
)
{
if((App.RightUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
&& (App.RightUpCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[0] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
&& (App.LeftUpCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] <= -PRESS_VPT))
{
if(App.MotorPara.Current_height > 725)
{
BDC_DOWN();
App.CaliZeroFlag = false;
App.UpFlag = false;
App.StopFlag = false;
App.DownFlag = true;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
}
}
}
}
else if(abs(Oll_diff1) + 100 < abs(Orl_diff1)//判断偏左
&& abs(Oll_diff2) < abs(Orl_diff2) + 100
&& abs(Oll_diff3) < abs(Orl_diff3) + 100
&& abs(Olu_diff1) < abs(Oru_diff1) + 100
&& abs(Olu_diff2) < abs(Oru_diff2) + 100
&& abs(Olu_diff3) < abs(Oru_diff3) + 100)
{
App.RightRunFalg = true;
if(((Olu_diff1+Oru_diff1) > (App.ADTrack.Stop_VPT))//判断上拉
&& ((Olu_diff2+Oru_diff2) > (App.ADTrack.Stop_VPT))
&& ((Olu_diff3+Oru_diff3) > (App.ADTrack.Stop_VPT))
&& ((Oll_diff1+Orl_diff1) < -(App.ADTrack.Stop_VPT))
&& ((Oll_diff2+Orl_diff2) < -(App.ADTrack.Stop_VPT))
&& ((Oll_diff3+Orl_diff3) < -(App.ADTrack.Stop_VPT))
)
{
if((App.RightUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
&& (App.RightUpCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[0] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
&& (App.LeftUpCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] >= PRESS_VPT))
{
if(App.MotorPara.Current_height < 1195)
{
BDC_UP();
App.CaliZeroFlag = false;
App.UpFlag = true;
App.StopFlag = false;
App.DownFlag = false;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
}
}
}
else if(((Olu_diff1+Oru_diff1) < -(App.ADTrack.Stop_VPT))//判断下拉
&& ((Olu_diff2+Oru_diff2) < -(App.ADTrack.Stop_VPT))
&& ((Olu_diff3+Oru_diff3) < -(App.ADTrack.Stop_VPT))
&& ((Oll_diff1+Orl_diff1) > (App.ADTrack.Stop_VPT))
&& ((Oll_diff2+Orl_diff2) > (App.ADTrack.Stop_VPT))
&& ((Oll_diff3+Orl_diff3) > (App.ADTrack.Stop_VPT))
)
{
if((App.RightUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
&& (App.RightUpCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[0] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
&& (App.LeftUpCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] <= -PRESS_VPT))
{
if(App.MotorPara.Current_height > 725)
{
BDC_DOWN();
App.CaliZeroFlag = false;
App.UpFlag = false;
App.StopFlag = false;
App.DownFlag = true;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
}
}
}
}
}
else if((LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum && App.CentCollDelay1mSCnt <= 0))//判断左边操作
{
App.LeftRunFalg = true;
if((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(((App.LeftUpCS1237_SlopeArray[0] <= -PRESS_VPT)//判断是否不是正面压
&& (App.LeftUpCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
&& (App.RightUpCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[0] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] <= -PRESS_VPT))
|| ((App.LeftUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
&& (App.LeftUpCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
&& (App.RightUpCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[0] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] <= -PRESS_VPT))
)
{
// if(App.CaliZeroFlag == true)//判断是否校零
{
// if((abs(Olu_diff1) > abs(Oru_diff1) * SHIELD_VPT)//判断是否不是侧面推拉
// && (abs(Olu_diff2) > abs(Oru_diff2) * SHIELD_VPT)
// && (abs(Olu_diff3) > abs(Oru_diff3) * SHIELD_VPT)
// && (abs(Oll_diff1) > abs(Orl_diff1) * SHIELD_VPT)
// && (abs(Oll_diff3) > abs(Orl_diff2) * SHIELD_VPT)
// && (abs(Oll_diff3) > abs(Orl_diff3) * SHIELD_VPT)
// )
{
if(App.MotorPara.Current_height < 1195)
{
BDC_UP();
App.CaliZeroFlag = false;
App.UpFlag = true;
App.DownFlag = false;
App.StopFlag = false;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
}
}
}
}
}
else if(((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(((App.LeftUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
&& (App.LeftUpCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
&& (App.RightUpCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[0] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] >= PRESS_VPT))
|| ((App.LeftUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
&& (App.LeftUpCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
&& (App.RightUpCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[0] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] >= PRESS_VPT))
)
{
// if(App.CaliZeroFlag == true)//判断是否校零
{
// if((abs(Olu_diff1) > abs(Oru_diff1) * SHIELD_VPT)//判断是否不是侧面推拉
// && (abs(Olu_diff2) > abs(Oru_diff2) * SHIELD_VPT)
// && (abs(Olu_diff3) > abs(Oru_diff3) * SHIELD_VPT)
// && (abs(Oll_diff1) > abs(Orl_diff1) * SHIELD_VPT)
// && (abs(Oll_diff3) > abs(Orl_diff2) * SHIELD_VPT)
// && (abs(Oll_diff3) > abs(Orl_diff3) * SHIELD_VPT)
// )
{
if(App.MotorPara.Current_height > 725)
{
BDC_DOWN();
App.CaliZeroFlag = false;
App.UpFlag = false;
App.DownFlag = true;
App.StopFlag = false;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
}
}
}
}
}
}
else if(LeftSlopeSum < RightSlopeSum && LeftDiffSum < RightDiffSum && App.CentCollDelay1mSCnt <= 0)//判断右边操作
{
App.RightRunFalg = true;
if((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(((App.RightUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
&& (App.RightUpCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[0] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
&& (App.LeftUpCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] <= -PRESS_VPT))
|| ((App.RightUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
&& (App.RightUpCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[0] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
&& (App.LeftUpCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] >= PRESS_VPT))
)
{
// if(App.CaliZeroFlag == true)//判断是否校零
{
// if((abs(Oru_diff1) > abs(Olu_diff1) * SHIELD_VPT)//判断是否不是侧面推拉
// && (abs(Oru_diff2) > abs(Olu_diff2) * SHIELD_VPT)
// && (abs(Oru_diff3) > abs(Olu_diff3) * SHIELD_VPT)
// && (abs(Orl_diff1) > abs(Oll_diff1) * SHIELD_VPT)
// && (abs(Orl_diff3) > abs(Oll_diff2) * SHIELD_VPT)
// && (abs(Orl_diff3) > abs(Oll_diff3) * SHIELD_VPT)
// )
{
if(App.MotorPara.Current_height < 1195)
{
BDC_UP();
App.CaliZeroFlag = false;
App.UpFlag = true;
App.StopFlag = false;
App.DownFlag = false;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
}
}
}
}
}
else if((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(((App.RightUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
&& (App.RightUpCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[0] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
&& (App.LeftUpCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] >= PRESS_VPT))
|| ((App.RightUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
&& (App.RightUpCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[0] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
&& (App.LeftUpCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] <= -PRESS_VPT))
)
{
// if(App.CaliZeroFlag == true)//判断是否校零
{
// if((abs(Oru_diff1) > abs(Olu_diff1) * SHIELD_VPT)//判断是否不是侧面推拉
// && (abs(Oru_diff2) > abs(Olu_diff2) * SHIELD_VPT)
// && (abs(Oru_diff3) > abs(Olu_diff3) * SHIELD_VPT)
// && (abs(Orl_diff1) > abs(Oll_diff1) * SHIELD_VPT)
// && (abs(Orl_diff3) > abs(Oll_diff2) * SHIELD_VPT)
// && (abs(Orl_diff3) > abs(Oll_diff3) * SHIELD_VPT)
// )
{
if(App.MotorPara.Current_height > 725)
{
BDC_DOWN();
App.CaliZeroFlag = false;
App.DownFlag = true;
App.StopFlag = false;
App.UpFlag = false;
App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
}
}
}
}
}
}
}
else if(App.DownFlag == true && App.RunCollDelay1mSCnt <= 0)
{
if(App.LeftRunFalg == true)
{
if(((App.CentreRunFalg == false)
&& (Olu_diff1 > -App.ADTrack.Stop_VPT)
&& (Oll_diff1 < App.ADTrack.Stop_VPT)
&& (Olu_diff2 > -App.ADTrack.Stop_VPT)
&& (Oll_diff2 < App.ADTrack.Stop_VPT)
&& (Olu_diff3 > -App.ADTrack.Stop_VPT)
&& (Oll_diff3 < App.ADTrack.Stop_VPT)
&& (Olu_diff4 > -App.ADTrack.Stop_VPT)
&& (Oll_diff4 < App.ADTrack.Stop_VPT)
&& (Olu_diff5 > -App.ADTrack.Stop_VPT)
&& (Oll_diff5 < App.ADTrack.Stop_VPT)
&& (Olu_diff6 > -App.ADTrack.Stop_VPT)
&& (Oll_diff6 < App.ADTrack.Stop_VPT))
|| ((App.CentreRunFalg == true)
&& ((Olu_diff1 + Oru_diff1) > -(App.ADTrack.Stop_VPT*2/3))
&& ((Olu_diff2 + Oru_diff2) > -(App.ADTrack.Stop_VPT*2/3))
&& ((Olu_diff3 + Oru_diff3) > -(App.ADTrack.Stop_VPT*2/3)))
|| App.MotorPara.Current_height >= 1195
|| App.MotorPara.Current_height <= 725
)
{
BDC_STOP();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.SlopeCollDelay1mSCnt = SLOPE_START_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)
// && (abs(Olu_diff1) + EME_STOP_VPT) < abs(Oru_diff1)
// && (abs(Olu_diff2) + EME_STOP_VPT) < abs(Oru_diff2)
// && (abs(Olu_diff3) + EME_STOP_VPT) < abs(Oru_diff3)
// && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
// {
// BDC_STOP();
// App.EmeFlag = true;
// App.EmergencyDelay1mSCnt = EME_STOP_MS;
// }
}
else if(App.RightRunFalg == true)
{
if(((App.CentreRunFalg == false )
&& (Oru_diff1 > -App.ADTrack.Stop_VPT)
&& (Orl_diff1 < App.ADTrack.Stop_VPT)
&& (Oru_diff2 > -App.ADTrack.Stop_VPT)
&& (Orl_diff2 < App.ADTrack.Stop_VPT)
&& (Oru_diff3 > -App.ADTrack.Stop_VPT)
&& (Orl_diff3 < App.ADTrack.Stop_VPT)
&& (Oru_diff4 > -App.ADTrack.Stop_VPT)
&& (Orl_diff4 < App.ADTrack.Stop_VPT)
&& (Oru_diff5 > -App.ADTrack.Stop_VPT)
&& (Orl_diff5 < App.ADTrack.Stop_VPT)
&& (Oru_diff6 > -App.ADTrack.Stop_VPT)
&& (Orl_diff6 < App.ADTrack.Stop_VPT))
|| ((App.CentreRunFalg == true)
&& ((Olu_diff1 + Oru_diff1) > -(App.ADTrack.Stop_VPT*2/3))
&& ((Olu_diff2 + Oru_diff2) > -(App.ADTrack.Stop_VPT*2/3))
&& ((Olu_diff3 + Oru_diff3) > -(App.ADTrack.Stop_VPT*2/3)))
|| App.MotorPara.Current_height >= 1195
|| App.MotorPara.Current_height <= 725
)
{
BDC_STOP();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.SlopeCollDelay1mSCnt = SLOPE_START_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)
// && abs(Olu_diff1) > (abs(Oru_diff1) + EME_STOP_VPT)
// && abs(Olu_diff2) > (abs(Oru_diff2) + EME_STOP_VPT)
// && abs(Olu_diff3) > (abs(Oru_diff3) + EME_STOP_VPT)
// && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
// {
// BDC_STOP();
// App.EmeFlag = true;
// App.EmergencyDelay1mSCnt = EME_STOP_MS;
// }
}
}
else if(App.UpFlag == true && App.RunCollDelay1mSCnt <= 0)
{
if(App.LeftRunFalg == true)
{
if(((App.CentreRunFalg == false)
&& (Olu_diff1 < App.ADTrack.Stop_VPT)
&& (Oll_diff1 > -App.ADTrack.Stop_VPT)
&& (Olu_diff2 < App.ADTrack.Stop_VPT)
&& (Oll_diff2 > -App.ADTrack.Stop_VPT)
&& (Olu_diff3 < App.ADTrack.Stop_VPT)
&& (Oll_diff3 > -App.ADTrack.Stop_VPT)
&& (Olu_diff4 < App.ADTrack.Stop_VPT)
&& (Oll_diff4 > -App.ADTrack.Stop_VPT)
&& (Olu_diff5 < App.ADTrack.Stop_VPT)
&& (Oll_diff5 > -App.ADTrack.Stop_VPT)
&& (Olu_diff6 < App.ADTrack.Stop_VPT)
&& (Oll_diff6 > -App.ADTrack.Stop_VPT))
|| ((App.CentreRunFalg == true)
&& ((Olu_diff1 + Oru_diff1) < (App.ADTrack.Stop_VPT*2/3))
&& ((Olu_diff2 + Oru_diff2) < (App.ADTrack.Stop_VPT*2/3))
&& ((Olu_diff3 + Oru_diff3) < (App.ADTrack.Stop_VPT*2/3)))
|| App.MotorPara.Current_height >= 1195
|| App.MotorPara.Current_height <= 725
)
{
BDC_STOP();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.SlopeCollDelay1mSCnt = SLOPE_START_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)
// && (abs(Olu_diff1) + EME_STOP_VPT) < abs(Oru_diff1)
// && (abs(Olu_diff2) + EME_STOP_VPT) < abs(Oru_diff2)
// && (abs(Olu_diff3) + EME_STOP_VPT) < abs(Oru_diff3)
// && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
// {
// BDC_STOP();
// App.EmeFlag = true;
// App.EmergencyDelay1mSCnt = EME_STOP_MS;
// }
}
else if(App.RightRunFalg == true)
{
if(((App.CentreRunFalg == false )
&& (Oru_diff1 < App.ADTrack.Stop_VPT)
&& (Orl_diff1 > -App.ADTrack.Stop_VPT)
&& (Oru_diff2 < App.ADTrack.Stop_VPT)
&& (Orl_diff2 > -App.ADTrack.Stop_VPT)
&& (Oru_diff3 < App.ADTrack.Stop_VPT)
&& (Orl_diff3 > -App.ADTrack.Stop_VPT)
&& (Oru_diff4 < App.ADTrack.Stop_VPT)
&& (Orl_diff4 > -App.ADTrack.Stop_VPT)
&& (Oru_diff5 < App.ADTrack.Stop_VPT)
&& (Orl_diff5 > -App.ADTrack.Stop_VPT)
&& (Oru_diff6 < App.ADTrack.Stop_VPT)
&& (Orl_diff6 > -App.ADTrack.Stop_VPT))
|| ((App.CentreRunFalg == true)
&& ((Olu_diff1 + Oru_diff1) < (App.ADTrack.Stop_VPT*2/3))
&& ((Olu_diff2 + Oru_diff2) < (App.ADTrack.Stop_VPT*2/3))
&& ((Olu_diff3 + Oru_diff3) < (App.ADTrack.Stop_VPT*2/3)))
|| App.MotorPara.Current_height >= 1195
|| App.MotorPara.Current_height <= 725
)
{
BDC_STOP();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
App.SlopeCollDelay1mSCnt = SLOPE_START_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)
// && abs(Olu_diff1) > (abs(Oru_diff1) + EME_STOP_VPT)
// && abs(Olu_diff2) > (abs(Oru_diff2) + EME_STOP_VPT)
// && abs(Olu_diff3) > (abs(Oru_diff3) + EME_STOP_VPT)
// && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
// {
// BDC_STOP();
// App.EmeFlag = true;
// App.EmergencyDelay1mSCnt = EME_STOP_MS;
// }
}
}
if(
// lu_K.peaks_positive
// && lu_K.valleys_negative
App.LeftUpCS1237_SlopeArray[0] < FOLLOW_VPT
&& App.LeftUpCS1237_SlopeArray[0] > -FOLLOW_VPT
&& App.LeftUpCS1237_SlopeArray[1] < FOLLOW_VPT
&& App.LeftUpCS1237_SlopeArray[1] > -FOLLOW_VPT
&& App.LeftUpCS1237_SlopeArray[2] < FOLLOW_VPT
&& App.LeftUpCS1237_SlopeArray[2] > -FOLLOW_VPT
// && ll_K.peaks_positive
// && ll_K.valleys_negative
&& App.LeftLowCS1237_SlopeArray[0] < FOLLOW_VPT
&& App.LeftLowCS1237_SlopeArray[0] > -FOLLOW_VPT
&& App.LeftLowCS1237_SlopeArray[1] < FOLLOW_VPT
&& App.LeftLowCS1237_SlopeArray[1] > -FOLLOW_VPT
&& App.LeftLowCS1237_SlopeArray[2] < FOLLOW_VPT
&& App.LeftLowCS1237_SlopeArray[2] > -FOLLOW_VPT
// && ru_K.peaks_positive
// && ru_K.valleys_negative
&& App.RightUpCS1237_SlopeArray[0] < FOLLOW_VPT
&& App.RightUpCS1237_SlopeArray[0] > -FOLLOW_VPT
&& App.RightUpCS1237_SlopeArray[1] < FOLLOW_VPT
&& App.RightUpCS1237_SlopeArray[1] > -FOLLOW_VPT
&& App.RightUpCS1237_SlopeArray[2] < FOLLOW_VPT
&& App.RightUpCS1237_SlopeArray[2] > -FOLLOW_VPT
// && rl_K.peaks_positive
// && rl_K.valleys_negative
&& App.RightLowCS1237_SlopeArray[0] < FOLLOW_VPT
&& App.RightLowCS1237_SlopeArray[0] > -FOLLOW_VPT
&& App.RightLowCS1237_SlopeArray[1] < FOLLOW_VPT
&& App.RightLowCS1237_SlopeArray[1] > -FOLLOW_VPT
&& App.RightLowCS1237_SlopeArray[2] < FOLLOW_VPT
&& App.RightLowCS1237_SlopeArray[2] > -FOLLOW_VPT)
{
if(App.StopFlag == true
&& App.StopCollDelay1mSCnt <= 0
&& App.MotorPara.Current_height <= 1195
&& App.MotorPara.Current_height >= 725){
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];
App.CaliZeroFlag = true;
}
}
}
#endif
static void AppInit(void)
{
memset(&App, 0x00, sizeof(App));
READ_APP_PARA((uint32_t *)&App.Para);
if(App.Para.FactoryFlag != 0x00){
App.Para.FactoryFlag = 0x00;
/*参数初始化*/
App.Para.Layout.ControlLevel = LEVEL_3;//出厂默认等级3
App.Para.Layout.DBGWaveOut = false;
SAVE_APP_PARA((uint32_t *)&App.Para);
}
if(App.Para.Layout.ControlLevel > 5 || App.Para.Layout.ControlLevel < 1){
App.Para.Layout.ControlLevel = LEVEL_3;//出厂默认等级3
SAVE_APP_PARA((uint32_t *)&App.Para);
}
DBG_LOG("\r\n\r\n");
DBG_LOG("****************************************************\r\n");
DBG_LOG("** **\r\n");
DBG_LOG("** Acc_Support_29V **\r\n");
DBG_LOG("** SoftWare V:%d.%d **\r\n", SOFTWARE_VERSION / 10, SOFTWARE_VERSION % 10);
DBG_LOG("** HardWare V:%d.%d **\r\n", HARDWARE_VERSION / 10, HARDWARE_VERSION % 10);
DBG_LOG("** Compile: %s %s **\r\n", __DATE__, __TIME__);
DBG_LOG("** **\r\n");
DBG_LOG("****************************************************\r\n\r\n");
//FeedDog();
LEFTUP_CS1237_Init();
LEFTLOW_CS1237_Init();
RIGHTUP_CS1237_Init();
RIGHTLOW_CS1237_Init();
BDC_STOP();
LED_RED_OFF();
LED_GREEN_ON();
App.Status = APP_STATUS_MOTOR_INIT;
App.MotorPara.InitialFlag = false;
App.RStatus = READ_STATUS_START;
GradeControl(App.Para.Layout.ControlLevel);//等级控制
App.WaitFlag = false;
}
static void ReadLoopHandler(void)
{
switch(App.RStatus){
case READ_STATUS_IDLE:{
break;}
case READ_STATUS_START:{
App.LeftUpCS1237ReadEndFlag = false;
App.LeftLowCS1237ReadEndFlag = false;
App.RightUpCS1237ReadEndFlag = false;
App.RightLowCS1237ReadEndFlag = false;
App.RStatus = READ_STATUS_LEFTUP;
break;}
case READ_STATUS_LEFTUP:{
LEFTUP_CS1237Start();
App.Read1mSDelayCnt = 50;
App.RStatus = READ_STATUS_WAIT_LEFTUP;
break;}
case READ_STATUS_WAIT_LEFTUP:{
if(App.Read1mSDelayCnt <= 0)
{
App.RStatus = READ_STATUS_LEFTLOW;
}
else if(App.LeftUpCS1237ReadEndFlag == true)
{
App.RStatus = READ_STATUS_LEFTLOW;
}
break;}
case READ_STATUS_LEFTLOW:{
LEFTLOW_CS1237Start();
App.Read1mSDelayCnt = 50;
App.RStatus = READ_STATUS_WAIT_LEFTLOW;
break;}
case READ_STATUS_WAIT_LEFTLOW:{
if(App.Read1mSDelayCnt <= 0)
{
App.RStatus = READ_STATUS_RIGHTUP;
}
else if(App.LeftLowCS1237ReadEndFlag == true)
{
App.RStatus = READ_STATUS_RIGHTUP;
}
break;}
case READ_STATUS_RIGHTUP:{
RIGHTUP_CS1237Start();
App.Read1mSDelayCnt = 50;
App.RStatus = READ_STATUS_WAIT_RIGHTUP;
break;}
case READ_STATUS_WAIT_RIGHTUP:{
if(App.Read1mSDelayCnt <= 0)
{
App.RStatus = READ_STATUS_RIGHTLOW;
}
else if(App.RightUpCS1237ReadEndFlag == true)
{
App.RStatus = READ_STATUS_RIGHTLOW;
}
break;}
case READ_STATUS_RIGHTLOW:{
RIGHTLOW_CS1237Start();
App.Read1mSDelayCnt = 50;
App.RStatus = READ_STATUS_WAIT_RIGHTLOW;
break;}
case READ_STATUS_WAIT_RIGHTLOW:{
if(App.Read1mSDelayCnt <= 0)
{
App.RStatus = READ_STATUS_SUPPORT_CONT;
}
else if(App.RightLowCS1237ReadEndFlag == true)
{
App.RStatus = READ_STATUS_SUPPORT_CONT;
}
break;}
case READ_STATUS_SUPPORT_CONT:{
if(App.LeftUpCS1237ReadEndFlag == true &&
App.LeftLowCS1237ReadEndFlag == true &&
App.RightUpCS1237ReadEndFlag == true &&
App.RightLowCS1237ReadEndFlag == true &&
App.WaitFlag == true)
{
#if(USE_AGC == 1)
SupportControlHandle(App.LeftUpCS1237_TrendArray, App.LeftLowCS1237_TrendArray, App.RightUpCS1237_TrendArray, App.RightLowCS1237_TrendArray);
#endif
}
App.RStatus = READ_STATUS_START;
break;}
}
}
static void AppLoopHandler(void)
{
switch(App.Status) {
case APP_STATUS_IDLE:{
break;}
case APP_STATUS_ON:{
App.Status = APP_STATUS_IDLE;
break;}
case APP_STATUS_OFF:{
App.Status = APP_STATUS_IDLE;
break;}
case APP_STATUS_MOTOR_INIT:{
Beeper_Start(200,200,2);
LED_RED_ON();
LED_GREEN_OFF();
BDC_UP();
Ddl_Delay1ms(1000);
BDC_DOWN();
App.DownFlag = true;
App.StopFlag = false;
App.Status = APP_STATUS_WAIT_MOTOR_INIT;
App.TD1mSDelayCnt = TRAVEL_TIME_MS;
break;}
case APP_STATUS_WAIT_MOTOR_INIT:{
if(App.TD1mSDelayCnt <= 0)
{
BDC_STOP();
LED_RED_OFF();
LED_GREEN_ON();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.Status = APP_STATUS_IDLE;
}
if(App.MotorPara.Current_height <= 722 && App.MotorPara.Current_height >= 720)
{
BDC_STOP();
LED_RED_OFF();
LED_GREEN_ON();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.MotorPara.CaliFlag = false;
App.TD1mSDelayCnt = 1000;
App.Status = APP_STATUS_ZERO_MARKING;
}
break;}
case APP_STATUS_ZERO_MARKING:{
if(App.TD1mSDelayCnt <= 0)
{
App.ADTrack.LeftUp_Org_AdZero = App.LeftUpCS1237_TrendArray[0];
App.ADTrack.LeftLow_Org_AdZero = App.LeftLowCS1237_TrendArray[0];
App.ADTrack.RightUp_Org_AdZero = App.RightUpCS1237_TrendArray[0];
App.ADTrack.RightLow_Org_AdZero = App.RightLowCS1237_TrendArray[0];
DBG_LOG("Frist Pack Data LeftUp Zero AD: %08d\tLeftUp Zero AD: %08d\tRightUp Zero AD: %08d\tRightLow Zero AD: %08d\r\n",
App.ADTrack.LeftUp_Org_AdZero,
App.ADTrack.LeftLow_Org_AdZero,
App.ADTrack.RightUp_Org_AdZero,
App.ADTrack.RightLow_Org_AdZero);
App.Status = APP_STATUS_WAIT_MOTOR_CALI_UP;
App.MotorPara.InitialFlag = true;
App.MotorPara.CaliNum = 0;
App.TD1mSDelayCnt = TRAVEL_TIME_MS;
App.StopFlag = false;
App.UpFlag = true;
BDC_UP();
LED_RED_ON();
LED_GREEN_OFF();
App.CailUp1mSCnt = 0;
App.MotorPara.LoopOne = false;
}
break;}
case APP_STATUS_WAIT_MOTOR_CALI_UP:{
if(App.MotorPara.Current_height >= 1200 && App.CailUp1mSCnt >= 17000)
{
App.CailUp1mSCnt = 0;
BDC_STOP();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
Ddl_Delay1ms(1000);
App.DownFlag = true;
App.StopFlag = false;
App.Status = APP_STATUS_WAIT_MOTOR_CALI_DOWN;
BDC_DOWN();
}
else if(App.MotorPara.CaliNum >= 480 && App.MotorPara.LoopOne == true)
{
BDC_STOP();
LED_RED_OFF();
LED_GREEN_ON();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.MotorPara.LoopOne = false;
App.MotorPara.InitialFlag = false;
App.MotorPara.CaliFlag = true;
DBG_LOG("===================LU Data monitoring====================\r\n");
weightedMovingAverageWithEnhance(App.MotorPara.LU_HeightStress,480,7,380,480,4,0);
DBG_LOG("===================LL Data monitoring====================\r\n");
weightedMovingAverageWithEnhance(App.MotorPara.LL_HeightStress,480,7,380,480,4,0);
DBG_LOG("===================RU Data monitoring====================\r\n");
weightedMovingAverageWithEnhance(App.MotorPara.RU_HeightStress,480,7,380,480,4,0);
DBG_LOG("===================RL Data monitoring====================\r\n");
weightedMovingAverageWithEnhance(App.MotorPara.RL_HeightStress,480,7,380,480,4,0);
DBG_LOG("Motor Cali Ok\r\n");
App.TD1mSDelayCnt = 1000;
App.Status = APP_STATUS_WAIT_STEADY;
}
break;}
case APP_STATUS_WAIT_MOTOR_CALI_DOWN:{
if(App.MotorPara.Current_height <= 725)
{
BDC_STOP();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.MotorPara.LoopOne = true;
Ddl_Delay1ms(1000);
App.UpFlag = true;
App.StopFlag = false;
App.Status = APP_STATUS_WAIT_MOTOR_CALI_UP;
BDC_UP();
App.CailUp1mSCnt = 0;
}
else if(App.MotorPara.CaliNum >= 480 && App.MotorPara.LoopOne == true)
{
BDC_STOP();
LED_RED_OFF();
LED_GREEN_ON();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.MotorPara.LoopOne = false;
App.MotorPara.InitialFlag = false;
App.MotorPara.CaliFlag = true;
DBG_LOG("===================LU Data monitoring====================\r\n");
weightedMovingAverageWithEnhance(App.MotorPara.LU_HeightStress,480,7,380,480,4,0);
DBG_LOG("===================LL Data monitoring====================\r\n");
weightedMovingAverageWithEnhance(App.MotorPara.LL_HeightStress,480,7,380,480,4,0);
DBG_LOG("===================RU Data monitoring====================\r\n");
weightedMovingAverageWithEnhance(App.MotorPara.RU_HeightStress,480,7,380,480,4,0);
DBG_LOG("===================RL Data monitoring====================\r\n");
weightedMovingAverageWithEnhance(App.MotorPara.RL_HeightStress,480,7,380,480,4,0);
DBG_LOG("Motor Cali Ok\r\n");
App.TD1mSDelayCnt = 1000;
App.Status = APP_STATUS_WAIT_STEADY;
}
break;}
case APP_STATUS_WAIT_STEADY:{
if(App.TD1mSDelayCnt <= 0)
{
Beeper_Start(300,0,1);
App.SideFlag = false;
App.CaliZeroFlag = false;
App.UpFlag = false;
App.DownFlag = false;
App.EmeFlag = false;
App.StopFlag = true;
App.WaitFlag = true;
App.Status = APP_STATUS_HEIGHT_CONTROL;
}
break;}
case APP_STATUS_HEIGHT_CONTROL:{
if(App.MotorPara.Current_height <= 725
&& App.MotorPara.RestFlag == false
&& App.HeightContRolCnt >= 500
&& GET_BIT0_DOUT() == 1
&& GET_BIT1_DOUT() == 1)
{
App.HeightContRolCnt = 0;
BDC_UP();
Ddl_Delay1ms(500);
BDC_STOP();
}
else if(App.MotorPara.Current_height >= 1195
&& App.MotorPara.RestFlag == false
&& App.HeightContRolCnt >= 500
&& GET_BIT0_DOUT() == 1
&& GET_BIT1_DOUT() == 1)
{
App.HeightContRolCnt = 0;
BDC_DOWN();
Ddl_Delay1ms(500);
BDC_STOP();
}
break;}
}
}
static void App1mSRoutine(void)
{
if(App.TD1mSDelayCnt > 0)
App.TD1mSDelayCnt--;
if(App.Read1mSDelayCnt > 0)
App.Read1mSDelayCnt--;
if(App.Uart1Delay1mSCnt > 0)
App.Uart1Delay1mSCnt--;
if(App.Uart3RxTimeOut1mSCnt > 0)
App.Uart3RxTimeOut1mSCnt--;
if(App.StopCollDelay1mSCnt > 0)
App.StopCollDelay1mSCnt--;
if(App.RunCollDelay1mSCnt > 0)
App.RunCollDelay1mSCnt--;
if(App.EmergencyDelay1mSCnt > 0)
App.EmergencyDelay1mSCnt--;
if(App.SlopeCollDelay1mSCnt > 0)
App.SlopeCollDelay1mSCnt--;
if(App.CentCollDelay1mSCnt > 0)
App.CentCollDelay1mSCnt--;
if(App.MotorPara.InitialFlag == true)
App.CailUp1mSCnt++;
if((App.MotorPara.Current_height < 725 || App.MotorPara.Current_height > 1195))
{
App.HeightContRolCnt++;
}
else
{
App.HeightContRolCnt = 0;
}
App.FeedDogDlyCnt++;
}
void FeedDogHandler(void)
{
if(App.FeedDogDlyCnt >= 200) {
FeedDog();
App.FeedDogDlyCnt = 0;
}
}
int main(void)
{
BSP_Init();
AppInit();
App.aaa = CENTRE_FRONT_CNT;
App.bbb = CENTRE_FRONT_SUM;
App.ccc = CENTRE_FRONT_VPT;
App.ddd = CENTRE_VPT;
while(1) {
#if(USE_WDT == 1)
FeedDogHandler();
#endif
DebugLoopHandler();
#if(USE_CS1237_LEFTUP == 1)
LEFTUP_CS1237DataLoopCollect();
#endif
#if(USE_CS1237_LEFTLOW == 1)
LEFTLOW_CS1237DataLoopCollect();
#endif
#if(USE_CS1237_RIGHTUP == 1)
RIGHTUP_CS1237DataLoopCollect();
#endif
#if(USE_CS1237_RIGHTLOW == 1)
RIGHTLOW_CS1237DataLoopCollect();
#endif
ReadLoopHandler();
DcmotorLoopHandler();
BeeperLoopHandler();
AdcLoopHander();
AppLoopHandler();
}
}
/*****************************************************************************************
* 函数名称: SysTick_IrqHandler
* 功能描述: 系统滴答中断
* 参 数: 无
* 返 回 值: 无
*****************************************************************************************/
void SysTick_IrqHandler(void)
{
SysTick_IncTick();
App1mSRoutine();
Dbg1msRoutine();
LEFTUP_CS12371mSRoutine();
LEFTLOW_CS12371mSRoutine();
RIGHTUP_CS12371mSRoutine();
RIGHTLOW_CS12371mSRoutine();
Beeper_ClearDelay();
Dcmotor1msRoutine();
Adc1mSRoutine();
}
/*****************************************************************************************
* 函数名称: DBG_RxPinIntCallBack
* 功能描述: DBG接收中断回调函数
* 参 数: 无
* 返 回 值: 无
*****************************************************************************************/
void DBG_RxPinIntCallBack(void)
{
//PORT_Toggle(LED_Blue_PORT, LED_Blue_PIN);
}
#if(USE_USART3 == 1)
// 定义函数Usart3RxIrqCallback，用于处理串口接收中断
void Usart3RxIrqCallback(void)
{
// 如果1分钟超时计数器为0，则清空RxLen
if(App.Uart3RxTimeOut1mSCnt == 0)
{
App.RxUart3Data.HeaderFrame = false;
App.RxUart3Data.RxLen = 0;
}
App.Uart3RxTimeOut1mSCnt = 5;
// 读取串口接收到的数据
uint8_t Data = Uart3Rec();
if(App.RxUart3Data.HeaderFrame == false)
{
if(App.RxUart3Data.RxLen == 0 && Data == 0x01)
{
App.RxUart3Data.HeaderFrame = true;
App.RxUart3Data.RxLen = 0;
App.RxUart3Data.RxBuff[App.RxUart3Data.RxLen++] = Data;
}
else if(App.RxUart3Data.RxLen == 0)
{
App.RxUart3Data.HeaderFrame = true;
App.RxUart3Data.RxLen = 0;
App.RxUart3Data.RxBuff[App.RxUart3Data.RxLen++] = Data;
}
}
else
{
// if(App.RxUart3Data.RxLen == 1 && Data != 0x01)
// {
// App.RxUart3Data.HeaderFrame = false;
// return;
// }
// 如果RxLen小于128，则将数据添加到RxBuff中
if(App.RxUart3Data.RxLen < 128) {
App.RxUart3Data.RxBuff[App.RxUart3Data.RxLen++] = Data;
}
if(App.RxUart3Data.RxLen >= 4)
{
if(App.RxUart3Data.RxBuff[0] == 0x01 && App.RxUart3Data.RxBuff[1] == 0x04)
{
App.MotorPara.RestFlag = true;
}
else if(App.RxUart3Data.RxBuff[0] == 0x01 && App.RxUart3Data.RxBuff[1] == 0x01)
{
App.MotorPara.RestFlag = false;
int16_t h = ((App.RxUart3Data.RxBuff[2]<<8) & 0xff00) + (App.RxUart3Data.RxBuff[3] & 0x00ff);
if(h >=720 && h <= 1208)
{
App.MotorPara.Current_height = h;
}
if(App.MotorPara.InitialFlag == true && h >=720 && h <= 1200)
{//缓存每个高度点对于零点应力的差值，对不同高度点的应力进行补偿
if(App.MotorPara.LU_HeightStress[(App.MotorPara.Current_height)-720] == 0
|| App.MotorPara.LL_HeightStress[(App.MotorPara.Current_height)-720] == 0
|| App.MotorPara.RU_HeightStress[(App.MotorPara.Current_height)-720] == 0
|| App.MotorPara.RL_HeightStress[(App.MotorPara.Current_height)-720] == 0)
{
App.MotorPara.CaliNum++;
}
App.MotorPara.LU_HeightStress[(App.MotorPara.Current_height)-720] = App.ADTrack.LeftUp_Org_AdZero - App.LeftUpCS1237_TrendArray[0];
App.MotorPara.LL_HeightStress[(App.MotorPara.Current_height)-720] = App.ADTrack.LeftLow_Org_AdZero - App.LeftLowCS1237_TrendArray[0];
App.MotorPara.RU_HeightStress[(App.MotorPara.Current_height)-720] = App.ADTrack.RightUp_Org_AdZero - App.RightUpCS1237_TrendArray[0];
App.MotorPara.RL_HeightStress[(App.MotorPara.Current_height)-720] = App.ADTrack.RightLow_Org_AdZero - App.RightLowCS1237_TrendArray[0];
}
}
memset(App.RxUart3Data.RxBuff, 0, sizeof(App.RxUart3Data.RxBuff));
App.RxUart3Data.RxLen = 0;
App.Uart3RxTimeOut1mSCnt = 0;
App.RxUart3Data.HeaderFrame = false;
}
}
}
#endif
void RtcPeriod_IrqCallback(void)
{
//PORT_Toggle(LED_Green_PORT, LED_Green_PIN);
}
void ADC_DMA_CallBack(void)
{
}
Reference in New Issue View Git Blame Copy Permalink