#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)
{
	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)
{
	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)
{

	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)
{
	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.LeftUpVPT = 5000;
			App.ADTrack.LeftLowVPT = 5000;
			App.ADTrack.RightUpVPT = 5000;
			App.ADTrack.RightLowVPT = 5000;	
			App.ADTrack.LeftUpShieldVPT = SHIELD_VPT;
			App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
			App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
			App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
			App.ADTrack.Stop_VPT = 4000;
		break;}
		
		case LEVEL_2:{
			App.ADTrack.LeftUpVPT = 7000;
			App.ADTrack.LeftLowVPT = 7000;
			App.ADTrack.RightUpVPT = 7000;
			App.ADTrack.RightLowVPT = 7000;	
			App.ADTrack.LeftUpShieldVPT = SHIELD_VPT;
			App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
			App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
			App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
			App.ADTrack.Stop_VPT = 5000;
		break;}
		
		case LEVEL_3:{
			App.ADTrack.LeftUpVPT = 10000;
			App.ADTrack.LeftLowVPT = 10000;
			App.ADTrack.RightUpVPT = 10000;
			App.ADTrack.RightLowVPT = 10000;	
			App.ADTrack.LeftUpShieldVPT = SHIELD_VPT;
			App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
			App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
			App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
			App.ADTrack.Stop_VPT = 6000;
		break;}
		
		case LEVEL_4:{
			App.ADTrack.LeftUpVPT = 15000;
			App.ADTrack.LeftLowVPT = 15000;
			App.ADTrack.RightUpVPT = 15000;
			App.ADTrack.RightLowVPT = 15000;	
			App.ADTrack.LeftUpShieldVPT = SHIELD_VPT;
			App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
			App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
			App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
			App.ADTrack.Stop_VPT = 10000;
		break;}
		
		case LEVEL_5:{
			App.ADTrack.LeftUpVPT = 20000;
			App.ADTrack.LeftLowVPT = 20000;
			App.ADTrack.RightUpVPT = 20000;
			App.ADTrack.RightLowVPT = 20000;	
			App.ADTrack.LeftUpShieldVPT = SHIELD_VPT;
			App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
			App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
			App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
			App.ADTrack.Stop_VPT = 15000;	
		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 LeftDiff = abs(Oll_diff1) + abs(Olu_diff1) + abs(Oll_diff2) + abs(Olu_diff2) + abs(Oll_diff3) + abs(Olu_diff3);
	int32_t RightDiff = abs(Orl_diff1) + abs(Oru_diff1) + abs(Orl_diff2) + abs(Oru_diff2) + abs(Orl_diff3) + abs(Oru_diff3);
	
	if(App.EmeFlag == true)
	{
		if(App.DownFlag == true && App.EmergencyDelay1mSCnt > 0)
		{
			BDC_UP();
			LED_RED_ON();
			LED_GREEN_OFF();
		}
		else if(App.UpFlag == true && App.EmergencyDelay1mSCnt > 0)
		{
			BDC_DOWN();
			LED_RED_ON();
			LED_GREEN_OFF();
		}
		else if(App.EmergencyDelay1mSCnt <= 0)
		{
			BDC_STOP();
			LED_RED_OFF();
			LED_GREEN_ON();
			App.StopFlag = true;
			App.DownFlag = false;
			App.UpFlag = false;
			App.EmeFlag = false;
			App.StopCollDelay1mSCnt = STOP_TIME_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-CENTRE_VPT)//判断中间操作
		&& (ll_ad[0] > App.ADTrack.LeftLow_Org_AdZero+CENTRE_VPT)
		&& (ru_ad[0] < App.ADTrack.RightUp_Org_AdZero-CENTRE_VPT)
		&& (rl_ad[0] > App.ADTrack.RightLow_Org_AdZero+CENTRE_VPT)
		&& (lu_ad[1] < App.ADTrack.LeftUp_Org_AdZero-CENTRE_VPT)
		&& (ll_ad[1] > App.ADTrack.LeftLow_Org_AdZero+CENTRE_VPT)
		&& (ru_ad[1] < App.ADTrack.RightUp_Org_AdZero-CENTRE_VPT)
		&& (rl_ad[1] > App.ADTrack.RightLow_Org_AdZero+CENTRE_VPT)
		&& (lu_ad[2] < App.ADTrack.LeftUp_Org_AdZero-CENTRE_VPT)
		&& (ll_ad[2] > App.ADTrack.LeftLow_Org_AdZero+CENTRE_VPT)
		&& (ru_ad[2] < App.ADTrack.RightUp_Org_AdZero-CENTRE_VPT)
		&& (rl_ad[2] > App.ADTrack.RightLow_Org_AdZero+CENTRE_VPT))
		|| ((lu_ad[0] > App.ADTrack.LeftUp_Org_AdZero+CENTRE_VPT)
		&& (ll_ad[0] < App.ADTrack.LeftLow_Org_AdZero-CENTRE_VPT)
		&& (ru_ad[0] > App.ADTrack.RightUp_Org_AdZero+CENTRE_VPT)
		&& (rl_ad[0] < App.ADTrack.RightLow_Org_AdZero-CENTRE_VPT)
		&& (lu_ad[1] > App.ADTrack.LeftUp_Org_AdZero+CENTRE_VPT)
		&& (ll_ad[1] < App.ADTrack.LeftLow_Org_AdZero-CENTRE_VPT)
		&& (ru_ad[1] > App.ADTrack.RightUp_Org_AdZero+CENTRE_VPT)
		&& (rl_ad[1] < App.ADTrack.RightLow_Org_AdZero-CENTRE_VPT)
		&& (lu_ad[2] > App.ADTrack.LeftUp_Org_AdZero+CENTRE_VPT)
		&& (ll_ad[2] < App.ADTrack.LeftLow_Org_AdZero-CENTRE_VPT)
		&& (ru_ad[2] > App.ADTrack.RightUp_Org_AdZero+CENTRE_VPT)
		&& (rl_ad[2] < App.ADTrack.RightLow_Org_AdZero-CENTRE_VPT)))
		{
			App.CentreRunFalg = true;
			App.CentCollDelay1mSCnt = CENT_TIME_MS;
			if(LeftDiff > RightDiff)
			{
				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((abs(Olu_diff1) < (abs(Oll_diff1) + CENTRE_SHIELD_VPT))
					&& (abs(Oru_diff1) < (abs(Orl_diff1) + CENTRE_SHIELD_VPT))
					&& (abs(Olu_diff2) < (abs(Oll_diff2) + CENTRE_SHIELD_VPT))
					&& (abs(Oru_diff2) < (abs(Orl_diff2) + CENTRE_SHIELD_VPT))
					&& (abs(Olu_diff3) < (abs(Oll_diff3) + CENTRE_SHIELD_VPT))
					&& (abs(Oru_diff3) < (abs(Orl_diff3) + CENTRE_SHIELD_VPT))
					)//判断是否不是正面压
					{
						BDC_UP();
						LED_RED_ON();
						LED_GREEN_OFF();
						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((abs(Olu_diff1) > (abs(Oll_diff1) - CENTRE_SHIELD_VPT))
					&& (abs(Oru_diff1) > (abs(Orl_diff1) - CENTRE_SHIELD_VPT))
					&& (abs(Olu_diff2) > (abs(Oll_diff2) - CENTRE_SHIELD_VPT))
					&& (abs(Oru_diff2) > (abs(Orl_diff2) - CENTRE_SHIELD_VPT))
					&& (abs(Olu_diff3) > (abs(Oll_diff3) - CENTRE_SHIELD_VPT))
					&& (abs(Oru_diff3) > (abs(Orl_diff3) - CENTRE_SHIELD_VPT))
					)//判断是否不是正面压
					{
						BDC_DOWN();
						LED_RED_ON();
						LED_GREEN_OFF();
						App.CaliZeroFlag = false;
						App.UpFlag = false;
						App.StopFlag = false;
						App.DownFlag = true;
						App.EmeFlag = false;
						App.RunCollDelay1mSCnt = RUN_TIME_MS;
					}
				}
			}
			else if(RightDiff > LeftDiff)
			{
				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((abs(Olu_diff1) < (abs(Oll_diff1) + CENTRE_SHIELD_VPT))
					&& (abs(Oru_diff1) < (abs(Orl_diff1) + CENTRE_SHIELD_VPT))
					&& (abs(Olu_diff2) < (abs(Oll_diff2) + CENTRE_SHIELD_VPT))
					&& (abs(Oru_diff2) < (abs(Orl_diff2) + CENTRE_SHIELD_VPT))
					&& (abs(Olu_diff3) < (abs(Oll_diff3) + CENTRE_SHIELD_VPT))
					&& (abs(Oru_diff3) < (abs(Orl_diff3) + CENTRE_SHIELD_VPT))
					)//判断是否不是正面压
					{
					BDC_UP();
					LED_RED_ON();
					LED_GREEN_OFF();
					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((abs(Olu_diff1) > (abs(Oll_diff1) - CENTRE_SHIELD_VPT))
					&& (abs(Oru_diff1) > (abs(Orl_diff1) - CENTRE_SHIELD_VPT))
					&& (abs(Olu_diff2) > (abs(Oll_diff2) - CENTRE_SHIELD_VPT))
					&& (abs(Oru_diff2) > (abs(Orl_diff2) - CENTRE_SHIELD_VPT))
					&& (abs(Olu_diff3) > (abs(Oll_diff3) - CENTRE_SHIELD_VPT))
					&& (abs(Oru_diff3) > (abs(Orl_diff3) - CENTRE_SHIELD_VPT))
					)//判断是否不是正面压
					{
						BDC_DOWN();
						LED_RED_ON();
						LED_GREEN_OFF();
						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))
				{
					if((abs(Oll_diff1) > abs(Orl_diff1) + App.ADTrack.LeftLowShieldVPT)//判断是否不是侧面推拉
					&& (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT)
					&& (abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT)
					&& (abs(Olu_diff1) > abs(Oru_diff1) + App.ADTrack.LeftUpShieldVPT))
					{
						BDC_UP();
						LED_RED_ON();
						LED_GREEN_OFF();
						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))
				{
					if((abs(Oll_diff1) > abs(Orl_diff1) + App.ADTrack.LeftLowShieldVPT)//判断是否不是侧面推拉
					&& (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT)
					&& (abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT)
					&& (abs(Olu_diff1) > abs(Oru_diff1) + App.ADTrack.LeftUpShieldVPT))
					{
						BDC_DOWN();
						LED_RED_ON();
						LED_GREEN_OFF();
						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))
				{
					if((abs(Orl_diff1) > abs(Oll_diff1) + App.ADTrack.RightLowShieldVPT)//判断是否不是侧面推拉
					&& (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT)
					&& (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT)
					&& (abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT))
					{
						BDC_UP();
						LED_RED_ON();
						LED_GREEN_OFF();
						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))
				{
					if((abs(Orl_diff1) > abs(Oll_diff1) + App.ADTrack.RightLowShieldVPT)//判断是否不是侧面推拉
					&& (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT)
					&& (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT)
					&& (abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT))
					{
						BDC_DOWN();
						LED_RED_ON();
						LED_GREEN_OFF();
						App.CaliZeroFlag = false;
						App.DownFlag = true;
						App.StopFlag = false;
						App.UpFlag = false;
						App.EmeFlag = false;
						App.RunCollDelay1mSCnt = RUN_TIME_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))
				|| (((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))
				&& ((Oll_diff1 + Orl_diff1) > -(App.ADTrack.Stop_VPT*2/3))
				&& ((Oll_diff2 + Orl_diff2) > -(App.ADTrack.Stop_VPT*2/3))
				&& ((Oll_diff3 + Orl_diff3) > -(App.ADTrack.Stop_VPT*2/3)))
				)
			{
				BDC_STOP();
				LED_RED_OFF();
				LED_GREEN_ON();
				App.StopFlag = true;
				App.DownFlag = false;
				App.UpFlag = false;
				App.EmeFlag = false;
				App.StopCollDelay1mSCnt = STOP_TIME_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))
				|| (((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))
				&& ((Oll_diff1 + Orl_diff1) > -(App.ADTrack.Stop_VPT*2/3))
				&& ((Oll_diff2 + Orl_diff2) > -(App.ADTrack.Stop_VPT*2/3))
				&& ((Oll_diff3 + Orl_diff3) > -(App.ADTrack.Stop_VPT*2/3)))
				)
			{
				BDC_STOP();
				LED_RED_OFF();
				LED_GREEN_ON();
				App.StopFlag = true;
				App.DownFlag = false;
				App.UpFlag = false;
				App.EmeFlag = false;
				App.StopCollDelay1mSCnt = STOP_TIME_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.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))
				|| (((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))
				&& ((Oll_diff1 + Orl_diff1) < (App.ADTrack.Stop_VPT*2/3))
				&& ((Oll_diff2 + Orl_diff2) < (App.ADTrack.Stop_VPT*2/3))
				&& ((Oll_diff3 + Orl_diff3) < (App.ADTrack.Stop_VPT*2/3)))
				)
			{
				BDC_STOP();
				LED_RED_OFF();
				LED_GREEN_ON();
				App.StopFlag = true;
				App.DownFlag = false;
				App.UpFlag = false;
				App.EmeFlag = false;
				App.StopCollDelay1mSCnt = STOP_TIME_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))
				|| (((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))
				&& ((Oll_diff1 + Orl_diff1) < (App.ADTrack.Stop_VPT*2/3))
				&& ((Oll_diff2 + Orl_diff2) < (App.ADTrack.Stop_VPT*2/3))
				&& ((Oll_diff3 + Orl_diff3) < (App.ADTrack.Stop_VPT*2/3)))
				)
			{
				BDC_STOP();
				LED_RED_OFF();
				LED_GREEN_ON();
				App.StopFlag = true;
				App.DownFlag = false;
				App.UpFlag = false;
				App.EmeFlag = false;
				App.StopCollDelay1mSCnt = STOP_TIME_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.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("**           	SingleColumn_Trolley            **\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();
	
	Beeper_Init();
	
	BDC_STOP();
	LED_RED_OFF();
	LED_GREEN_ON();
	App.Status = APP_STATUS_ZERO_MARKING;
	App.TD1mSDelayCnt = 2000;
	App.RStatus = READ_STATUS_START;
	GradeControl(App.Para.Layout.ControlLevel);//等级控制
}
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)
			{
				#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_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.CaliZeroFlag = false;
				App.StopFlag = true;
				App.UpFlag = false;
				App.DownFlag = false;
				App.EmeFlag = false;
				
				BDC_STOP();
				LED_RED_OFF();
				LED_GREEN_ON();
				App.Status = APP_STATUS_IDLE;
				
				Beeper_Start(200,200,2);
			}
		break;}
	}
}
static void App1mSRoutine(void)
{
	if(App.TD1mSDelayCnt > 0)
		App.TD1mSDelayCnt--;
	if(App.Read1mSDelayCnt > 0)
		App.Read1mSDelayCnt--;
	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.CentCollDelay1mSCnt > 0)
		App.CentCollDelay1mSCnt--;
	App.FeedDogDlyCnt++;
}
void FeedDogHandler(void)
{
	if(App.FeedDogDlyCnt >= 200) {
		FeedDog(); 
		App.FeedDogDlyCnt = 0;
	}
}
int main(void) 
{
	BSP_Init();
  AppInit();
	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();
    AdcLoopHander();
		BeeperLoopHandler();
		AppLoopHandler();
	}
}
/*****************************************************************************************
* 函数名称: SysTick_IrqHandler
* 功能描述: 系统滴答中断
* 参    数: 无
* 返 回 值: 无
*****************************************************************************************/
void SysTick_IrqHandler(void)
{
	SysTick_IncTick();
	App1mSRoutine();
	Dbg1msRoutine();
	LEFTUP_CS12371mSRoutine();
	LEFTLOW_CS12371mSRoutine();
	RIGHTUP_CS12371mSRoutine();
	RIGHTLOW_CS12371mSRoutine();
	Dcmotor1msRoutine();
	Beeper_ClearDelay();
  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
	{
		// 如果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 == false)
			{
				App.MotorPara.RestFlag = true;
				Beeper_Start(1500,0,1);
			}
			else if(App.RxUart3Data.RxBuff[0] == 0x01 && App.RxUart3Data.RxBuff[1] == 0x01)
			{
				if(App.MotorPara.RestFlag == true)
				{
					App.MotorPara.RestFlag = false;
					Beeper_Start(200,200,2);
				}
				int16_t h = ((App.RxUart3Data.RxBuff[2]<<8) & 0xff00) + (App.RxUart3Data.RxBuff[3] & 0x00ff);
				App.MotorPara.Current_height = h;
			}
			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)
{
    
}