diff --git a/Project/HC32F460/MDK/HC32F460.uvoptx b/Project/HC32F460/MDK/HC32F460.uvoptx
index a6a444e..fe7fab1 100644
--- a/Project/HC32F460/MDK/HC32F460.uvoptx
+++ b/Project/HC32F460/MDK/HC32F460.uvoptx
@@ -228,57 +228,52 @@
         <Ww>
           <count>14</count>
           <WinNumber>1</WinNumber>
-          <ItemText>App.ADTrack.Slope_VPT,0x0A</ItemText>
+          <ItemText>App.MotorPara.LU_HeightStress,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>15</count>
           <WinNumber>1</WinNumber>
-          <ItemText>App.RightUpCS1237_SlopeArray[0],0x0A</ItemText>
+          <ItemText>App.MotorPara.LL_HeightStress,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>16</count>
           <WinNumber>1</WinNumber>
-          <ItemText>App.ADTrack.Stop_VPT,0x0A</ItemText>
+          <ItemText>App.MotorPara.RU_HeightStress,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>17</count>
           <WinNumber>1</WinNumber>
-          <ItemText>Olu_diff1,0x0A</ItemText>
+          <ItemText>App.MotorPara.RL_HeightStress</ItemText>
         </Ww>
         <Ww>
           <count>18</count>
           <WinNumber>1</WinNumber>
-          <ItemText>Oru_diff1,0x0A</ItemText>
+          <ItemText>App.TD1mSDelayCnt,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>19</count>
           <WinNumber>1</WinNumber>
-          <ItemText>Olu_diff2,0x0A</ItemText>
+          <ItemText>App.RxUart3Data.RxBuff,0x10</ItemText>
         </Ww>
         <Ww>
           <count>20</count>
           <WinNumber>1</WinNumber>
-          <ItemText>Oru_diff2,0x0A</ItemText>
+          <ItemText>App.RxUart3Data.RxLen,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>21</count>
           <WinNumber>1</WinNumber>
-          <ItemText>Olu_diff3,0x0A</ItemText>
+          <ItemText>App.MotorPara.Current_height,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>22</count>
           <WinNumber>1</WinNumber>
-          <ItemText>Oru_diff3,0x0A</ItemText>
+          <ItemText>App.Status</ItemText>
         </Ww>
         <Ww>
           <count>23</count>
           <WinNumber>1</WinNumber>
-          <ItemText>App.LeftUpCS1237_SlopeArray[0],0x0A</ItemText>
-        </Ww>
-        <Ww>
-          <count>24</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>App.RightUpCS1237_SlopeArray[0],0x0A</ItemText>
+          <ItemText>App.MotorPara.CaliNum,0x0A</ItemText>
         </Ww>
       </WatchWindow1>
       <Tracepoint>
diff --git a/Project/HC32F460/source/Module/CS1237/CS1237_LEFTLOW.c b/Project/HC32F460/source/Module/CS1237/CS1237_LEFTLOW.c
index 7e7969d..8238a51 100644
--- a/Project/HC32F460/source/Module/CS1237/CS1237_LEFTLOW.c
+++ b/Project/HC32F460/source/Module/CS1237/CS1237_LEFTLOW.c
@@ -120,10 +120,10 @@ static void Set_CS1237_Config(uint8_t ad_reg)
     }
     /* 8: clk46 */
 	 CS1237_Clock();
-    CS1237_DOUT_H();
+   CS1237_DOUT_H();
 	 CS1237_DOUT_IN();
 	 CS1237IntEnable();
-    return ;
+   return ;
 }
 
 /**读CS1237寄存器*/
diff --git a/Project/HC32F460/source/User/Inc/Algorithm.h b/Project/HC32F460/source/User/Inc/Algorithm.h
index dd97796..f9ca1b7 100644
--- a/Project/HC32F460/source/User/Inc/Algorithm.h
+++ b/Project/HC32F460/source/User/Inc/Algorithm.h
@@ -49,6 +49,7 @@ bool count_smaller(int32_t *arr, uint8_t size, int32_t target);
 uint16_t CRC_Modbus(uint16_t wBase, uint8_t *para, uint16_t length);
 uint8_t CRC_Sum(uint8_t *_pbuff, uint16_t _cmdLen);
 bool isAllZero(uint8_t *arr, int size);
+int fixWithMedianWindow(int arr[], int size, int windowSize, int threshold);
 #endif
 
 
diff --git a/Project/HC32F460/source/User/Inc/bsp.h b/Project/HC32F460/source/User/Inc/bsp.h
index 4fa2a17..9c11293 100644
--- a/Project/HC32F460/source/User/Inc/bsp.h
+++ b/Project/HC32F460/source/User/Inc/bsp.h
@@ -21,13 +21,14 @@
 #define  USE_I2C1					  	0					//I2C1¿ª¹Ø£¬0¡¢¹Ø±Õ£¬1¡¢ÆôÓÃ
 #define  USE_SPI1					  	0					//SPI1¿ª¹Ø£¬0¡¢¹Ø±Õ£¬1¡¢ÆôÓÃ
 #define  USE_SPI3					  	0					//SPI3¿ª¹Ø£¬0¡¢¹Ø±Õ£¬1¡¢ÆôÓÃ
+#define	 USE_USART3						1
 
 #define  USE_BOOTLOADER			  	0					//BOOTLOADER¿ª¹Ø£¬0¡¢¹Ø±Õ£¬1¡¢ÆôÓÃ
 
 #define	USE_CS1237_LEFTUP			1					//×óÉÏCS1237¿ª¹Ø£¬0¡¢¹Ø±Õ£¬1¡¢ÆôÓÃ
 #define	USE_CS1237_LEFTLOW		1					//×óÏÂCS1237¿ª¹Ø£¬0¡¢¹Ø±Õ£¬1¡¢ÆôÓÃ
 #define	USE_CS1237_RIGHTUP		1					//ÓÒÉÏCS1237¿ª¹Ø£¬0¡¢¹Ø±Õ£¬1¡¢ÆôÓÃ
-#define	USE_CS1237_RIGHTLOW		1					//ÓÒÉÏCS1237¿ª¹Ø£¬0¡¢¹Ø±Õ£¬1¡¢ÆôÓÃ
+#define	USE_CS1237_RIGHTLOW		1					//ÓÒÏÂCS1237¿ª¹Ø£¬0¡¢¹Ø±Õ£¬1¡¢ÆôÓÃ
 
 #define	USE_AGC						1					//Ëã·¨¿ØÖÆ¿ª¹Ø£¬0¡¢¹Ø±Õ£¬1¡¢ÆôÓÃ
 //<<----------------------------	MCU  -------------------------------->>
@@ -50,7 +51,64 @@
 #define	DBG_RX_IRQn								(Int000_IRQn)
 #define	DBG_ER_IRQn								(Int001_IRQn)
 #endif
+//<			USART3
+//#if(USE_USART3 == 1)
+//#define	USAR3_CH							(M4_USART3)
+//#define	USAR3_RX_PORT					(PortH)
+//#define	USAR3_RX_PIN					(Pin02)
+//#define	USAR3_TX_PORT					(PortC)
+//#define	USAR3_TX_PIN						(Pin13)
+//#define	USAR3_RX_FUNC						(Func_Usart3_Rx)
+//#define 	USAR3_TX_FUNC       	(Func_Usart3_Tx)
+//#define 	USAR3_RI_NUM						(INT_USART3_RI)
+//#define 	USAR3_EI_NUM						(INT_USART3_EI)
+//#define 	USAR3_TI_NUM						(INT_USART3_TI)
+//#define 	USAR3_TCI_NUM						(INT_USART3_TCI)
+//#define	USAR3_FCG1_PERIPH						(PWC_FCG1_PERIPH_USART3)
+//#define	USAR3_RXPIN_EXIT_CH						(ExtiCh14)
+//#define	USAR3_RXPIN_EXIT_SRC					(INT_PORT_EIRQ14)
+//#define	USAR3_RX_IRQn								(Int006_IRQn)
+//#define	USAR3_ER_IRQn								(Int007_IRQn)
+//#endif
+#if(USE_USART3 == 1)
+#define	USART3_CH							(M4_USART3)
+#define	USART3_RX_PORT						(PortH)
+#define	USART3_RX_PIN						(Pin02)
+#define	USART3_RX_FUNC						(Func_Usart3_Rx)
+#define USART3_USART_RI_NUM						(INT_USART3_RI)
+#define USART3_USART_EI_NUM						(INT_USART3_EI)
+#define	USART3_FCG1_PERIPH						(PWC_FCG1_PERIPH_USART3)
+#define	USART3_RX_IRQn								(Int006_IRQn)
+#define	USART3_ER_IRQn								(Int007_IRQn)
+//#define	USART3_RXPIN_IRQn					Int002_IRQn
+#endif
 
+//			RS485
+#if(USE_RS485 == 1)
+#define	RS485_USART_CH							(M4_USART1)
+//#define	RS485_USART_RX_PORT						(PortB)
+//#define	RS485_USART_RX_PIN						(Pin08)
+//#define	RS485_USART_TX_PORT						(PortB)
+//#define	RS485_USART_TX_PIN						(Pin09)
+#define	RS485_USART_RX_PORT						(PortA)
+#define	RS485_USART_RX_PIN						(Pin15)
+#define	RS485_USART_TX_PORT						(PortA)
+#define	RS485_USART_TX_PIN						(Pin12)
+#define	RS485_USART_RX_FUNC						(Func_Usart1_Rx)
+#define 	RS485_USART_TX_FUNC       	(Func_Usart1_Tx)
+#define 	RS485_USART_RI_NUM						(INT_USART1_RI)
+#define 	RS485_USART_EI_NUM						(INT_USART1_EI)
+#define 	RS485_USART_TI_NUM						(INT_USART1_TI)
+#define 	RS485_USART_TCI_NUM						(INT_USART1_TCI)
+#define	RS485_FCG1_PERIPH						(PWC_FCG1_PERIPH_USART1)
+#define	RS485_RXPIN_EXIT_CH						(ExtiCh14)
+#define	RS485_RXPIN_EXIT_SRC					(INT_PORT_EIRQ14)
+
+#define 	RS485_CTRL_PORT                    (PortC)
+#define 	RS485_CTRL_PIN                     (Pin13)
+#define	RS485_TX()								PORT_SetBits(RS485_CTRL_PORT, RS485_CTRL_PIN)
+#define	RS485_RX()								PORT_ResetBits(RS485_CTRL_PORT, RS485_CTRL_PIN)
+#endif
 //			SPI3
 #if(USE_SPI3 == 1)
 /* SPI3_SCK Port/Pin definition */
@@ -79,32 +137,6 @@
 #define SPI3_UNIT                        (M4_SPI3)
 #define SPI3_UNIT_CLOCK                  (PWC_FCG1_PERIPH_SPI3)
 #endif
-//			RS485
-#if(USE_RS485 == 1)
-#define	RS485_USART_CH							(M4_USART1)
-//#define	RS485_USART_RX_PORT						(PortB)
-//#define	RS485_USART_RX_PIN						(Pin08)
-//#define	RS485_USART_TX_PORT						(PortB)
-//#define	RS485_USART_TX_PIN						(Pin09)
-#define	RS485_USART_RX_PORT						(PortA)
-#define	RS485_USART_RX_PIN						(Pin15)
-#define	RS485_USART_TX_PORT						(PortA)
-#define	RS485_USART_TX_PIN						(Pin12)
-#define	RS485_USART_RX_FUNC						(Func_Usart1_Rx)
-#define 	RS485_USART_TX_FUNC       	            (Func_Usart1_Tx)
-#define 	RS485_USART_RI_NUM						(INT_USART1_RI)
-#define 	RS485_USART_EI_NUM						(INT_USART1_EI)
-#define 	RS485_USART_TI_NUM						(INT_USART1_TI)
-#define 	RS485_USART_TCI_NUM						(INT_USART1_TCI)
-#define	RS485_FCG1_PERIPH						(PWC_FCG1_PERIPH_USART1)
-#define	RS485_RXPIN_EXIT_CH						(ExtiCh14)
-#define	RS485_RXPIN_EXIT_SRC					(INT_PORT_EIRQ14)
-
-#define 	RS485_CTRL_PORT                    (PortC)
-#define 	RS485_CTRL_PIN                     (Pin13)
-#define	RS485_TX()								PORT_SetBits(RS485_CTRL_PORT, RS485_CTRL_PIN)
-#define	RS485_RX()								PORT_ResetBits(RS485_CTRL_PORT, RS485_CTRL_PIN)
-#endif
 //<		RTC
 #if(USE_RTC == 1)
 #define	RTC_IRQn									Int001_IRQn
@@ -207,9 +239,15 @@ void RS485UsartErrIrqCallback(void);
 void RS485_RxPinIntCallBack(void);
 void RS485UsartRxIrqCallback(void);
 void RS485UartSend(uint8_t *sData, uint16_t sLen);
-
 void RS485_1msRoutine(void);
 
+uint8_t Uart3Rec(void);
+void Usart3ErrIrqCallback(void);
+void Usart3_RxPinIntCallBack(void);
+void Usart3RxIrqCallback(void);
+void Usart3Send(uint8_t *sData, uint16_t sLen);
+void Uart3_Config(uint32_t BaudRate);
+
 void DbgUsartRxIrqCallback(void);
 void DBGUsartErrIrqCallback(void);
 void DBG_RxPinIntCallBack(void);
@@ -237,7 +275,7 @@ uint32_t mf_showfree(uint8_t *drv);
 void TimeGet(struct tm *cTime);
 int TimeTs(void);
 void TimeShow(uint32_t dwStamp);
-void TimeSync(time_t ts) ;
+void TimeSync(time_t ts);
 
 void delay_5ns(uint32_t ns);
 
@@ -253,3 +291,6 @@ void ExtInt06IntDisable(void);
 
 
 
+
+
+
diff --git a/Project/HC32F460/source/User/Inc/main.h b/Project/HC32F460/source/User/Inc/main.h
index 52fd958..213641a 100644
--- a/Project/HC32F460/source/User/Inc/main.h
+++ b/Project/HC32F460/source/User/Inc/main.h
@@ -11,8 +11,8 @@
 #define 	SLOPE_CNT			  20
 
 #define 	SLOPE_VPT				1000
-#define		PRESS_VPT				10
-#define		FOLLOW_VPT			20
+#define		PRESS_VPT				3
+#define		FOLLOW_VPT			10
 #define		SHIELD_VPT			5000
 #define		EME_STOP_VPT		4000
 #define		CENTRE_VPT			2000
@@ -25,6 +25,8 @@
 
 #define		TRAVEL_TIME_MS	30000
 
+#define		PEAK						1200		//1200mm
+#define		VALLEY					720			//720mm
 
 #define	BDC_STOP()			(SET_TWIN_BIT0(),SET_TWIN_BIT1())
 #define BDC_UP()				(SET_TWIN_BIT0(),CLR_TWIN_BIT1())
@@ -63,7 +65,16 @@ typedef struct{
 	int32_t RightUp_Org_AdZero;//ÓÒÉÏÉ豸ԭʼADÁãµã
 	int32_t RightLow_Org_AdZero;//ÓÒÏÂÉ豸ԭʼADÁãµã
 }ADTrack_t;
-
+typedef struct{//µç»ú²ÎÊý
+	bool InitialFlag;//³õʼ±ê־λ
+	bool CaliFlag;
+	int16_t Current_height;//µ±Ç°¸ß¶Èmm
+	int16_t CaliNum;//±êУÊýÁ¿
+	int32_t LU_HeightStress[488];//×óÉÏ´«¸ÐÆ÷¶ÔÓ¦¸ß¶ÈÓ¦Á¦
+	int32_t LL_HeightStress[488];//×óÏ´«¸ÐÆ÷¶ÔÓ¦¸ß¶ÈÓ¦Á¦
+	int32_t RU_HeightStress[488];//ÓÒÉÏ´«¸ÐÆ÷¶ÔÓ¦¸ß¶ÈÓ¦Á¦
+	int32_t RL_HeightStress[488];//ÓÒÉÏ´«¸ÐÆ÷¶ÔÓ¦¸ß¶ÈÓ¦Á¦
+}MotorPara_t;
 /*Êý¾Ý¶Áȡ״̬»ú*/
 typedef enum {
 	READ_STATUS_IDLE,
@@ -84,10 +95,12 @@ typedef enum {
 	APP_STATUS_IDLE,
 	APP_STATUS_ON,
 	APP_STATUS_OFF,
-	APP_STATUS_UP1,
-	APP_STATUS_DOWN,
-	APP_STATUS_UP2,
-	APP_STATUS_WAIT,
+	APP_STATUS_MOTOR_INIT,
+	APP_STATUS_WAIT_MOTOR_INIT,
+	APP_STATUS_WAIT_MOTOR_CALI_UP,
+	APP_STATUS_WAIT_MOTOR_CALI_DOWN,
+	APP_STATUS_WAIT_STEADY,
+	APP_STATUS_ZERO_MARKING,
 }AppStatus_m;
 
 typedef struct{
@@ -97,16 +110,26 @@ typedef struct{
 	uint8_t RxBuff[10];
 }Uart1Rx_t;
 
+typedef struct{
+	__IO uint16_t rPayLen;
+	__IO bool HeaderFrame;
+	__IO uint16_t RxLen;
+	uint8_t RxBuff[4];
+}Uart3Rx_t;
+
 typedef struct {
 	AppStatus_m Status;//״̬
 	ReadStatus_m RStatus;
 	AppPara_T Para;
 	Uart1Rx_t RxUart1Data;
+	Uart3Rx_t RxUart3Data;
 
-	ADTrack_t ADTrack;	
+	ADTrack_t ADTrack;
+	MotorPara_t	MotorPara;
 	uint32_t TD1mSDelayCnt;
 	uint32_t Read1mSDelayCnt;
 	uint32_t Uart1Delay1mSCnt;
+	uint32_t Uart3RxTimeOut1mSCnt;
 	uint32_t StopCollDelay1mSCnt;
 	uint32_t RunCollDelay1mSCnt;
 	uint32_t EmergencyDelay1mSCnt;
diff --git a/Project/HC32F460/source/User/Src/Algorithm.c b/Project/HC32F460/source/User/Src/Algorithm.c
index fd98f88..ca7235f 100644
--- a/Project/HC32F460/source/User/Src/Algorithm.c
+++ b/Project/HC32F460/source/User/Src/Algorithm.c
@@ -1,7 +1,115 @@
+#include <stdio.h>
+#include <stdlib.h>
 #include "bsp.h"
 #include "Algorithm.h"
 #include "Debug.h"
 
+/**
+ * ʹÓû¬¶¯´°¿ÚÐÞÕýÍ»±äÖµ£¬²¢´òÓ¡ÐÞ¸ÄÐÅÏ¢
+ * 
+ * @param arr ´ý´¦ÀíµÄintÀàÐÍÊý×é
+ * @param size Êý×éµÄ´óС
+ * @param windowSize »¬¶¯´°¿Ú´óС£¨½¨ÒéÆæÊý£©
+ * @param threshold Í»±äãÐÖµ
+ * @return ·µ»ØÐÞÕýµÄ´ÎÊý
+ */
+int fixWithMedianWindow(int arr[], int size, int windowSize, int threshold) {
+    if (size <= 2 || windowSize < 3) {
+        return 0;
+    }
+    
+    int fixCount = 0;
+    int *temp = (int*)malloc(size * sizeof(int));
+    if (temp == NULL) {
+        DBG_LOG("ÄÚ´æ·ÖÅäʧ°Ü£¡\n");
+        return 0;
+    }
+    
+    memcpy(temp, arr, size * sizeof(int));
+    
+    DBG_LOG("¿ªÊ¼¼ì²â²¢ÐÞÕýÍ»±äµã (ʹÓÃÖÐλÊý´°¿Ú)...\n");
+    DBG_LOG("Êý×é´óС: %d, ´°¿Ú´óС: %d, ãÐÖµ: %d\n", size, windowSize, threshold);
+    DBG_LOG("========================================\n");
+    
+    // ¼ì²â²¢ÐÞÕý
+    for (int i = 0; i < size; i++) {
+        // ¼ÆËã´°¿Ú±ß½ç
+        int halfWindow = windowSize / 2;
+        int start = (i - halfWindow) < 0 ? 0 : (i - halfWindow);
+        int end = (i + halfWindow) >= size ? (size - 1) : (i + halfWindow);
+        int windowActualSize = end - start + 1;
+        
+        // ¼ì²éµ±Ç°µãÊÇ·ñΪͻ±äµã
+        int isMutated = 0;
+        if (i > 0 && i < size - 1) {
+            int diffPrev = abs(arr[i] - arr[i-1]);
+            int diffNext = abs(arr[i+1] - arr[i]);
+            if (diffPrev > threshold && diffNext > threshold) {
+                isMutated = 1;
+            }
+        }
+        
+        if (isMutated || (i > 0 && abs(arr[i] - arr[i-1]) > threshold * 2)) {
+            // ÊÕ¼¯´°¿ÚÄÚµÄÖµ
+            int *window = (int*)malloc(windowActualSize * sizeof(int));
+            if (window == NULL) continue;
+            
+            int idx = 0;
+            for (int j = start; j <= end; j++) {
+                if (j != i) {  // Åųý×ÔÉí
+                    window[idx++] = arr[j];
+                }
+            }
+            
+            // Èç¹û´°¿ÚÄÚÓÐЧֵ̫ÉÙ£¬Ê¹ÓÃÇ°ºóֵƽ¾ù
+            if (idx < 2) {
+                if (i > 0 && i < size - 1) {
+                    int correctedValue = (arr[i-1] + arr[i+1]) / 2;
+                    DBG_LOG("λÖà [%d]: Ô­Öµ %d -> ÐÂÖµ %d (´°¿ÚÄÚÎÞ×ã¹»²Î¿¼Öµ)\n", 
+                           i, arr[i], correctedValue);
+                    temp[i] = correctedValue;
+                    fixCount++;
+                }
+                free(window);
+                continue;
+            }
+            
+            // ÅÅÐòÕÒÖÐλÊý£¨¼òµ¥Ã°ÅÝÅÅÐò£¬ÒòΪ´°¿Úͨ³£²»´ó£©
+            for (int m = 0; m < idx - 1; m++) {
+                for (int n = 0; n < idx - m - 1; n++) {
+                    if (window[n] > window[n+1]) {
+                        int tmp = window[n];
+                        window[n] = window[n+1];
+                        window[n+1] = tmp;
+                    }
+                }
+            }
+            
+            // È¡ÖÐλÊý
+            int correctedValue = window[idx / 2];
+            
+            DBG_LOG("λÖà [%d]: Ô­Öµ %d -> ÐÂÖµ %d\n", i, arr[i], correctedValue);
+            DBG_LOG("        ´°¿Ú·¶Î§: [%d-%d], ʹÓÃÖÐλÊý: %d\n", start, end, correctedValue);
+            
+            temp[i] = correctedValue;
+            fixCount++;
+            free(window);
+        }
+    }
+    
+    // ½«ÐÞÕýºóµÄֵд»ØÔ­Êý×é
+    memcpy(arr, temp, size * sizeof(int));
+    free(temp);
+    
+    if (fixCount == 0) {
+        DBG_LOG("δ·¢ÏÖÍ»±äµã\n");
+    } else {
+        DBG_LOG("========================================\n");
+        DBG_LOG("¹²ÐÞÕýÁË %d ¸öÍ»±äµã\n", fixCount);
+    }
+    
+    return fixCount;
+}
 /*****************************************************************************************
 * º¯ÊýÃû³Æ: check_peaks_valleys_ratio
 * ¹¦ÄÜÃèÊö: ¼ì²éÊý¾ÝµÄ·åÖµÊÇ·ñ´óÓÚ0£¬¹ÈÖµÊÇ·ñСÓÚ0£¨ºöÂÔ0Öµ£©
diff --git a/Project/HC32F460/source/User/Src/bsp.c b/Project/HC32F460/source/User/Src/bsp.c
index 350a495..42e5b57 100644
--- a/Project/HC32F460/source/User/Src/bsp.c
+++ b/Project/HC32F460/source/User/Src/bsp.c
@@ -429,6 +429,78 @@ static void DBGRxWakeupInit(void)
 	enIntWakeupEnable(Extint2WU);
 }
 #endif
+#if(USE_USART3 == 1)
+static void Uart3_Config(uint32_t BaudRate)
+{
+	stc_usart_uart_init_t stcInitCfg;
+	stc_irq_regi_conf_t stcIrqRegiCfg;
+	en_result_t enRet;
+	stc_port_init_t stcPortInit;
+
+	MEM_ZERO_STRUCT(stcPortInit);
+	
+	stcPortInit.enPinOType = Pin_OType_Cmos;
+	PORT_Init(USART3_RX_PORT, USART3_RX_PIN, &stcPortInit);
+	
+	stcInitCfg.enClkMode = UsartIntClkCkNoOutput;
+	stcInitCfg.enClkDiv = UsartClkDiv_16;
+	stcInitCfg.enDataLength = UsartDataBits8;
+	stcInitCfg.enDirection = UsartDataLsbFirst;
+	stcInitCfg.enStopBit = UsartOneStopBit;
+	stcInitCfg.enParity = UsartParityNone;
+	stcInitCfg.enSampleMode = UsartSampleBit8;
+	stcInitCfg.enDetectMode = UsartStartBitFallEdge;
+	stcInitCfg.enHwFlow = UsartRtsEnable;
+	
+	PWC_Fcg1PeriphClockCmd(USART3_FCG1_PERIPH, Enable);
+	
+	PORT_SetFunc(USART3_RX_PORT, USART3_RX_PIN, USART3_RX_FUNC, Disable);
+	
+	enRet = USART_UART_Init(USART3_CH, &stcInitCfg);
+	if(enRet != Ok) {
+		Error_Handler();
+	}
+	enRet = USART_SetBaudrate(USART3_CH, BaudRate);
+	if(enRet != Ok) {
+		Error_Handler();
+	}
+	
+	stcIrqRegiCfg.enIRQn = USART3_RX_IRQn;
+	stcIrqRegiCfg.pfnCallback = &Usart3RxIrqCallback;
+	stcIrqRegiCfg.enIntSrc = USART3_USART_RI_NUM;
+	enIrqRegistration(&stcIrqRegiCfg);
+	NVIC_SetPriority(stcIrqRegiCfg.enIRQn, DDL_IRQ_PRIORITY_DEFAULT);
+	NVIC_ClearPendingIRQ(stcIrqRegiCfg.enIRQn);
+	NVIC_EnableIRQ(stcIrqRegiCfg.enIRQn);
+
+
+	/* Set USART RX error IRQ */
+	stcIrqRegiCfg.enIRQn = USART3_ER_IRQn;
+	stcIrqRegiCfg.pfnCallback = &Usart3ErrIrqCallback;
+	stcIrqRegiCfg.enIntSrc = USART3_USART_EI_NUM;
+	enIrqRegistration(&stcIrqRegiCfg);
+	NVIC_SetPriority(stcIrqRegiCfg.enIRQn, DDL_IRQ_PRIORITY_DEFAULT);
+	NVIC_ClearPendingIRQ(stcIrqRegiCfg.enIRQn);
+	NVIC_EnableIRQ(stcIrqRegiCfg.enIRQn);
+
+	USART_FuncCmd(USART3_CH, UsartRx, Enable);
+	USART_FuncCmd(USART3_CH, UsartRxInt, Enable);
+}
+
+void Uart3Send(uint8_t *sData, uint16_t sLen)
+{
+	for(int i = 0; i < sLen; i++) {
+		USART_SendData(USART3_CH, sData[i]);
+		while(USART_GetStatus(USART3_CH, UsartTxComplete) == Reset);
+	}
+}
+
+uint8_t Uart3Rec(void)
+{
+	uint16_t u16Data = USART_RecData(USART3_CH);
+	return (uint8_t)u16Data;
+}
+#endif
 static void GPIO_Config(void)
 {
 	stc_port_init_t stcPortInit;
@@ -1359,15 +1431,18 @@ void BSP_Init(void)
 	ExtiCh04_Init();
 	
 	DbgUart_Config(700000);
+#if(USE_USART3 == 1)
+	Uart3_Config(9600);
+#endif
 	//RS485Uart_Config(9600);
 	
 	//Rtc_Config();
 	//SdioInit();
 	//Spi_Config();
    // Spi3_Config();
-    dcmotor_init();
-	Adc1_Config();
-	Dma_Config();
+//    dcmotor_init();
+//	Adc1_Config();
+//	Dma_Config();
 	//FeedDog();
 
 	SysTick_Init(1000u);
@@ -1431,7 +1506,10 @@ void Wakeup(void)
 	PWC_ClkRecover();
 	//SystemClockConfig();
 	//Spi3_Config();
-	DbgUart_Config(5000000);
+	DbgUart_Config(700000);
+#if(USE_USART3 == 1)
+	Uart3_Config(9600);
+#endif
    // RS485Uart_Config(9600);
 	//GPIO_Config();
 	//SysTick_Resume();
@@ -1469,11 +1547,8 @@ __WEAK void RS485_RxPinIntCallBack(void)
 {
     
 }
-
-
-
-
 #endif
+#if(USE_DEBUG == 1)
 __WEAK void DBGUsartErrIrqCallback(void)
 {
 	if(DBG_USART_CH->SR_f.ORE) {
@@ -1496,7 +1571,32 @@ __WEAK void DbgUsartRxIrqCallback(void)
 __WEAK void DBG_RxPinIntCallBack(void)
 {
 }
+#endif
+#if(USE_USART3 == 1)
+void Usart3ErrIrqCallback(void)
+{
+	if(USART3_CH->SR_f.ORE) {
+		USART3_CH->CR1_f.CORE = 1;
+		USART_RecData(USART3_CH);
+	}
+	
+	if(USART3_CH->SR_f.PE)
+		USART3_CH->CR1_f.CPE = 1;
+	
+	if(USART3_CH->SR_f.FE)
+		USART3_CH->CR1_f.CFE = 1;
+}
 
+
+__WEAK void Usart3RxIrqCallback(void)
+{
+	
+}
+
+__WEAK void Usart3_RxPinIntCallBack(void)
+{
+}
+#endif
 /*****************************************************************************************
 * º¯ÊýÃû³Æ: RtcPeriod_IrqCallback
 * ¹¦ÄÜÃèÊö: RTCÖжϻص÷
diff --git a/Project/HC32F460/source/User/Src/main.c b/Project/HC32F460/source/User/Src/main.c
index 764c262..e2e4168 100644
--- a/Project/HC32F460/source/User/Src/main.c
+++ b/Project/HC32F460/source/User/Src/main.c
@@ -40,12 +40,23 @@ void LEFTUP_CS1237AinADataProcessCallBack(int32_t AD)
 //	{
 //		AD = App.LeftUpCS1237_TrendArray[0];
 //	}
-	show(1,AD);
-	for(int i = OSC_CNT-1; i > 0; i--)
+	if(App.MotorPara.CaliFlag == true)
 	{
-		App.LeftUpCS1237_TrendArray[i] = App.LeftUpCS1237_TrendArray[i - 1];
+		AD = AD + App.MotorPara.LU_HeightStress[(App.MotorPara.Current_height)-720];
+		for(int i = OSC_CNT-1; i > 0; i--)
+		{
+			App.LeftUpCS1237_TrendArray[i] = App.LeftUpCS1237_TrendArray[i - 1];
+		}
+		App.LeftUpCS1237_TrendArray[0] = AD;
 	}
-	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)
@@ -55,12 +66,24 @@ void LEFTLOW_CS1237AinADataProcessCallBack(int32_t AD)
 //	{
 //		AD = App.LeftLowCS1237_TrendArray[0];
 //	}
-	show(2,AD);
-	for(int i = OSC_CNT-1; i > 0; i--)
+	if(App.MotorPara.CaliFlag == true)
 	{
-		App.LeftLowCS1237_TrendArray[i] = App.LeftLowCS1237_TrendArray[i - 1];
+		AD = AD + App.MotorPara.LL_HeightStress[(App.MotorPara.Current_height)-720];
+		for(int i = OSC_CNT-1; i > 0; i--)
+		{
+			App.LeftLowCS1237_TrendArray[i] = App.LeftLowCS1237_TrendArray[i - 1];
+		}
+		App.LeftLowCS1237_TrendArray[0] = AD;
 	}
-	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)
@@ -69,12 +92,24 @@ void RIGHTUP_CS1237AinADataProcessCallBack(int32_t AD)
 //	{
 //		AD = App.RightUpCS1237_TrendArray[0];
 //	}
-	show(3,AD);
-	for(int i = OSC_CNT-1; i > 0; i--)
+	if(App.MotorPara.CaliFlag == true)
 	{
-		App.RightUpCS1237_TrendArray[i] = App.RightUpCS1237_TrendArray[i - 1];
+		AD = AD + App.MotorPara.RU_HeightStress[(App.MotorPara.Current_height)-720];
+		for(int i = OSC_CNT-1; i > 0; i--)
+		{
+			App.RightUpCS1237_TrendArray[i] = App.RightUpCS1237_TrendArray[i - 1];
+		}
+		App.RightUpCS1237_TrendArray[0] = AD;
 	}
-	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)
@@ -83,12 +118,24 @@ void RIGHTLOW_CS1237AinADataProcessCallBack(int32_t AD)
 //	{
 //		AD = App.RightLowCS1237_TrendArray[0];
 //	}
-	show(4,AD);
-	for(int i = OSC_CNT-1; i > 0; i--)
+	if(App.MotorPara.CaliFlag == true)
 	{
-		App.RightLowCS1237_TrendArray[i] = App.RightLowCS1237_TrendArray[i - 1];
+		AD = AD + App.MotorPara.RL_HeightStress[(App.MotorPara.Current_height)-720];
+		for(int i = OSC_CNT-1; i > 0; i--)
+		{
+			App.RightLowCS1237_TrendArray[i] = App.RightLowCS1237_TrendArray[i - 1];
+		}
+		App.RightLowCS1237_TrendArray[0] = AD;
 	}
-	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)
@@ -222,8 +269,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 	
 	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]);
-	int32_t RightDiffSum = abs(ru_diff[0]) + abs(ru_diff[1]) + abs(ru_diff[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);
 	int32_t LuSideDiff = lu_ad[0] - lu_ad[5];
@@ -288,27 +335,35 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 				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)))
+				&& ((Olu_diff3+Oru_diff3) < -(App.ADTrack.Stop_VPT))
+				)
 				{
-					BDC_UP();
-					App.CaliZeroFlag = false;
-					App.UpFlag = true;
-					App.StopFlag = false;
-					App.DownFlag = false;
-					App.EmeFlag = false;
-					App.RunCollDelay1mSCnt = RUN_TIME_MS;
+					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;
+					}
 				}
 				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))
+				&& (Olu_diff3+Oru_diff3) > (App.ADTrack.Stop_VPT)
+				)
 				{
-					BDC_DOWN();
-					App.CaliZeroFlag = false;
-					App.UpFlag = false;
-					App.StopFlag = false;
-					App.DownFlag = true;
-					App.EmeFlag = false;
-					App.RunCollDelay1mSCnt = RUN_TIME_MS;
+					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(RightDiff > LeftDiff)
@@ -316,27 +371,35 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 				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))
+				&& (Olu_diff3+Oru_diff3) < -(App.ADTrack.Stop_VPT)
+				)
 				{
-					BDC_UP();
-					App.CaliZeroFlag = false;
-					App.UpFlag = true;
-					App.StopFlag = false;
-					App.DownFlag = false;
-					App.EmeFlag = false;
-					App.RunCollDelay1mSCnt = RUN_TIME_MS;
+					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;
+					}
 				}
 				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))
+				&& (Olu_diff3+Oru_diff3) > (App.ADTrack.Stop_VPT)
+				)
 				{
-					BDC_DOWN();
-					App.CaliZeroFlag = false;					
-					App.UpFlag = false;
-					App.StopFlag = false;
-					App.DownFlag = true;
-					App.EmeFlag = false;
-					App.RunCollDelay1mSCnt = RUN_TIME_MS;
+					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;
+					}
 				}
 			}
 		}
@@ -351,9 +414,12 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 //			&& (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*5/6))	//ͨ¹ý²îÖµÅжÏÉÏÏÂÀ­·½Ïò
+			   (Olu_diff1 >= (App.ADTrack.LeftUpVPT*5/6))	//ͨ¹ý²îÖµÅжÏÉÏÏÂÀ­·½Ïò
 			&& (Olu_diff2 >= (App.ADTrack.LeftUpVPT*5/6))
-			&& (Olu_diff3 >= (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) 
@@ -369,24 +435,30 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 						&& (abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT)
 						&& (abs(Olu_diff1) > abs(Oru_diff1) + App.ADTrack.LeftUpShieldVPT))
 						{
-							BDC_DOWN();
-							App.CaliZeroFlag = false;
-							App.UpFlag = false;
-							App.DownFlag = true;
-							App.StopFlag = false;
-							App.EmeFlag = false;
-							App.RunCollDelay1mSCnt = RUN_TIME_MS;
+							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
 					{
-							BDC_DOWN();
-							App.CaliZeroFlag = false;
-							App.UpFlag = false;
-							App.DownFlag = true;
-							App.StopFlag = false;
-							App.EmeFlag = false;
-							App.RunCollDelay1mSCnt = RUN_TIME_MS;
+						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;
+						}
 					}
 				}
 			}
@@ -398,9 +470,13 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 //			&& (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*5/6))//ͨ¹ý²îÖµÅжÏÉÏÏÂÀ­·½Ïò
+				 (Olu_diff1 <= -(App.ADTrack.LeftUpVPT*5/6))//ͨ¹ý²îÖµÅжÏÉÏÏÂÀ­·½Ïò
 			&& (Olu_diff2 <= -(App.ADTrack.LeftUpVPT*5/6))
-			&& (Olu_diff3 <= -(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) 
@@ -416,24 +492,30 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 						&& (abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT)
 						&& (abs(Olu_diff1) > abs(Oru_diff1) + App.ADTrack.LeftUpShieldVPT))
 						{
-							BDC_UP();
-							App.CaliZeroFlag = false;
-							App.UpFlag = true;
-							App.DownFlag = false;
-							App.StopFlag = false;
-							App.EmeFlag = false;
-							App.RunCollDelay1mSCnt = RUN_TIME_MS;
+							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
 					{
-							BDC_UP();
-							App.CaliZeroFlag = false;
-							App.UpFlag = true;
-							App.DownFlag = false;
-							App.StopFlag = false;
-							App.EmeFlag = false;
-							App.RunCollDelay1mSCnt = RUN_TIME_MS;
+							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;
+							}
 					}
 				}
 			}
@@ -449,9 +531,12 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 //			&& (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*5/6))				//ͨ¹ý²îÖµÅжÏÉÏÏÂÀ­·½Ïò
+				 (Oru_diff1 >= (App.ADTrack.RightUpVPT*5/6))				//ͨ¹ý²îÖµÅжÏÉÏÏÂÀ­·½Ïò
 			&& (Oru_diff2 >= (App.ADTrack.RightUpVPT*5/6))
-			&& (Oru_diff3 >= (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) 
@@ -466,6 +551,22 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 						&& (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))
+						{
+							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.MotorPara.Current_height > 725)
 						{
 							BDC_DOWN();		
 							App.CaliZeroFlag = false;
@@ -476,16 +577,6 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 							App.RunCollDelay1mSCnt = RUN_TIME_MS;
 						}
 					}
-					else
-					{
-						BDC_DOWN();		
-						App.CaliZeroFlag = false;
-						App.DownFlag = true;
-						App.StopFlag = false;
-						App.UpFlag = false;
-						App.EmeFlag = false;
-						App.RunCollDelay1mSCnt = RUN_TIME_MS;
-					}
 				}
 			}
 			else if((
@@ -498,7 +589,10 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 //			&& (App.RightLowCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT))|| (
 			 (Oru_diff1 <= -(App.ADTrack.RightUpVPT*5/6))		//ͨ¹ý²îÖµÅжÏÉÏÏÂÀ­·½Ïò
 			&& (Oru_diff2 <= -(App.ADTrack.RightUpVPT*5/6))
-			&& (Oru_diff3 <= -(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) 
@@ -513,6 +607,22 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 						&& (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))
+						{
+							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(App.MotorPara.Current_height < 1195)
 						{
 							BDC_UP();
 							App.CaliZeroFlag = false;
@@ -523,16 +633,6 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 							App.RunCollDelay1mSCnt = RUN_TIME_MS;
 						}
 					}
-					else
-					{
-						BDC_UP();
-						App.CaliZeroFlag = false;
-						App.UpFlag = true;
-						App.StopFlag = false;
-						App.DownFlag = false;
-						App.EmeFlag = false;
-						App.RunCollDelay1mSCnt = RUN_TIME_MS;
-					}
 				}
 			}
 		}
@@ -542,7 +642,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 		if(App.LeftRunFalg == true)
 		{
 			if(((App.CentreRunFalg == false ) && (lu_slope > App.ADTrack.Slope_VPT))
-			|| ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0)))
+			|| ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0))
+			|| (App.MotorPara.Current_height <= 725))
 			{
 				BDC_STOP();
 				App.StopFlag = true;
@@ -557,8 +658,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 				|| ((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))
-				))
+				&& ((Olu_diff3 + Oru_diff3) < (App.ADTrack.Stop_VPT*2/3)))
+				|| (App.MotorPara.Current_height <= 725))
 			{
 				BDC_STOP();
 				App.StopFlag = true;
@@ -569,23 +670,24 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 				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((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 ) && (ru_slope > App.ADTrack.Slope_VPT))
-			|| ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0)))
+			|| ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0))
+			|| (App.MotorPara.Current_height <= 725))
 			{
 				BDC_STOP();
 				App.StopFlag = true;
@@ -600,8 +702,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 				|| ((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))
-				))
+				&& ((Olu_diff3 + Oru_diff3) < (App.ADTrack.Stop_VPT*2/3)))
+				|| (App.MotorPara.Current_height <= 725))
 			{
 				BDC_STOP();
 				App.StopFlag = true;
@@ -612,18 +714,18 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 				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(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)
@@ -631,7 +733,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 		if(App.LeftRunFalg == true)
 		{
 			if(((App.CentreRunFalg == false ) && (lu_slope < -App.ADTrack.Slope_VPT))
-			|| ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0)))
+			|| ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0))
+			|| (App.MotorPara.Current_height >= 1195))
 			{
 				BDC_STOP();
 				App.StopFlag = true;
@@ -646,8 +749,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 				|| ((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))
-				))
+				&& ((Olu_diff3 + Oru_diff3) > -(App.ADTrack.Stop_VPT*2/3)))
+				|| (App.MotorPara.Current_height >= 1195))
 			{
 				BDC_STOP();
 				App.StopFlag = true;
@@ -658,23 +761,24 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 				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((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 ) && (ru_slope < -App.ADTrack.Slope_VPT))
-			|| ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0)))
+			|| ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0))
+			|| (App.MotorPara.Current_height >= 1195))
 			{
 				BDC_STOP();
 				App.StopFlag = true;
@@ -689,8 +793,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 				|| ((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))
-				))
+				&& ((Olu_diff3 + Oru_diff3) > -(App.ADTrack.Stop_VPT*2/3)))
+				|| (App.MotorPara.Current_height >= 1195))
 			{
 				BDC_STOP();
 				App.StopFlag = true;
@@ -701,18 +805,18 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 				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(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;
+//			}
 		}
 	}
 	
@@ -750,7 +854,10 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 		&& App.RightLowCS1237_SlopeArray[2] < FOLLOW_VPT
 		&& App.RightLowCS1237_SlopeArray[2] > -FOLLOW_VPT) 
 	{
-		if(App.StopFlag == true && App.StopCollDelay1mSCnt <= 0){
+		if(App.StopFlag == true 
+		&& App.StopCollDelay1mSCnt <= 0 
+		&& App.MotorPara.Current_height <= 1995
+		&& 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];
@@ -793,8 +900,10 @@ static void AppInit(void)
 	
 	BDC_STOP();
 	
-	App.Status = APP_STATUS_WAIT;
-	App.TD1mSDelayCnt = 1000;
+//	App.Status = APP_STATUS_WAIT;
+	App.Status = APP_STATUS_MOTOR_INIT;
+	App.MotorPara.InitialFlag = false;
+//	App.TD1mSDelayCnt = 1000;
 //	App.Status = APP_STATUS_UP1;
 //	App.TD1mSDelayCnt = TRAVEL_TIME_MS;
 	App.RStatus = READ_STATUS_START;
@@ -915,88 +1024,40 @@ static void AppLoopHandler(void)
 			App.Status = APP_STATUS_IDLE;
 		break;}
 		
-		case APP_STATUS_UP1:{
-			if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-500))
-			{
-				BDC_UP();
-			}
-			else if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-2500) && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-500))
-			{
-				BDC_STOP();
-			}
-			else if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-25000) && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-3000))
-			{
-				BDC_UP();
-			}
-			else if(App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-25000))
-			{
-				BDC_STOP();
-				App.Status = APP_STATUS_DOWN;
-				App.TD1mSDelayCnt = TRAVEL_TIME_MS;
-			}
+		case APP_STATUS_MOTOR_INIT:{
+			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_DOWN:{
-			if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-500))
-			{
-				BDC_DOWN();
-			}
-			else if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-1500) && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-500))
-			{
-				BDC_STOP();
-			}
-			else if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-2500) && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-1500))
-			{
-				BDC_DOWN();
-			}
-			else if(App.TD1mSDelayCnt >= (TRAVEL_TIME_MS-26100) && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-26000))
-			{
-				BDC_STOP();
-			}
-			else if(App.TD1mSDelayCnt > 0 && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-27100))
-			{
-				BDC_DOWN();
-			}
-			else if(App.TD1mSDelayCnt <= 0)
-			{
-				BDC_STOP();
-				App.Status = APP_STATUS_UP2;
-				App.TD1mSDelayCnt = TRAVEL_TIME_MS;
-			}
-		break;}
-		
-		case APP_STATUS_UP2:{
-			if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-500))
-			{
-				BDC_UP();
-			}
-			else if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-2500) && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-500))
-			{
-				BDC_STOP();
-			}
-			else if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-25000) && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-3000))
-			{
-				BDC_UP();
-			}
-			else if(App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-25000))
-			{
-				BDC_STOP();
-				App.Status = APP_STATUS_WAIT;
-				App.TD1mSDelayCnt = 1000;
-			}
-		break;}
-				
-		case APP_STATUS_WAIT:{
+		case APP_STATUS_WAIT_MOTOR_INIT:{
 			if(App.TD1mSDelayCnt <= 0)
 			{
-				App.WaitFlag = true;
-				App.SideFlag = false;
-				App.CaliZeroFlag = false;
-				App.UpFlag = false;
-				App.DownFlag = false;
-				App.EmeFlag = false;
+				BDC_STOP();
 				App.StopFlag = true;
-				
+				App.DownFlag = false;
+				App.UpFlag = false;
+				App.Status = APP_STATUS_IDLE;
+			}
+			if(App.MotorPara.Current_height <= 721 && App.MotorPara.Current_height >= 720)
+			{
+				BDC_STOP();
+				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];
@@ -1007,8 +1068,98 @@ static void AppLoopHandler(void)
 				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();
+					//App.Status = APP_STATUS_WAIT_STEADY;
+			}
+		break;}
+				
+		case APP_STATUS_WAIT_MOTOR_CALI_UP:{
+			if(App.MotorPara.Current_height >= 1204)
+			{
+				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 >= 488)
+			{
+				BDC_STOP();
+				App.StopFlag = true;
+				App.DownFlag = false;
+				App.UpFlag = false;
+				App.MotorPara.InitialFlag = false;
+				App.MotorPara.CaliFlag = true;
+				DBG_LOG("===================LU Data monitoring====================");
+				fixWithMedianWindow(App.MotorPara.LU_HeightStress,sizeof(App.MotorPara.LU_HeightStress)/4,7,2000);
+				DBG_LOG("===================LL Data monitoring====================");
+				fixWithMedianWindow(App.MotorPara.LL_HeightStress,sizeof(App.MotorPara.LL_HeightStress)/4,7,2000);
+				DBG_LOG("===================RU Data monitoring====================");
+				fixWithMedianWindow(App.MotorPara.RU_HeightStress,sizeof(App.MotorPara.RU_HeightStress)/4,7,2000);
+				DBG_LOG("===================RL Data monitoring====================");
+				fixWithMedianWindow(App.MotorPara.RL_HeightStress,sizeof(App.MotorPara.RL_HeightStress)/4,7,2000);
+				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 <= 721 && App.MotorPara.Current_height >= 720)
+			{
+				BDC_STOP();
+				App.StopFlag = true;
+				App.DownFlag = false;
+				App.UpFlag = false;
+				Ddl_Delay1ms(1000);
+				App.UpFlag = true;
+				App.StopFlag = false;
+				App.Status = APP_STATUS_WAIT_MOTOR_CALI_UP;
+				BDC_UP();
+			}
+			else if(App.MotorPara.CaliNum >= 488)
+			{
+				BDC_STOP();
+				App.StopFlag = true;
+				App.DownFlag = false;
+				App.UpFlag = false;
+				App.MotorPara.InitialFlag = false;
+				App.MotorPara.CaliFlag = true;
+				DBG_LOG("===================LU Data monitoring====================\r\n");
+				fixWithMedianWindow(App.MotorPara.LU_HeightStress,sizeof(App.MotorPara.LU_HeightStress)/4,7,2000);
+				DBG_LOG("===================LL Data monitoring====================\r\n");
+				fixWithMedianWindow(App.MotorPara.LL_HeightStress,sizeof(App.MotorPara.LL_HeightStress)/4,7,2000);
+				DBG_LOG("===================RU Data monitoring====================\r\n");
+				fixWithMedianWindow(App.MotorPara.RU_HeightStress,sizeof(App.MotorPara.RU_HeightStress)/4,7,2000);
+				DBG_LOG("===================RL Data monitoring====================\r\n");
+				fixWithMedianWindow(App.MotorPara.RL_HeightStress,sizeof(App.MotorPara.RL_HeightStress)/4,7,2000);
+				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)
+			{
+				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_IDLE;
-				App.TD1mSDelayCnt = 0;
 			}
 		break;}
 	}
@@ -1021,6 +1172,8 @@ static void App1mSRoutine(void)
 		App.Read1mSDelayCnt--;
 	if(App.Uart1Delay1mSCnt > 0)
 		App.Uart1Delay1mSCnt--;
+	if(App.Uart3RxTimeOut1mSCnt > 0)
+		App.Uart3RxTimeOut1mSCnt--;
 	if(App.StopCollDelay1mSCnt > 0)
 		App.StopCollDelay1mSCnt--;
 	if(App.RunCollDelay1mSCnt > 0)
@@ -1065,7 +1218,6 @@ int main(void)
 		AppLoopHandler();
 	}
 }
-
 /*****************************************************************************************
 * º¯ÊýÃû³Æ: SysTick_IrqHandler
 * ¹¦ÄÜÃèÊö: ϵͳµÎ´ðÖжÏ
@@ -1098,6 +1250,71 @@ void DBG_RxPinIntCallBack(void)
 
 
 
+#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 == 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] == 0x01)
+			{
+				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 <= 1210)
+				{//»º´æÿ¸ö¸ß¶Èµã¶ÔÓÚÁãµãÓ¦Á¦µÄ²îÖµ£¬¶Ô²»Í¬¸ß¶ÈµãµÄÓ¦Á¦½øÐв¹³¥
+					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.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];
+						App.MotorPara.CaliNum++;
+					}
+				}
+			}
+			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);