From 2657c1ca27aefb14a357c3708f838a9a61a62247 Mon Sep 17 00:00:00 2001 From: "DESKTOP-KQHDCM4\\97555" <975559679@qq.com> Date: Thu, 26 Mar 2026 10:11:46 +0800 Subject: [PATCH] =?UTF-8?q?=E9=80=9A=E8=BF=87=E9=AB=98=E5=BA=A6=E6=A0=A1?= =?UTF-8?q?=E5=87=86=E5=BA=94=E5=8A=9B=E5=88=9D=E7=89=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Project/HC32F460/MDK/HC32F460.uvoptx | 25 +- .../source/Module/CS1237/CS1237_LEFTLOW.c | 4 +- Project/HC32F460/source/User/Inc/Algorithm.h | 1 + Project/HC32F460/source/User/Inc/bsp.h | 99 ++- Project/HC32F460/source/User/Inc/main.h | 39 +- Project/HC32F460/source/User/Src/Algorithm.c | 108 +++ Project/HC32F460/source/User/Src/bsp.c | 116 ++- Project/HC32F460/source/User/Src/main.c | 711 ++++++++++++------ 8 files changed, 794 insertions(+), 309 deletions(-) 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 @@ 14 1 - App.ADTrack.Slope_VPT,0x0A + App.MotorPara.LU_HeightStress,0x0A 15 1 - App.RightUpCS1237_SlopeArray[0],0x0A + App.MotorPara.LL_HeightStress,0x0A 16 1 - App.ADTrack.Stop_VPT,0x0A + App.MotorPara.RU_HeightStress,0x0A 17 1 - Olu_diff1,0x0A + App.MotorPara.RL_HeightStress 18 1 - Oru_diff1,0x0A + App.TD1mSDelayCnt,0x0A 19 1 - Olu_diff2,0x0A + App.RxUart3Data.RxBuff,0x10 20 1 - Oru_diff2,0x0A + App.RxUart3Data.RxLen,0x0A 21 1 - Olu_diff3,0x0A + App.MotorPara.Current_height,0x0A 22 1 - Oru_diff3,0x0A + App.Status 23 1 - App.LeftUpCS1237_SlopeArray[0],0x0A - - - 24 - 1 - App.RightUpCS1237_SlopeArray[0],0x0A + App.MotorPara.CaliNum,0x0A 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 +#include #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);