From 26cccd60a03c547bfd3010c41d6929a24be6c132 Mon Sep 17 00:00:00 2001 From: "DESKTOP-KQHDCM4\\97555" <975559679@qq.com> Date: Sun, 7 Jun 2026 12:31:21 +0800 Subject: [PATCH] =?UTF-8?q?=E5=88=9D=E7=89=88=E8=B0=83=E8=AF=95=E5=AE=8C?= =?UTF-8?q?=E6=88=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Project/MDK/HC32F460.uvoptx | 125 +-- Project/MDK/HC32F460.uvprojx | 18 +- Project/source/Module/BeepAndLed/BeepAndLed.c | 177 +++++ Project/source/Module/BeepAndLed/BeepAndLed.h | 39 + Project/source/User/Inc/bsp.h | 4 +- Project/source/User/Inc/main.h | 56 +- Project/source/User/Src/main.c | 730 ++++++------------ 7 files changed, 504 insertions(+), 645 deletions(-) create mode 100644 Project/source/Module/BeepAndLed/BeepAndLed.c create mode 100644 Project/source/Module/BeepAndLed/BeepAndLed.h diff --git a/Project/MDK/HC32F460.uvoptx b/Project/MDK/HC32F460.uvoptx index 68f0518..9c3cfd4 100644 --- a/Project/MDK/HC32F460.uvoptx +++ b/Project/MDK/HC32F460.uvoptx @@ -103,7 +103,7 @@ 1 0 0 - 12 + 3 @@ -145,7 +145,7 @@ 0 CMSIS_AGDI - -X"Any" -UAny -O206 -S0 -C0 -P00000000 -N00("") -D00(00000000) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD1FFF8000 -FC1000 -FN2 -FF0HC32F460_512K.FLM -FS00 -FL080000 -FP0($$Device:HC32F460JETA$FlashARM\HC32F460_512K.FLM) -FF1HC32F460_otp.FLM -FS13000C00 -FL13FC -FP1($$Device:HC32F460JETA$FlashARM\HC32F460_otp.FLM) + -X"Any" -UAny -O206 -S0 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD1FFF8000 -FC1000 -FN2 -FF0HC32F460_512K.FLM -FS00 -FL080000 -FP0($$Device:HC32F460JETA$FlashARM\HC32F460_512K.FLM) -FF1HC32F460_otp.FLM -FS13000C00 -FL13FC -FP1($$Device:HC32F460JETA$FlashARM\HC32F460_otp.FLM) 0 @@ -153,40 +153,7 @@ UL2CM3(-S0 -C0 -P0 -FD1FFF8000 -FC1000 -FN2 -FF0HC32F460_512K -FS00 -FL080000 -FF1HC32F460_otp -FS13000C00 -FL13FC -FP0($$Device:HC32F460JETA$FlashARM\HC32F460_512K.FLM) -FP1($$Device:HC32F460JETA$FlashARM\HC32F460_otp.FLM)) - - - 0 - 0 - 1118 - 1 -
4658
- 0 - 0 - 0 - 0 - 0 - 1 - ..\source\User\Src\main.c - - \\BombSensor\../source/User/Src/main.c\1118 - - - 1 - 0 - 1156 - 1 -
5298
- 0 - 0 - 0 - 0 - 0 - 1 - ..\source\User\Src\main.c - - \\BombSensor\../source/User/Src/main.c\1156 - - + 0 @@ -251,97 +218,57 @@ 12 1 - App.SlopeCollDelay1mSCnt,0x0A + App.EmergencyDelay1mSCnt,0x0A 13 1 - App.EmergencyDelay1mSCnt,0x0A + App.MotorPara.Current_height,0x0A 14 1 - App.MotorPara.LU_HeightStress,0x0A + App.RightUpCS1237_SlopeArray[0],0x0A 15 1 - App.MotorPara.LL_HeightStress,0x0A + App.RightLowCS1237_SlopeArray[0],0x0A 16 1 - App.MotorPara.RU_HeightStress,0x0A + Olu_diff1,0x0A 17 1 - App.MotorPara.RL_HeightStress,0x0A + Olu_diff2,0x0A 18 1 - App.TD1mSDelayCnt,0x0A + Olu_diff3,0x0A 19 1 - App.RxUart3Data.RxBuff,0x10 + Oru_diff1,0x0A 20 1 - App.RxUart3Data.RxLen,0x0A + Oru_diff2,0x0A 21 1 - App.Status + Oru_diff3,0x0A 22 1 - App.MotorPara.CaliNum,0x0A - - - 23 - 1 - App.CailUp1mSCnt,0x0A - - - 24 - 1 - Oru_diff1,0x0A - - - 25 - 1 - Orl_diff1,0x0A - - - 26 - 1 - App.ADTrack.Stop_VPT,0x0A - - - 27 - 1 - ru_ad[0],0x0A - - - 28 - 1 - App.MotorPara.RestFlag,0x0A - - - 29 - 1 - App.MotorPara.Current_height,0x0A - - - 30 - 1 - App.HeightContRolCnt,0x0A + App.CentCollDelay1mSCnt,0x0A @@ -1010,7 +937,7 @@ CS1237 - 1 + 0 0 0 0 @@ -1065,7 +992,7 @@ - dc_motor + BeepAndLed 1 0 0 @@ -1077,6 +1004,26 @@ 0 0 0 + ..\source\Module\BeepAndLed\BeepAndLed.c + BeepAndLed.c + 0 + 0 + + + + + dc_motor + 1 + 0 + 0 + 0 + + 6 + 55 + 1 + 0 + 0 + 0 ..\source\Module\dc_motor\dc_motor.c dc_motor.c 0 diff --git a/Project/MDK/HC32F460.uvprojx b/Project/MDK/HC32F460.uvprojx index 08478fa..ff63e43 100644 --- a/Project/MDK/HC32F460.uvprojx +++ b/Project/MDK/HC32F460.uvprojx @@ -10,14 +10,14 @@ Release 0x4 ARM-ADS - 5060960::V5.06 update 7 (build 960)::ARMCC - 5060960::V5.06 update 7 (build 960)::ARMCC + 5060960::V5.06 update 7 (build 960)::.\ARMCC + 5060960::V5.06 update 7 (build 960)::.\ARMCC 0 HC32F460JETA HDSC - HDSC.HC32F460.1.0.12 + HDSC.HC32F460.1.0.11 https://raw.githubusercontent.com/hdscmcu/pack/master/ IRAM(0x1FFF8000,0x2F000) IRAM2(0x200F0000,0x1000) IROM(0x00000000,0x80000) IROM2(0x03000C00,0x003FC) CPUTYPE("Cortex-M4") FPU2 CLOCK(12000000) ELITTLE @@ -341,7 +341,7 @@ __DEBUG,HC32F460,USE_DEVICE_DRIVER_LIB,JEUA - ..\..\mcu\common;..\source\Module\CS1237;..\source\Module\dc_motor;..\source\User\Src;..\source\User\Inc;..\..\driver\src;..\..\driver\inc + ..\..\mcu\common;..\source\Module\CS1237;..\source\Module\dc_motor;..\source\User\Src;..\source\User\Inc;..\..\driver\src;..\..\driver\inc;..\source\Module\BeepAndLed @@ -667,6 +667,16 @@ + + BeepAndLed + + + BeepAndLed.c + 1 + ..\source\Module\BeepAndLed\BeepAndLed.c + + + dc_motor diff --git a/Project/source/Module/BeepAndLed/BeepAndLed.c b/Project/source/Module/BeepAndLed/BeepAndLed.c new file mode 100644 index 0000000..089160e --- /dev/null +++ b/Project/source/Module/BeepAndLed/BeepAndLed.c @@ -0,0 +1,177 @@ +#include "BeepAndLed.h" +#include "beepAndLed.h" +#include "main.h" // 包含BUZZER_ON/OFF, LED等宏定义 + +// ========== 变量定义 ========== +static BeeperCtrl_t g_beeper = { + .state = BEEPER_STATE_IDLE, + .is_active = 0, + .repeat = 0, + .repeat_done = 0, + .on_ms = 0, + .off_ms = 0 +}; + +// 延时计数器（供滴答定时器使用） +uint32_t BeepAndLedCollDelay1mSCnt = 0; + +// ========== 硬件控制函数（根据您的实际宏定义修改）========== +static void Beeper_HW_On(void) +{ + BUZZER_ON(); // 请替换为您的实际宏 + LED_GREEN_OFF(); // 请替换为您的实际宏 + LED_RED_ON(); // 请替换为您的实际宏 +} + +static void Beeper_HW_Off(void) +{ + BUZZER_OFF(); // 请替换为您的实际宏 + LED_GREEN_ON(); // 请替换为您的实际宏 + LED_RED_OFF(); // 请替换为您的实际宏 +} + +// ========== 实现函数 ========== + +/** + * @brief 蜂鸣器初始化 + */ +void Beeper_Init(void) +{ + Beeper_Stop(); +} + +/** + * @brief 开始鸣响（非阻塞） + * @param on_ms 鸣响时间(ms) + * @param off_ms 间隔时间(ms) + * @param repeat 重复次数（0表示无效） + * + * @example Beeper_Start(200, 100, 3); // 鸣200ms，停100ms，重复3次 + */ +void Beeper_Start(uint16_t on_ms, uint16_t off_ms, uint8_t repeat) +{ + if (repeat == 0) return; + + // 如果已经在鸣响，可以选择停止并重新开始 + if (g_beeper.is_active) { + Beeper_Stop(); // 停止当前，重新开始新的 + // return; // 或者选择忽略新请求 + } + + // 保存参数 + g_beeper.on_ms = on_ms; + g_beeper.off_ms = off_ms; + g_beeper.repeat = repeat; + g_beeper.repeat_done = 0; + g_beeper.is_active = 1; + + // 开启蜂鸣器 + Beeper_HW_On(); + + // 设置延时计数器（开启状态延时） + BeepAndLedCollDelay1mSCnt = on_ms; + g_beeper.state = BEEPER_STATE_ON; +} + +/** + * @brief 停止鸣响 + */ +void Beeper_Stop(void) +{ + // 关闭硬件 + Beeper_HW_Off(); + + // 重置状态机 + g_beeper.state = BEEPER_STATE_IDLE; + g_beeper.is_active = 0; + g_beeper.repeat = 0; + g_beeper.repeat_done = 0; + BeepAndLedCollDelay1mSCnt = 0; +} + +/** + * @brief 蜂鸣器状态机更新（放在main.c的while(1)中循环调用） + * @note 必须在滴答定时器更新BeepAndLedCollDelay1mSCnt后才能正确工作 + */ +void BeeperLoopHandler(void) +{ + if (!g_beeper.is_active) { + return; + } + + switch (g_beeper.state) { + case BEEPER_STATE_ON: + // 蜂鸣器开启状态，等待计数器归零 + if (BeepAndLedCollDelay1mSCnt == 0) { + // 延时结束，关闭蜂鸣器 + Beeper_HW_Off(); + + g_beeper.repeat_done++; + + // 检查是否完成所有重复 + if (g_beeper.repeat_done >= g_beeper.repeat) { + // 全部完成，结束 + g_beeper.is_active = 0; + g_beeper.state = BEEPER_STATE_IDLE; + } else { + // 还有下次，进入等待状态 + BeepAndLedCollDelay1mSCnt = g_beeper.off_ms; + g_beeper.state = BEEPER_STATE_OFF; + } + } + break; + + case BEEPER_STATE_OFF: + // 蜂鸣器关闭等待状态（间隔时间） + if (BeepAndLedCollDelay1mSCnt == 0) { + // 间隔结束，开启下一次鸣响 + Beeper_HW_On(); + + BeepAndLedCollDelay1mSCnt = g_beeper.on_ms; + g_beeper.state = BEEPER_STATE_ON; + } + break; + + case BEEPER_STATE_WAIT: + case BEEPER_STATE_IDLE: + default: + break; + } +} + +/** + * @brief 查询是否正在鸣响 + * @return true:正在鸣响, false:空闲 + */ +bool Beeper_IsPlaying(void) +{ + return g_beeper.is_active; +} + +/** + * @brief 手动开启蜂鸣器（不计入状态机） + */ +void Beeper_On(void) +{ + Beeper_HW_On(); +} + +/** + * @brief 手动关闭蜂鸣器（不计入状态机） + */ +void Beeper_Off(void) +{ + Beeper_HW_Off(); +} + +/** + * @brief 清除延时计数器（由滴答定时器调用） + * @note 这个函数放在滴答定时器的1ms中断中调用 + */ +void Beeper_ClearDelay(void) +{ + if (BeepAndLedCollDelay1mSCnt > 0) { + BeepAndLedCollDelay1mSCnt--; + } +} + diff --git a/Project/source/Module/BeepAndLed/BeepAndLed.h b/Project/source/Module/BeepAndLed/BeepAndLed.h new file mode 100644 index 0000000..bd8fded --- /dev/null +++ b/Project/source/Module/BeepAndLed/BeepAndLed.h @@ -0,0 +1,39 @@ +#ifndef __BEEPANDLED_H +#define __BEEPANDLED_H + +#include +#include +#include "bsp.h" + +// ========== 蜂鸣器状态机状态定义 ========== +typedef enum { + BEEPER_STATE_IDLE = 0, // 空闲 + BEEPER_STATE_ON, // 蜂鸣器开启延时中 + BEEPER_STATE_OFF, // 蜂鸣器关闭延时中 + BEEPER_STATE_WAIT // 等待下一次鸣响 +} BeeperState_t; + +// ========== 蜂鸣器控制结构体 ========== +typedef struct { + BeeperState_t state; // 当前状态 + uint16_t on_ms; // 鸣响时间(ms) + uint16_t off_ms; // 间隔时间(ms) + uint8_t repeat; // 重复次数 + uint8_t repeat_done; // 已完成次数 + uint8_t is_active; // 是否激活 +} BeeperCtrl_t; + +// ========== 外部可访问变量 ========== +extern uint32_t BeepAndLedCollDelay1mSCnt; // 延时计数器（在beepAndLed.c中定义） + +// ========== API函数 ========== +void Beeper_Init(void); // 初始化 +void Beeper_Start(uint16_t on_ms, uint16_t off_ms, uint8_t repeat); // 开始鸣响（非阻塞） +void Beeper_Stop(void); // 停止鸣响 +void BeeperLoopHandler(void); // 状态机更新（放在while(1)中） +bool Beeper_IsPlaying(void); // 查询是否正在鸣响 +void Beeper_On(void); // 手动开启蜂鸣器 +void Beeper_Off(void); // 手动关闭蜂鸣器 +void Beeper_ClearDelay(void); // 清除延时计数器（供滴答定时器使用） +#endif /* _BEEP_AND_LED_H_ */ + diff --git a/Project/source/User/Inc/bsp.h b/Project/source/User/Inc/bsp.h index fd2ddde..270cfe8 100644 --- a/Project/source/User/Inc/bsp.h +++ b/Project/source/User/Inc/bsp.h @@ -221,12 +221,12 @@ //< LED #define LED_GREEN_PORTx (PortA) -#define LED_GREEN_PINx (Pin09) +#define LED_GREEN_PINx (Pin08) #define LED_GREEN_ON() PORT_SetBits(LED_GREEN_PORTx, LED_GREEN_PINx)//拉高 #define LED_GREEN_OFF() PORT_ResetBits(LED_GREEN_PORTx, LED_GREEN_PINx)//拉低 #define LED_RED_PORTx (PortA) -#define LED_RED_PINx (Pin08) +#define LED_RED_PINx (Pin09) #define LED_RED_ON() PORT_SetBits(LED_RED_PORTx, LED_RED_PINx)//拉高 #define LED_RED_OFF() PORT_ResetBits(LED_RED_PORTx, LED_RED_PINx)//拉低 diff --git a/Project/source/User/Inc/main.h b/Project/source/User/Inc/main.h index eefb3d5..e0cc140 100644 --- a/Project/source/User/Inc/main.h +++ b/Project/source/User/Inc/main.h @@ -3,34 +3,25 @@ #include #include "bsp.h" - -#define USE_FILE_SYSTEM 1 -#define SDCARD_BLOCK_MAX (10000) - #define OSC_CNT 40 #define SLOPE_CNT 20 -#define SLOPE_VPT 1000 #define PRESS_VPT 3 #define FOLLOW_VPT 10 -#define SHIELD_VPT 5000 +#define SHIELD_VPT 8000 #define EME_STOP_VPT 4000 -#define CENTRE_VPT 2000 +#define CENTRE_VPT 8000 +#define CENTRE_SHIELD_VPT 2000 #define STOP_TIME_MS 500 +#define CENT_TIME_MS 200 #define RUN_TIME_MS 0 #define EME_START_MS 0 #define EME_STOP_MS 2000 -#define SLOPE_START_MS 3000 -#define TRAVEL_TIME_MS 30000 - -#define PEAK 1200 //1200mm -#define VALLEY 720 //720mm - -#define BDC_STOP() (SET_TWIN_BIT0(),SET_TWIN_BIT1()) -#define BDC_DOWN() (SET_TWIN_BIT0(),CLR_TWIN_BIT1()) -#define BDC_UP() (SET_TWIN_BIT1(),CLR_TWIN_BIT0()) +#define BDC_STOP() (SET_TWIN_BIT0(),SET_TWIN_BIT1()) +#define BDC_DOWN() (SET_TWIN_BIT0(),CLR_TWIN_BIT1()) +#define BDC_UP() (SET_TWIN_BIT1(),CLR_TWIN_BIT0()) //#define BDC_STOP() Motor_Control(MOTOR_CMD_STOP) //#define BDC_UP() Motor_Control(MOTOR_CMD_FORWARD) @@ -47,7 +38,6 @@ typedef enum { /*设备阈值*/ typedef struct{ - int32_t Slope_VPT; int32_t Stop_VPT; int32_t LeftUpVPT;//左上设备阈值 @@ -66,16 +56,8 @@ typedef struct{ int32_t RightLow_Org_AdZero;//右下设备原始AD零点 }ADTrack_t; typedef struct{//电机参数 - bool InitialFlag;//初始标志位 bool RestFlag; - bool CaliFlag; - bool LoopOne;//循环一次标志位 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 { @@ -97,22 +79,9 @@ typedef enum { APP_STATUS_IDLE, APP_STATUS_ON, APP_STATUS_OFF, - 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, - APP_STATUS_HEIGHT_CONTROL, }AppStatus_m; -typedef struct{ - __IO uint16_t rPayLen; - __IO bool HeaderFrame; - __IO uint16_t RxLen; - uint8_t RxBuff[10]; -}Uart1Rx_t; - typedef struct{ __IO uint16_t rPayLen; __IO bool HeaderFrame; @@ -124,25 +93,20 @@ typedef struct { AppStatus_m Status;//状态 ReadStatus_m RStatus; AppPara_T Para; - Uart1Rx_t RxUart1Data; Uart3Rx_t RxUart3Data; ADTrack_t ADTrack; MotorPara_t MotorPara; uint32_t TD1mSDelayCnt; - uint32_t Read1mSDelayCnt; - uint32_t Uart1Delay1mSCnt; + uint32_t Read1mSDelayCnt; uint32_t Uart3RxTimeOut1mSCnt; uint32_t StopCollDelay1mSCnt; uint32_t RunCollDelay1mSCnt; + uint32_t CentCollDelay1mSCnt; uint32_t EmergencyDelay1mSCnt; - uint32_t SlopeCollDelay1mSCnt; - uint32_t CailUp1mSCnt; - uint32_t HeightContRolCnt; uint8_t FeedDogDlyCnt; bool CaliZeroFlag;//校零标志位 - bool SideFlag;//侧压标志位 bool UpFlag;//上升标志位 bool DownFlag;//下降标志位 bool StopFlag;//停止标志位 @@ -165,8 +129,6 @@ typedef struct { int32_t LeftLowCS1237_SlopeArray[SLOPE_CNT]; int32_t RightUpCS1237_SlopeArray[SLOPE_CNT]; int32_t RightLowCS1237_SlopeArray[SLOPE_CNT]; - - bool WaitFlag; }AppDetect_t; extern AppDetect_t App; diff --git a/Project/source/User/Src/main.c b/Project/source/User/Src/main.c index 1c354b4..e464351 100644 --- a/Project/source/User/Src/main.c +++ b/Project/source/User/Src/main.c @@ -10,6 +10,7 @@ #include "CS1237_LEFTLOW.h" #include "CS1237_RIGHTUP.h" #include "CS1237_RIGHTLOW.h" +#include "BeepAndLed.h" #define MAIN_DBG_LOG(...) DBG_LOG(__VA_ARGS__) #define MAIN_DBG_ARRAY(ARRAY, SIZE) DBG_ARRAY(ARRAY,SIZE) @@ -19,30 +20,6 @@ AppDetect_t App; -/** - * @brief 蜂鸣器阻塞式鸣响 - * @param on_ms : 每次鸣响持续时间，单位毫秒 - * @param off_ms : 两次鸣响之间的间隔时间，单位毫秒（若只鸣一声可设0） - * @param repeat : 鸣响次数（1~255），0 则直接返回（不响） - */ -static void Beeper_Play_Blocking(uint16_t on_ms, uint16_t off_ms, uint8_t repeat) -{ - if (repeat == 0) return; - - for (uint8_t i = 0; i < repeat; i++) { - BUZZER_ON(); - LED_GREEN_OFF(); - LED_RED_ON(); - delay_1ms(on_ms); - BUZZER_OFF(); - LED_GREEN_ON(); - LED_RED_OFF(); - // 最后一次鸣完不需要再间隔，否则会多延时 - if (i < repeat - 1) { - delay_1ms(off_ms); - } - } -} static void show(uint8_t ch,int32_t ad) { if(App.Para.Layout.DBGWaveOut == true) @@ -60,105 +37,42 @@ static void show(uint8_t ch,int32_t ad) } void LEFTUP_CS1237AinADataProcessCallBack(int32_t AD) { -// if(abs(AD - App.LeftUpCS1237_TrendArray[0]) > 2000000) -// { -// AD = App.LeftUpCS1237_TrendArray[0]; -// } - if(App.MotorPara.CaliFlag == true) + for(int i = OSC_CNT-1; i > 0; i--) { - 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; - } - else{ -// 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[i] = App.LeftUpCS1237_TrendArray[i - 1]; } + App.LeftUpCS1237_TrendArray[0] = AD; show(1,AD); App.LeftUpCS1237ReadEndFlag = true; } void LEFTLOW_CS1237AinADataProcessCallBack(int32_t AD) { - -// if(abs(AD - App.LeftLowCS1237_TrendArray[0]) > 2000000) -// { -// AD = App.LeftLowCS1237_TrendArray[0]; -// } - if(App.MotorPara.CaliFlag == true) + for(int i = OSC_CNT-1; i > 0; i--) { - 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; - } - else - { -// 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[i] = App.LeftLowCS1237_TrendArray[i - 1]; } + App.LeftLowCS1237_TrendArray[0] = AD; show(2,AD); App.LeftLowCS1237ReadEndFlag = true; } void RIGHTUP_CS1237AinADataProcessCallBack(int32_t AD) { -// if(abs(AD - App.RightUpCS1237_TrendArray[0]) > 2000000) -// { -// AD = App.RightUpCS1237_TrendArray[0]; -// } - if(App.MotorPara.CaliFlag == true) + + for(int i = OSC_CNT-1; i > 0; i--) { - 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; - } - else - { -// 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[i] = App.RightUpCS1237_TrendArray[i - 1]; } + App.RightUpCS1237_TrendArray[0] = AD; show(3,AD); App.RightUpCS1237ReadEndFlag = true; } void RIGHTLOW_CS1237AinADataProcessCallBack(int32_t AD) { -// if(abs(AD - App.RightLowCS1237_TrendArray[0]) > 2000000) -// { -// AD = App.RightLowCS1237_TrendArray[0]; -// } - if(App.MotorPara.CaliFlag == true) + for(int i = OSC_CNT-1; i > 0; i--) { - 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; - } - else - { -// 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[i] = App.RightLowCS1237_TrendArray[i - 1]; } + App.RightLowCS1237_TrendArray[0] = AD; show(4,AD); App.RightLowCS1237ReadEndFlag = true; } @@ -166,7 +80,6 @@ void GradeControl(ControlLevel_m level) { switch(level){ case LEVEL_1:{ - App.ADTrack.Slope_VPT = SLOPE_VPT; App.ADTrack.LeftUpVPT = 5000; App.ADTrack.LeftLowVPT = 5000; App.ADTrack.RightUpVPT = 5000; @@ -179,7 +92,18 @@ void GradeControl(ControlLevel_m level) break;} case LEVEL_2:{ - App.ADTrack.Slope_VPT = SLOPE_VPT; + App.ADTrack.LeftUpVPT = 7000; + App.ADTrack.LeftLowVPT = 7000; + App.ADTrack.RightUpVPT = 7000; + App.ADTrack.RightLowVPT = 7000; + App.ADTrack.LeftUpShieldVPT = SHIELD_VPT; + App.ADTrack.LeftLowShieldVPT = SHIELD_VPT; + App.ADTrack.RightUpShieldVPT = SHIELD_VPT; + App.ADTrack.RightLowShieldVPT = SHIELD_VPT; + App.ADTrack.Stop_VPT = 5000; + break;} + + case LEVEL_3:{ App.ADTrack.LeftUpVPT = 10000; App.ADTrack.LeftLowVPT = 10000; App.ADTrack.RightUpVPT = 10000; @@ -188,11 +112,22 @@ void GradeControl(ControlLevel_m level) App.ADTrack.LeftLowShieldVPT = SHIELD_VPT; App.ADTrack.RightUpShieldVPT = SHIELD_VPT; App.ADTrack.RightLowShieldVPT = SHIELD_VPT; - App.ADTrack.Stop_VPT = 8000; + App.ADTrack.Stop_VPT = 6000; break;} - case LEVEL_3:{ - App.ADTrack.Slope_VPT = SLOPE_VPT; + case LEVEL_4:{ + App.ADTrack.LeftUpVPT = 15000; + App.ADTrack.LeftLowVPT = 15000; + App.ADTrack.RightUpVPT = 15000; + App.ADTrack.RightLowVPT = 15000; + App.ADTrack.LeftUpShieldVPT = SHIELD_VPT; + App.ADTrack.LeftLowShieldVPT = SHIELD_VPT; + App.ADTrack.RightUpShieldVPT = SHIELD_VPT; + App.ADTrack.RightLowShieldVPT = SHIELD_VPT; + App.ADTrack.Stop_VPT = 10000; + break;} + + case LEVEL_5:{ App.ADTrack.LeftUpVPT = 20000; App.ADTrack.LeftLowVPT = 20000; App.ADTrack.RightUpVPT = 20000; @@ -201,33 +136,7 @@ void GradeControl(ControlLevel_m level) App.ADTrack.LeftLowShieldVPT = SHIELD_VPT; App.ADTrack.RightUpShieldVPT = SHIELD_VPT; App.ADTrack.RightLowShieldVPT = SHIELD_VPT; - App.ADTrack.Stop_VPT = 14000; - break;} - - case LEVEL_4:{ - App.ADTrack.Slope_VPT = SLOPE_VPT; - App.ADTrack.LeftUpVPT = 25000; - App.ADTrack.LeftLowVPT = 25000; - App.ADTrack.RightUpVPT = 25000; - App.ADTrack.RightLowVPT = 25000; - App.ADTrack.LeftUpShieldVPT = SHIELD_VPT; - App.ADTrack.LeftLowShieldVPT = SHIELD_VPT; - App.ADTrack.RightUpShieldVPT = SHIELD_VPT; - App.ADTrack.RightLowShieldVPT = SHIELD_VPT; - App.ADTrack.Stop_VPT = 19000; - break;} - - case LEVEL_5:{ - App.ADTrack.Slope_VPT = SLOPE_VPT; - App.ADTrack.LeftUpVPT = 30000; - App.ADTrack.LeftLowVPT = 30000; - App.ADTrack.RightUpVPT = 30000; - App.ADTrack.RightLowVPT = 30000; - App.ADTrack.LeftUpShieldVPT = SHIELD_VPT; - App.ADTrack.LeftLowShieldVPT = SHIELD_VPT; - App.ADTrack.RightUpShieldVPT = SHIELD_VPT; - App.ADTrack.RightLowShieldVPT = SHIELD_VPT; - App.ADTrack.Stop_VPT = 25000; + App.ADTrack.Stop_VPT = 15000; break;} } } @@ -288,7 +197,6 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ // show(3, ru_slope); // show(4, rl_slope); - for(int i = SLOPE_CNT-1; i > 0; i--) { App.LeftUpCS1237_SlopeArray[i] = App.LeftUpCS1237_SlopeArray[i - 1]; @@ -312,30 +220,31 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ 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]; - int32_t LlSideDiff = ll_ad[0] - ll_ad[5]; - int32_t RuSideDiff = ru_ad[0] - ru_ad[5]; - int32_t RlSideDiff = rl_ad[0] - rl_ad[5]; if(App.EmeFlag == true) { if(App.DownFlag == true && App.EmergencyDelay1mSCnt > 0) { BDC_UP(); + LED_RED_ON(); + LED_GREEN_OFF(); } else if(App.UpFlag == true && App.EmergencyDelay1mSCnt > 0) { BDC_DOWN(); + LED_RED_ON(); + LED_GREEN_OFF(); } else if(App.EmergencyDelay1mSCnt <= 0) { BDC_STOP(); + LED_RED_OFF(); + LED_GREEN_ON(); App.StopFlag = true; App.DownFlag = false; App.UpFlag = false; App.EmeFlag = false; App.StopCollDelay1mSCnt = STOP_TIME_MS; - App.SlopeCollDelay1mSCnt = SLOPE_START_MS; } } else if(App.StopFlag == true && App.StopCollDelay1mSCnt <= 0) @@ -369,17 +278,29 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (rl_ad[2] < App.ADTrack.RightLow_Org_AdZero-CENTRE_VPT))) { App.CentreRunFalg = true; + App.CentCollDelay1mSCnt = CENT_TIME_MS; if(LeftDiff > RightDiff) { App.LeftRunFalg = true; - if((Olu_diff1+Oru_diff1) < -(App.ADTrack.Stop_VPT) - && ((Olu_diff2+Oru_diff2) < -(App.ADTrack.Stop_VPT)) - && ((Olu_diff3+Oru_diff3) < -(App.ADTrack.Stop_VPT)) + if(((Olu_diff1+Oru_diff1) > (App.ADTrack.Stop_VPT)) + && ((Olu_diff2+Oru_diff2) > (App.ADTrack.Stop_VPT)) + && ((Olu_diff3+Oru_diff3) > (App.ADTrack.Stop_VPT)) + && ((Oll_diff1+Orl_diff1) < -(App.ADTrack.Stop_VPT)) + && ((Oll_diff2+Orl_diff2) < -(App.ADTrack.Stop_VPT)) + && ((Oll_diff3+Orl_diff3) < -(App.ADTrack.Stop_VPT)) ) { - if(App.MotorPara.Current_height < 1200) + if((abs(Olu_diff1) < (abs(Oll_diff1) + CENTRE_SHIELD_VPT)) + && (abs(Oru_diff1) < (abs(Orl_diff1) + CENTRE_SHIELD_VPT)) + && (abs(Olu_diff2) < (abs(Oll_diff2) + CENTRE_SHIELD_VPT)) + && (abs(Oru_diff2) < (abs(Orl_diff2) + CENTRE_SHIELD_VPT)) + && (abs(Olu_diff3) < (abs(Oll_diff3) + CENTRE_SHIELD_VPT)) + && (abs(Oru_diff3) < (abs(Orl_diff3) + CENTRE_SHIELD_VPT)) + )//判断是否不是正面压 { BDC_UP(); + LED_RED_ON(); + LED_GREEN_OFF(); App.CaliZeroFlag = false; App.UpFlag = true; App.StopFlag = false; @@ -388,14 +309,25 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ 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) + else if(((Olu_diff1+Oru_diff1) < -(App.ADTrack.Stop_VPT)) + && ((Olu_diff2+Oru_diff2) < -(App.ADTrack.Stop_VPT)) + && ((Olu_diff3+Oru_diff3) < -(App.ADTrack.Stop_VPT)) + && ((Oll_diff1+Orl_diff1) > (App.ADTrack.Stop_VPT)) + && ((Oll_diff2+Orl_diff2) > (App.ADTrack.Stop_VPT)) + && ((Oll_diff3+Orl_diff3) > (App.ADTrack.Stop_VPT)) ) { - if(App.MotorPara.Current_height > 730) + if((abs(Olu_diff1) > (abs(Oll_diff1) - CENTRE_SHIELD_VPT)) + && (abs(Oru_diff1) > (abs(Orl_diff1) - CENTRE_SHIELD_VPT)) + && (abs(Olu_diff2) > (abs(Oll_diff2) - CENTRE_SHIELD_VPT)) + && (abs(Oru_diff2) > (abs(Orl_diff2) - CENTRE_SHIELD_VPT)) + && (abs(Olu_diff3) > (abs(Oll_diff3) - CENTRE_SHIELD_VPT)) + && (abs(Oru_diff3) > (abs(Orl_diff3) - CENTRE_SHIELD_VPT)) + )//判断是否不是正面压 { BDC_DOWN(); + LED_RED_ON(); + LED_GREEN_OFF(); App.CaliZeroFlag = false; App.UpFlag = false; App.StopFlag = false; @@ -408,30 +340,52 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ else if(RightDiff > LeftDiff) { App.RightRunFalg = true; - if((Olu_diff1+Oru_diff1) < -(App.ADTrack.Stop_VPT) - && (Olu_diff2+Oru_diff2) < -(App.ADTrack.Stop_VPT) - && (Olu_diff3+Oru_diff3) < -(App.ADTrack.Stop_VPT) + if(((Olu_diff1+Oru_diff1) > (App.ADTrack.Stop_VPT)) + && ((Olu_diff2+Oru_diff2) > (App.ADTrack.Stop_VPT)) + && ((Olu_diff3+Oru_diff3) > (App.ADTrack.Stop_VPT)) + && ((Oll_diff1+Orl_diff1) < -(App.ADTrack.Stop_VPT)) + && ((Oll_diff2+Orl_diff2) < -(App.ADTrack.Stop_VPT)) + && ((Oll_diff3+Orl_diff3) < -(App.ADTrack.Stop_VPT)) ) { - if(App.MotorPara.Current_height < 1200) + if((abs(Olu_diff1) < (abs(Oll_diff1) + CENTRE_SHIELD_VPT)) + && (abs(Oru_diff1) < (abs(Orl_diff1) + CENTRE_SHIELD_VPT)) + && (abs(Olu_diff2) < (abs(Oll_diff2) + CENTRE_SHIELD_VPT)) + && (abs(Oru_diff2) < (abs(Orl_diff2) + CENTRE_SHIELD_VPT)) + && (abs(Olu_diff3) < (abs(Oll_diff3) + CENTRE_SHIELD_VPT)) + && (abs(Oru_diff3) < (abs(Orl_diff3) + CENTRE_SHIELD_VPT)) + )//判断是否不是正面压 { - BDC_UP(); - App.CaliZeroFlag = false; - App.UpFlag = true; - App.StopFlag = false; - App.DownFlag = false; - App.EmeFlag = false; - App.RunCollDelay1mSCnt = RUN_TIME_MS; + BDC_UP(); + LED_RED_ON(); + LED_GREEN_OFF(); + App.CaliZeroFlag = false; + App.UpFlag = true; + App.StopFlag = false; + App.DownFlag = false; + App.EmeFlag = false; + App.RunCollDelay1mSCnt = RUN_TIME_MS; } } - 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) + else if(((Olu_diff1+Oru_diff1) < -(App.ADTrack.Stop_VPT)) + && ((Olu_diff2+Oru_diff2) < -(App.ADTrack.Stop_VPT)) + && ((Olu_diff3+Oru_diff3) < -(App.ADTrack.Stop_VPT)) + && ((Oll_diff1+Orl_diff1) > (App.ADTrack.Stop_VPT)) + && ((Oll_diff2+Orl_diff2) > (App.ADTrack.Stop_VPT)) + && ((Oll_diff3+Orl_diff3) > (App.ADTrack.Stop_VPT)) ) { - if(App.MotorPara.Current_height > 730) + if((abs(Olu_diff1) > (abs(Oll_diff1) - CENTRE_SHIELD_VPT)) + && (abs(Oru_diff1) > (abs(Orl_diff1) - CENTRE_SHIELD_VPT)) + && (abs(Olu_diff2) > (abs(Oll_diff2) - CENTRE_SHIELD_VPT)) + && (abs(Oru_diff2) > (abs(Orl_diff2) - CENTRE_SHIELD_VPT)) + && (abs(Olu_diff3) > (abs(Oll_diff3) - CENTRE_SHIELD_VPT)) + && (abs(Oru_diff3) > (abs(Orl_diff3) - CENTRE_SHIELD_VPT)) + )//判断是否不是正面压 { BDC_DOWN(); + LED_RED_ON(); + LED_GREEN_OFF(); App.CaliZeroFlag = false; App.UpFlag = false; App.StopFlag = false; @@ -442,7 +396,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ } } } - else if((LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum))//判断左边操作 + else if(LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum && App.CentCollDelay1mSCnt <= 0)//判断左边操作 { App.LeftRunFalg = true; if((Olu_diff1 >= (App.ADTrack.LeftUpVPT*5/6)) //通过差值判断上下拉方向 @@ -459,37 +413,20 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (App.LeftLowCS1237_SlopeArray[1] >= PRESS_VPT) && (App.LeftLowCS1237_SlopeArray[2] >= PRESS_VPT)) { - if(App.CaliZeroFlag == true)//判断是否校零 + if((abs(Oll_diff1) > abs(Orl_diff1) + App.ADTrack.LeftLowShieldVPT)//判断是否不是侧面推拉 + && (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT) + && (abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT) + && (abs(Olu_diff1) > abs(Oru_diff1) + App.ADTrack.LeftUpShieldVPT)) { - if((abs(Oll_diff1) > abs(Orl_diff1) + App.ADTrack.LeftLowShieldVPT)//判断是否不是侧面推拉 - && (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT) - && (abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT) - && (abs(Olu_diff1) > abs(Oru_diff1) + App.ADTrack.LeftUpShieldVPT)) - { - if(App.MotorPara.Current_height > 730) - { - BDC_DOWN(); - App.CaliZeroFlag = false; - App.UpFlag = false; - App.DownFlag = true; - App.StopFlag = false; - App.EmeFlag = false; - App.RunCollDelay1mSCnt = RUN_TIME_MS; - } - } - } - else - { - if(App.MotorPara.Current_height > 730) - { - BDC_DOWN(); - App.CaliZeroFlag = false; - App.UpFlag = false; - App.DownFlag = true; - App.StopFlag = false; - App.EmeFlag = false; - App.RunCollDelay1mSCnt = RUN_TIME_MS; - } + BDC_UP(); + LED_RED_ON(); + LED_GREEN_OFF(); + App.CaliZeroFlag = false; + App.UpFlag = true; + App.DownFlag = false; + App.StopFlag = false; + App.EmeFlag = false; + App.RunCollDelay1mSCnt = RUN_TIME_MS; } } } @@ -508,42 +445,25 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (App.LeftLowCS1237_SlopeArray[1] <= -PRESS_VPT) && (App.LeftLowCS1237_SlopeArray[2] <= -PRESS_VPT)) { -// if(App.CaliZeroFlag == true)//判断是否校零 + if((abs(Oll_diff1) > abs(Orl_diff1) + App.ADTrack.LeftLowShieldVPT)//判断是否不是侧面推拉 + && (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT) + && (abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT) + && (abs(Olu_diff1) > abs(Oru_diff1) + App.ADTrack.LeftUpShieldVPT)) { - if((abs(Oll_diff1) > abs(Orl_diff1) + App.ADTrack.LeftLowShieldVPT)//判断是否不是侧面推拉 - && (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT) - && (abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT) - && (abs(Olu_diff1) > abs(Oru_diff1) + App.ADTrack.LeftUpShieldVPT)) - { - if(App.MotorPara.Current_height < 1200) - { - BDC_UP(); - App.CaliZeroFlag = false; - App.UpFlag = true; - App.DownFlag = false; - App.StopFlag = false; - App.EmeFlag = false; - App.RunCollDelay1mSCnt = RUN_TIME_MS; - } - } + BDC_DOWN(); + LED_RED_ON(); + LED_GREEN_OFF(); + App.CaliZeroFlag = false; + App.UpFlag = false; + App.DownFlag = true; + App.StopFlag = false; + App.EmeFlag = false; + App.RunCollDelay1mSCnt = RUN_TIME_MS; } -// else -// { -// if(App.MotorPara.Current_height < 1190) -// { -// BDC_UP(); -// App.CaliZeroFlag = false; -// App.UpFlag = true; -// App.DownFlag = false; -// App.StopFlag = false; -// App.EmeFlag = false; -// App.RunCollDelay1mSCnt = RUN_TIME_MS; -// } -// } } } } - else if(LeftSlopeSum < RightSlopeSum && LeftDiffSum < RightDiffSum)//判断右边操作 + else if(LeftSlopeSum < RightSlopeSum && LeftDiffSum < RightDiffSum && App.CentCollDelay1mSCnt <= 0)//判断右边操作 { App.RightRunFalg = true; if((Oru_diff1 >= (App.ADTrack.RightUpVPT*5/6)) //通过差值判断上下拉方向 @@ -560,38 +480,21 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (App.RightLowCS1237_SlopeArray[1] >= PRESS_VPT) && (App.RightLowCS1237_SlopeArray[2] >= PRESS_VPT)) { -// if(App.CaliZeroFlag == true)//判断是否校零 + if((abs(Orl_diff1) > abs(Oll_diff1) + App.ADTrack.RightLowShieldVPT)//判断是否不是侧面推拉 + && (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT) + && (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT) + && (abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT)) { - if((abs(Orl_diff1) > abs(Oll_diff1) + App.ADTrack.RightLowShieldVPT)//判断是否不是侧面推拉 - && (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT) - && (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT) - && (abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT)) - { - if(App.MotorPara.Current_height > 730) - { - BDC_DOWN(); - App.CaliZeroFlag = false; - App.DownFlag = true; - App.StopFlag = false; - App.UpFlag = false; - App.EmeFlag = false; - App.RunCollDelay1mSCnt = RUN_TIME_MS; - } - } + BDC_UP(); + LED_RED_ON(); + LED_GREEN_OFF(); + App.CaliZeroFlag = false; + App.UpFlag = true; + App.StopFlag = false; + App.DownFlag = false; + App.EmeFlag = false; + App.RunCollDelay1mSCnt = RUN_TIME_MS; } -// else -// { -// if(App.MotorPara.Current_height > 730) -// { -// BDC_DOWN(); -// App.CaliZeroFlag = false; -// App.DownFlag = true; -// App.StopFlag = false; -// App.UpFlag = false; -// App.EmeFlag = false; -// App.RunCollDelay1mSCnt = RUN_TIME_MS; -// } -// } } } else if((Oru_diff1 <= -(App.ADTrack.RightUpVPT*5/6)) //通过差值判断上下拉方向 @@ -608,43 +511,26 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (App.RightLowCS1237_SlopeArray[1] <= -PRESS_VPT) && (App.RightLowCS1237_SlopeArray[2] <= -PRESS_VPT)) { -// if(App.CaliZeroFlag == true)//判断是否校零 + if((abs(Orl_diff1) > abs(Oll_diff1) + App.ADTrack.RightLowShieldVPT)//判断是否不是侧面推拉 + && (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT) + && (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT) + && (abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT)) { - if((abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT)//判断是否不是侧面推拉 - && (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 < 1200) - { - BDC_UP(); - App.CaliZeroFlag = false; - App.UpFlag = true; - App.StopFlag = false; - App.DownFlag = false; - App.EmeFlag = false; - App.RunCollDelay1mSCnt = RUN_TIME_MS; - } - } + BDC_DOWN(); + LED_RED_ON(); + LED_GREEN_OFF(); + App.CaliZeroFlag = false; + App.DownFlag = true; + App.StopFlag = false; + App.UpFlag = false; + App.EmeFlag = false; + App.RunCollDelay1mSCnt = RUN_TIME_MS; } -// else -// { -// if(App.MotorPara.Current_height < 1190) -// { -// 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.DownFlag == true && App.RunCollDelay1mSCnt <= 0) + else if(App.UpFlag == true && App.RunCollDelay1mSCnt <= 0) { if(App.LeftRunFalg == true) { @@ -661,19 +547,22 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (Oll_diff5 > -App.ADTrack.Stop_VPT) && (Olu_diff6 < App.ADTrack.Stop_VPT) && (Oll_diff6 > -App.ADTrack.Stop_VPT)) - || ((App.CentreRunFalg == true) - && ((Olu_diff1 + Oru_diff1) < (App.ADTrack.Stop_VPT*2/3)) + || (((Olu_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)) + && ((Oll_diff1 + Orl_diff1) > -(App.ADTrack.Stop_VPT*2/3)) + && ((Oll_diff2 + Orl_diff2) > -(App.ADTrack.Stop_VPT*2/3)) + && ((Oll_diff3 + Orl_diff3) > -(App.ADTrack.Stop_VPT*2/3))) ) { BDC_STOP(); + LED_RED_OFF(); + LED_GREEN_ON(); App.StopFlag = true; App.DownFlag = false; App.UpFlag = false; App.EmeFlag = false; App.StopCollDelay1mSCnt = STOP_TIME_MS; - App.SlopeCollDelay1mSCnt = SLOPE_START_MS; App.EmergencyDelay1mSCnt = EME_START_MS; } // else if((abs(Oll_diff1) + EME_STOP_VPT) < abs(Orl_diff1) @@ -704,19 +593,22 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (Orl_diff5 > -App.ADTrack.Stop_VPT) && (Oru_diff6 < App.ADTrack.Stop_VPT) && (Orl_diff6 > -App.ADTrack.Stop_VPT)) - || ((App.CentreRunFalg == true) - && ((Olu_diff1 + Oru_diff1) < (App.ADTrack.Stop_VPT*2/3)) + || (((Olu_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)) + && ((Oll_diff1 + Orl_diff1) > -(App.ADTrack.Stop_VPT*2/3)) + && ((Oll_diff2 + Orl_diff2) > -(App.ADTrack.Stop_VPT*2/3)) + && ((Oll_diff3 + Orl_diff3) > -(App.ADTrack.Stop_VPT*2/3))) ) { BDC_STOP(); + LED_RED_OFF(); + LED_GREEN_ON(); App.StopFlag = true; App.DownFlag = false; App.UpFlag = false; App.EmeFlag = false; App.StopCollDelay1mSCnt = STOP_TIME_MS; - App.SlopeCollDelay1mSCnt = SLOPE_START_MS; App.EmergencyDelay1mSCnt = EME_START_MS; } // else if(abs(Oll_diff1) > (abs(Orl_diff1) + EME_STOP_VPT) @@ -733,7 +625,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ // } } } - else if(App.UpFlag == true && App.RunCollDelay1mSCnt <= 0) + else if(App.DownFlag == true && App.RunCollDelay1mSCnt <= 0) { if(App.LeftRunFalg == true) { @@ -750,19 +642,22 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (Oll_diff5 < App.ADTrack.Stop_VPT) && (Olu_diff6 > -App.ADTrack.Stop_VPT) && (Oll_diff6 < App.ADTrack.Stop_VPT)) - || ((App.CentreRunFalg == true) - && ((Olu_diff1 + Oru_diff1) > -(App.ADTrack.Stop_VPT*2/3)) + || (((Olu_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)) + && ((Oll_diff1 + Orl_diff1) < (App.ADTrack.Stop_VPT*2/3)) + && ((Oll_diff2 + Orl_diff2) < (App.ADTrack.Stop_VPT*2/3)) + && ((Oll_diff3 + Orl_diff3) < (App.ADTrack.Stop_VPT*2/3))) ) { BDC_STOP(); + LED_RED_OFF(); + LED_GREEN_ON(); App.StopFlag = true; App.DownFlag = false; App.UpFlag = false; App.EmeFlag = false; App.StopCollDelay1mSCnt = STOP_TIME_MS; - App.SlopeCollDelay1mSCnt = SLOPE_START_MS; App.EmergencyDelay1mSCnt = EME_START_MS; } // else if((abs(Oll_diff1) + EME_STOP_VPT) < abs(Orl_diff1) @@ -793,19 +688,22 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (Orl_diff5 < App.ADTrack.Stop_VPT) && (Oru_diff6 > -App.ADTrack.Stop_VPT) && (Orl_diff6 < App.ADTrack.Stop_VPT)) - || ((App.CentreRunFalg == true) - && ((Olu_diff1 + Oru_diff1) > -(App.ADTrack.Stop_VPT*2/3)) + || (((Olu_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)) + && ((Oll_diff1 + Orl_diff1) < (App.ADTrack.Stop_VPT*2/3)) + && ((Oll_diff2 + Orl_diff2) < (App.ADTrack.Stop_VPT*2/3)) + && ((Oll_diff3 + Orl_diff3) < (App.ADTrack.Stop_VPT*2/3))) ) { BDC_STOP(); + LED_RED_OFF(); + LED_GREEN_ON(); App.StopFlag = true; App.DownFlag = false; App.UpFlag = false; App.EmeFlag = false; App.StopCollDelay1mSCnt = STOP_TIME_MS; - App.SlopeCollDelay1mSCnt = SLOPE_START_MS; App.EmergencyDelay1mSCnt = EME_START_MS; } // else if(abs(Oll_diff1) > (abs(Orl_diff1) + EME_STOP_VPT) @@ -858,9 +756,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && App.RightLowCS1237_SlopeArray[2] > -FOLLOW_VPT) { if(App.StopFlag == true - && App.StopCollDelay1mSCnt <= 0 - && App.MotorPara.Current_height <= 1200 - && App.MotorPara.Current_height >= 722){ + && App.StopCollDelay1mSCnt <= 0){ App.ADTrack.LeftUp_Org_AdZero = lu_ad[0]; App.ADTrack.LeftLow_Org_AdZero = ll_ad[0]; App.ADTrack.RightUp_Org_AdZero = ru_ad[0]; @@ -888,7 +784,7 @@ static void AppInit(void) DBG_LOG("\r\n\r\n"); DBG_LOG("****************************************************\r\n"); DBG_LOG("** **\r\n"); - DBG_LOG("** Acc_Support_29V **\r\n"); + DBG_LOG("** SingleColumn_Trolley **\r\n"); DBG_LOG("** SoftWare V:%d.%d **\r\n", SOFTWARE_VERSION / 10, SOFTWARE_VERSION % 10); DBG_LOG("** HardWare V:%d.%d **\r\n", HARDWARE_VERSION / 10, HARDWARE_VERSION % 10); DBG_LOG("** Compile: %s %s **\r\n", __DATE__, __TIME__); @@ -901,18 +797,15 @@ static void AppInit(void) RIGHTUP_CS1237_Init(); RIGHTLOW_CS1237_Init(); - BDC_STOP(); + Beeper_Init(); -// 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; + BDC_STOP(); + LED_RED_OFF(); + LED_GREEN_ON(); + App.Status = APP_STATUS_ZERO_MARKING; + App.TD1mSDelayCnt = 2000; App.RStatus = READ_STATUS_START; GradeControl(App.Para.Layout.ControlLevel);//等级控制 - - App.WaitFlag = false; } static void ReadLoopHandler(void) { @@ -1000,8 +893,7 @@ static void ReadLoopHandler(void) if(App.LeftUpCS1237ReadEndFlag == true && App.LeftLowCS1237ReadEndFlag == true && App.RightUpCS1237ReadEndFlag == true && - App.RightLowCS1237ReadEndFlag == true && - App.WaitFlag == true) + App.RightLowCS1237ReadEndFlag == true) { #if(USE_AGC == 1) SupportControlHandle(App.LeftUpCS1237_TrendArray, App.LeftLowCS1237_TrendArray, App.RightUpCS1237_TrendArray, App.RightLowCS1237_TrendArray); @@ -1026,39 +918,7 @@ static void AppLoopHandler(void) App.Status = APP_STATUS_IDLE; break;} - - case APP_STATUS_MOTOR_INIT:{ - Beeper_Play_Blocking(200,200,2); - BDC_UP(); - Ddl_Delay1ms(1000); - BDC_DOWN(); - App.DownFlag = true; - App.StopFlag = false; - App.Status = APP_STATUS_WAIT_MOTOR_INIT; - App.TD1mSDelayCnt = TRAVEL_TIME_MS; - break;} - - case APP_STATUS_WAIT_MOTOR_INIT:{ - if(App.TD1mSDelayCnt <= 0) - { - BDC_STOP(); - App.StopFlag = true; - App.DownFlag = false; - App.UpFlag = false; - App.Status = APP_STATUS_IDLE; - } - if(App.MotorPara.Current_height <= 722 && App.MotorPara.Current_height >= 720) - { - BDC_STOP(); - 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) { @@ -1072,126 +932,18 @@ 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.CailUp1mSCnt = 0; - App.MotorPara.LoopOne = false; - } - break;} - - case APP_STATUS_WAIT_MOTOR_CALI_UP:{ - if(App.MotorPara.Current_height >= 1200 && App.CailUp1mSCnt >= 17000) - { - App.CailUp1mSCnt = 0; - BDC_STOP(); - App.StopFlag = true; - App.DownFlag = false; - App.UpFlag = false; - Ddl_Delay1ms(1000); - App.DownFlag = true; - App.StopFlag = false; - App.Status = APP_STATUS_WAIT_MOTOR_CALI_DOWN; - BDC_DOWN(); - } - else if(App.MotorPara.CaliNum >= 480 && App.MotorPara.LoopOne == true) - { - BDC_STOP(); - App.StopFlag = true; - App.DownFlag = false; - App.UpFlag = false; - App.MotorPara.LoopOne = false; - App.MotorPara.InitialFlag = false; - App.MotorPara.CaliFlag = true; - DBG_LOG("===================LU Data monitoring====================\r\n"); - weightedMovingAverageWithEnhance(App.MotorPara.LU_HeightStress,480,7,380,480,4,0); - DBG_LOG("===================LL Data monitoring====================\r\n"); - weightedMovingAverageWithEnhance(App.MotorPara.LL_HeightStress,480,7,380,480,4,0); - DBG_LOG("===================RU Data monitoring====================\r\n"); - weightedMovingAverageWithEnhance(App.MotorPara.RU_HeightStress,480,7,380,480,4,0); - DBG_LOG("===================RL Data monitoring====================\r\n"); - weightedMovingAverageWithEnhance(App.MotorPara.RL_HeightStress,480,7,380,480,4,0); - DBG_LOG("Motor Cali Ok\r\n"); - App.TD1mSDelayCnt = 1000; - App.Status = APP_STATUS_WAIT_STEADY; - } - break;} - - case APP_STATUS_WAIT_MOTOR_CALI_DOWN:{ - if(App.MotorPara.Current_height <= 725) - { - BDC_STOP(); - App.StopFlag = true; - App.DownFlag = false; - App.UpFlag = false; - App.MotorPara.LoopOne = true; - Ddl_Delay1ms(1000); - App.UpFlag = true; - App.StopFlag = false; - App.Status = APP_STATUS_WAIT_MOTOR_CALI_UP; - BDC_UP(); - App.CailUp1mSCnt = 0; - } - else if(App.MotorPara.CaliNum >= 480 && App.MotorPara.LoopOne == true) - { - BDC_STOP(); - App.StopFlag = true; - App.DownFlag = false; - App.UpFlag = false; - App.MotorPara.LoopOne = false; - App.MotorPara.InitialFlag = false; - App.MotorPara.CaliFlag = true; - DBG_LOG("===================LU Data monitoring====================\r\n"); - weightedMovingAverageWithEnhance(App.MotorPara.LU_HeightStress,480,7,380,480,4,0); - DBG_LOG("===================LL Data monitoring====================\r\n"); - weightedMovingAverageWithEnhance(App.MotorPara.LL_HeightStress,480,7,380,480,4,0); - DBG_LOG("===================RU Data monitoring====================\r\n"); - weightedMovingAverageWithEnhance(App.MotorPara.RU_HeightStress,480,7,380,480,4,0); - DBG_LOG("===================RL Data monitoring====================\r\n"); - weightedMovingAverageWithEnhance(App.MotorPara.RL_HeightStress,480,7,380,480,4,0); - DBG_LOG("Motor Cali Ok\r\n"); - App.TD1mSDelayCnt = 1000; - App.Status = APP_STATUS_WAIT_STEADY; - } - break;} - - case APP_STATUS_WAIT_STEADY:{ - if(App.TD1mSDelayCnt <= 0) - { - App.SideFlag = false; App.CaliZeroFlag = false; + App.StopFlag = true; App.UpFlag = false; App.DownFlag = false; App.EmeFlag = false; - App.StopFlag = true; - App.WaitFlag = true; - App.Status = APP_STATUS_HEIGHT_CONTROL; - Beeper_Play_Blocking(1000,0,1); - } - break;} - - case APP_STATUS_HEIGHT_CONTROL:{ - if(App.MotorPara.Current_height < 722 - && App.MotorPara.RestFlag == false - && App.HeightContRolCnt >= 500) - { - App.HeightContRolCnt = 0; - BDC_UP(); - Ddl_Delay1ms(500); - BDC_STOP(); - } - else if(App.MotorPara.Current_height > 1200 - && App.MotorPara.RestFlag == false - && App.HeightContRolCnt >= 500) - { - App.HeightContRolCnt = 0; - BDC_DOWN(); - Ddl_Delay1ms(400); + BDC_STOP(); + LED_RED_OFF(); + LED_GREEN_ON(); + App.Status = APP_STATUS_IDLE; + + Beeper_Start(200,200,2); } break;} } @@ -1202,8 +954,6 @@ static void App1mSRoutine(void) App.TD1mSDelayCnt--; if(App.Read1mSDelayCnt > 0) App.Read1mSDelayCnt--; - if(App.Uart1Delay1mSCnt > 0) - App.Uart1Delay1mSCnt--; if(App.Uart3RxTimeOut1mSCnt > 0) App.Uart3RxTimeOut1mSCnt--; if(App.StopCollDelay1mSCnt > 0) @@ -1212,18 +962,8 @@ static void App1mSRoutine(void) App.RunCollDelay1mSCnt--; if(App.EmergencyDelay1mSCnt > 0) App.EmergencyDelay1mSCnt--; - if(App.SlopeCollDelay1mSCnt > 0) - App.SlopeCollDelay1mSCnt--; - if(App.MotorPara.InitialFlag == true) - App.CailUp1mSCnt++; - if((App.MotorPara.Current_height < 722 || App.MotorPara.Current_height > 1200)) - { - App.HeightContRolCnt++; - } - else - { - App.HeightContRolCnt = 0; - } + if(App.CentCollDelay1mSCnt > 0) + App.CentCollDelay1mSCnt--; App.FeedDogDlyCnt++; } void FeedDogHandler(void) @@ -1257,6 +997,7 @@ int main(void) ReadLoopHandler(); DcmotorLoopHandler(); AdcLoopHander(); + BeeperLoopHandler(); AppLoopHandler(); } } @@ -1276,6 +1017,7 @@ void SysTick_IrqHandler(void) RIGHTUP_CS12371mSRoutine(); RIGHTLOW_CS12371mSRoutine(); Dcmotor1msRoutine(); + Beeper_ClearDelay(); Adc1mSRoutine(); } @@ -1322,12 +1064,7 @@ void Usart3RxIrqCallback(void) } else { -// if(App.RxUart3Data.RxLen == 1 && Data != 0x01) -// { -// App.RxUart3Data.HeaderFrame = false; -// return; -// } - // 如果RxLen小于128，则将数据添加到RxBuff中 + // 如果RxLen小于128，则将数据添加到RxBuff中 if(App.RxUart3Data.RxLen < 128) { App.RxUart3Data.RxBuff[App.RxUart3Data.RxLen++] = Data; } @@ -1336,30 +1073,17 @@ void Usart3RxIrqCallback(void) if(App.RxUart3Data.RxBuff[0] == 0x01 && App.RxUart3Data.RxBuff[1] == 0x04 && App.MotorPara.RestFlag == false) { App.MotorPara.RestFlag = true; - Beeper_Play_Blocking(50,50,5); + Beeper_Start(1500,0,1); } else if(App.RxUart3Data.RxBuff[0] == 0x01 && App.RxUart3Data.RxBuff[1] == 0x01) { - App.MotorPara.RestFlag = false; - int16_t h = ((App.RxUart3Data.RxBuff[2]<<8) & 0xff00) + (App.RxUart3Data.RxBuff[3] & 0x00ff); - if(h >=720 && h <= 1208) + if(App.MotorPara.RestFlag == true) { - App.MotorPara.Current_height = h; - } - if(App.MotorPara.InitialFlag == true && h >=720 && h <= 1208) - {//缓存每个高度点对于零点应力的差值，对不同高度点的应力进行补偿 - if(App.MotorPara.LU_HeightStress[(App.MotorPara.Current_height)-720] == 0 - || App.MotorPara.LL_HeightStress[(App.MotorPara.Current_height)-720] == 0 - || App.MotorPara.RU_HeightStress[(App.MotorPara.Current_height)-720] == 0 - || App.MotorPara.RL_HeightStress[(App.MotorPara.Current_height)-720] == 0) - { - App.MotorPara.CaliNum++; - } - App.MotorPara.LU_HeightStress[(App.MotorPara.Current_height)-720] = App.ADTrack.LeftUp_Org_AdZero - App.LeftUpCS1237_TrendArray[0]; - App.MotorPara.LL_HeightStress[(App.MotorPara.Current_height)-720] = App.ADTrack.LeftLow_Org_AdZero - App.LeftLowCS1237_TrendArray[0]; - App.MotorPara.RU_HeightStress[(App.MotorPara.Current_height)-720] = App.ADTrack.RightUp_Org_AdZero - App.RightUpCS1237_TrendArray[0]; - App.MotorPara.RL_HeightStress[(App.MotorPara.Current_height)-720] = App.ADTrack.RightLow_Org_AdZero - App.RightLowCS1237_TrendArray[0]; + App.MotorPara.RestFlag = false; + Beeper_Start(200,200,2); } + int16_t h = ((App.RxUart3Data.RxBuff[2]<<8) & 0xff00) + (App.RxUart3Data.RxBuff[3] & 0x00ff); + App.MotorPara.Current_height = h; } memset(App.RxUart3Data.RxBuff, 0, sizeof(App.RxUart3Data.RxBuff)); App.RxUart3Data.RxLen = 0;