diff --git a/Project/HC32F460/MDK/HC32F460.uvoptx b/Project/HC32F460/MDK/HC32F460.uvoptx index 10bf79a..5b7904c 100644 --- a/Project/HC32F460/MDK/HC32F460.uvoptx +++ b/Project/HC32F460/MDK/HC32F460.uvoptx @@ -103,7 +103,7 @@ 1 0 0 - 12 + 3 @@ -228,96 +228,168 @@ 14 1 - App.MotorPara.LU_HeightStress,0x0A + App.TD1mSDelayCnt,0x0A 15 1 - App.MotorPara.LL_HeightStress,0x0A + App.Status 16 1 - App.MotorPara.RU_HeightStress,0x0A + App.MotorPara.CaliNum,0x0A 17 1 - App.MotorPara.RL_HeightStress,0x0A + App.CailUp1mSCnt,0x0A 18 1 - App.TD1mSDelayCnt,0x0A + App.MotorPara.RestFlag,0x0A 19 1 - App.RxUart3Data.RxBuff,0x10 + App.MotorPara.Current_height,0x0A 20 1 - App.RxUart3Data.RxLen,0x0A + App.HeightContRolCnt,0x0A 21 1 - App.Status + App.aaa,0x0A 22 1 - App.MotorPara.CaliNum,0x0A + App.bbb,0x0A 23 1 - App.CailUp1mSCnt,0x0A + App.ccc,0x0A 24 1 - Oru_diff1,0x0A + App.ddd,0x0A 25 1 - Orl_diff1,0x0A + App.ADTrack.LeftUpVPT,0x0A 26 1 - App.ADTrack.Stop_VPT,0x0A + App.ADTrack.LeftLowVPT,0x0A 27 1 - ru_ad[0],0x0A + App.ADTrack.RightUpVPT,0x0A 28 1 - App.MotorPara.RestFlag,0x0A + App.ADTrack.RightLowVPT,0x0A 29 1 - App.MotorPara.Current_height,0x0A + App.ADTrack.Stop_VPT,0x0A 30 1 - App.HeightContRolCnt,0x0A + App.MotorPara.Current_height,0x0A + + + 0 + 2 + Olu_diff1,0x0A + + + 1 + 2 + Oll_diff1,0x0A + + + 2 + 2 + Oru_diff1,0x0A + + + 3 + 2 + Orl_diff1,0x0A + + + 4 + 2 + Olu_diff2,0x0A + + + 5 + 2 + Oll_diff2,0x0A + + + 6 + 2 + Oru_diff2,0x0A + + + 7 + 2 + Orl_diff2,0x0A + + + 8 + 2 + Olu_diff3,0x0A + + + 9 + 2 + Oll_diff3,0x0A + + + 10 + 2 + Oru_diff3,0x0A + + + 11 + 2 + Orl_diff3,0x0A + + + 12 + 2 + LeftDiff,0x0A + + + 13 + 2 + RightDiff,0x0A + + 0 0 1 - 1 + 0 0 0 0 @@ -1192,6 +1264,26 @@ + + BeepAndLed + 1 + 0 + 0 + 0 + + 6 + 55 + 1 + 0 + 0 + 0 + ..\source\Module\BeepAndLed\BeepAndLed.c + BeepAndLed.c + 0 + 0 + + + Readme 0 diff --git a/Project/HC32F460/MDK/HC32F460.uvprojx b/Project/HC32F460/MDK/HC32F460.uvprojx index b89011a..8a765fa 100644 --- a/Project/HC32F460/MDK/HC32F460.uvprojx +++ b/Project/HC32F460/MDK/HC32F460.uvprojx @@ -10,14 +10,14 @@ Debug 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\User\Inc;..\source\User\Src;..\..\..\driver\src;..\..\..\driver\inc;..\source\Module\CS1237;..\source\Module\dc_motor + ..\..\..\mcu\common;..\source\User\Inc;..\source\User\Src;..\..\..\driver\src;..\..\..\driver\inc;..\source\Module\CS1237;..\source\Module\dc_motor;..\source\Module\BeepAndLed @@ -677,6 +677,16 @@ + + BeepAndLed + + + BeepAndLed.c + 1 + ..\source\Module\BeepAndLed\BeepAndLed.c + + + Readme @@ -695,7 +705,7 @@ HC32F460PETB HDSC - HDSC.HC32F460.1.0.12 + HDSC.HC32F460.1.0.11 https://raw.githubusercontent.com/hdscmcu/pack/master/ IROM(0x00000000,0x80000) IROM2(0x03000C00,0x3FC) IRAM2(0x200F0000,0x1000) IROM(0x00000000,0x80000) CPUTYPE("Cortex-M4") FPU2 CLOCK(12000000) ELITTLE @@ -1355,6 +1365,16 @@ + + BeepAndLed + + + BeepAndLed.c + 1 + ..\source\Module\BeepAndLed\BeepAndLed.c + + + Readme diff --git a/Project/HC32F460/MDK/startup_hc32f460.s b/Project/HC32F460/MDK/startup_hc32f460.s index efd8bde..33dfb6e 100644 --- a/Project/HC32F460/MDK/startup_hc32f460.s +++ b/Project/HC32F460/MDK/startup_hc32f460.s @@ -224,7 +224,7 @@ Reset_Handler PROC IMPORT SystemInit IMPORT __main SET_SRAM3_WAIT - LDR R0, =0x40050804 +1 LDR R0, =0x40050804 MOV R1, #0x77 STR R1, [R0] diff --git a/Project/HC32F460/source/Module/BeepAndLed/BeepAndLed.c b/Project/HC32F460/source/Module/BeepAndLed/BeepAndLed.c new file mode 100644 index 0000000..089160e --- /dev/null +++ b/Project/HC32F460/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/HC32F460/source/Module/BeepAndLed/BeepAndLed.h b/Project/HC32F460/source/Module/BeepAndLed/BeepAndLed.h new file mode 100644 index 0000000..bd8fded --- /dev/null +++ b/Project/HC32F460/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/HC32F460/source/User/Inc/Algorithm.h b/Project/HC32F460/source/User/Inc/Algorithm.h index 60dd0b4..7ab0351 100644 --- a/Project/HC32F460/source/User/Inc/Algorithm.h +++ b/Project/HC32F460/source/User/Inc/Algorithm.h @@ -51,9 +51,8 @@ uint8_t CRC_Sum(uint8_t *_pbuff, uint16_t _cmdLen); bool isAllZero(uint8_t *arr, int size); int fixWithMedianIterative(int arr[], int size, int windowSize, int threshold, int maxIterations, int verbose); int finalCheck(int arr[], int size, int threshold, int verbose); -int weightedMovingAverageWithEnhance(int32_t arr[], int size, int windowSize, - int enhanceStart, int enhanceEnd, - double enhanceFactor, int verbose); +int weightedMovingAverageWithEnhance(int32_t arr[], int size, int windowSize,int enhanceStart, int enhanceEnd, double enhanceFactor, int verbose); +int CompareAndCountAbs(const int32_t *arr1, const int32_t *arr2, uint16_t length,int32_t zero1, int32_t zero2, int32_t offset); #endif diff --git a/Project/HC32F460/source/User/Inc/bsp.h b/Project/HC32F460/source/User/Inc/bsp.h index df63b87..7de4721 100644 --- a/Project/HC32F460/source/User/Inc/bsp.h +++ b/Project/HC32F460/source/User/Inc/bsp.h @@ -213,6 +213,24 @@ #define CLR_TWIN_BIT1() PORT_ResetBits(TWIN_BIT1_PORTx, TWIN_BIT1_PINx)//拉低 #define GET_BIT1_DOUT() PORT_GetBit(TWIN_BIT1_PORTx, TWIN_BIT1_PINx) +//< BUZZER +#define BUZZER_PORTx (PortB) +#define BUZZER_PINx (Pin15) +#define BUZZER_ON() PORT_SetBits(BUZZER_PORTx, BUZZER_PINx)//拉高 +#define BUZZER_OFF() PORT_ResetBits(BUZZER_PORTx, BUZZER_PINx)//拉低 + +//< LED +#define LED_GREEN_PORTx (PortA) +#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 (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)//拉低 + + typedef struct { int32_t ControlLevel;//控制等级 int32_t DBGWaveOut;//调试波形输出 @@ -289,6 +307,8 @@ void ExtInt04IntEnable(void); void ExtInt04IntDisable(void); void ExtInt06IntEnable(void); void ExtInt06IntDisable(void); +void Bit0AndBit1_OUT(void); +void Bit0AndBit1_IN(void); #endif diff --git a/Project/HC32F460/source/User/Inc/main.h b/Project/HC32F460/source/User/Inc/main.h index 943c989..c8a0986 100644 --- a/Project/HC32F460/source/User/Inc/main.h +++ b/Project/HC32F460/source/User/Inc/main.h @@ -11,26 +11,31 @@ #define SLOPE_CNT 20 #define SLOPE_VPT 1000 -#define PRESS_VPT 3 +#define PRESS_VPT 10 #define FOLLOW_VPT 10 -#define SHIELD_VPT 5000 +#define SHIELD_VPT 2 #define EME_STOP_VPT 4000 -#define CENTRE_VPT 2000 + +#define CENTRE_FRONT_CNT 0 +#define CENTRE_FRONT_SUM 0 +#define CENTRE_FRONT_VPT 0 +#define CENTRE_VPT 0 #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 TRAVEL_TIME_MS 50000 #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()) -#define BDC_DOWN() (SET_TWIN_BIT1(),CLR_TWIN_BIT0()) +#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) @@ -55,11 +60,6 @@ typedef struct{ int32_t RightUpVPT;//右上设备阈值 int32_t RightLowVPT;//右下设备阈值 - int32_t LeftUpShieldVPT; //左上屏蔽阈值 - int32_t LeftLowShieldVPT; //左下屏蔽阈值 - int32_t RightUpShieldVPT; //右上屏蔽阈值 - int32_t RightLowShieldVPT; //右下屏蔽阈值 - int32_t LeftUp_Org_AdZero;//左上设备原始AD零点 int32_t LeftLow_Org_AdZero;//左下设备原始AD零点 int32_t RightUp_Org_AdZero;//右上设备原始AD零点 @@ -135,6 +135,7 @@ typedef struct { uint32_t Uart3RxTimeOut1mSCnt; uint32_t StopCollDelay1mSCnt; uint32_t RunCollDelay1mSCnt; + uint32_t CentCollDelay1mSCnt; uint32_t EmergencyDelay1mSCnt; uint32_t SlopeCollDelay1mSCnt; uint32_t CailUp1mSCnt; @@ -166,6 +167,11 @@ typedef struct { int32_t RightUpCS1237_SlopeArray[SLOPE_CNT]; int32_t RightLowCS1237_SlopeArray[SLOPE_CNT]; + int32_t aaa; + int32_t bbb; + int32_t ccc; + int32_t ddd; + bool WaitFlag; }AppDetect_t; extern AppDetect_t App; diff --git a/Project/HC32F460/source/User/Src/Algorithm.c b/Project/HC32F460/source/User/Src/Algorithm.c index beeb12c..281118e 100644 --- a/Project/HC32F460/source/User/Src/Algorithm.c +++ b/Project/HC32F460/source/User/Src/Algorithm.c @@ -4,6 +4,50 @@ #include "Algorithm.h" #include "Debug.h" + +#include +#include +#include // 用于 abs() 函数 + +/** + * @brief 比较两个数组对应位置的绝对值差值，统计满足条件的个数 + * + * 判断条件：|arr1[i] - zero1| + offset > |arr2[i] - zero2| + * + * @param arr1 数组1指针 + * @param arr2 数组2指针 + * @param length 要比较的数组长度（截取值） + * @param zero1 数组1的零点值 + * @param zero2 数组2的零点值 + * @param offset 补偿值 + * @return int 满足条件的个数 + * + * @note 如果 length 为0，返回0 + * @note 如果 arr1 或 arr2 为 NULL，返回0 + */ +int CompareAndCountAbs(const int32_t *arr1, const int32_t *arr2, uint16_t length, + int32_t zero1, int32_t zero2, int32_t offset) +{ + // 参数校验 + if (arr1 == NULL || arr2 == NULL || length == 0) { + return 0; + } + + int count = 0; + + for (uint16_t i = 0; i < length; i++) { + // 计算绝对值 + int32_t abs1 = abs(arr1[i] - zero1); + int32_t abs2 = abs(arr2[i] - zero2); + + // 判断：|arr1[i] - zero1| + offset > |arr2[i] - zero2| + if (abs1 + offset > abs2) { + count++; + } + } + + return count; +} /** * 加强特定区域的平滑处理（使用更小的权重衰减，更平滑的效果） * diff --git a/Project/HC32F460/source/User/Src/bsp.c b/Project/HC32F460/source/User/Src/bsp.c index 42e5b57..7b22717 100644 --- a/Project/HC32F460/source/User/Src/bsp.c +++ b/Project/HC32F460/source/User/Src/bsp.c @@ -517,13 +517,43 @@ static void GPIO_Config(void) PORT_Init(LEFTLOW_CS1237_SCLK_PORTx, LEFTLOW_CS1237_SCLK_PINx, &stcPortInit); PORT_Init(RIGHTUP_CS1237_SCLK_PORTx, RIGHTUP_CS1237_SCLK_PINx, &stcPortInit); PORT_Init(RIGHTLOW_CS1237_SCLK_PORTx, RIGHTLOW_CS1237_SCLK_PINx, &stcPortInit); + PORT_Init(BUZZER_PORTx, BUZZER_PINx, &stcPortInit); + PORT_Init(LED_GREEN_PORTx, LED_GREEN_PINx, &stcPortInit); + PORT_Init(LED_RED_PORTx, LED_RED_PINx, &stcPortInit); + LED_GREEN_OFF(); + LED_RED_OFF(); + stcPortInit.enPullUp = Enable;//上拉 +} +void Bit0AndBit1_OUT(void) +{ + stc_port_init_t stcPortInit; + MEM_ZERO_STRUCT(stcPortInit); + + /********************************** GPIO输出 ****************************************/ + stcPortInit.enPinMode = Pin_Mode_Out; + stcPortInit.enPinOType = Pin_OType_Cmos; + stcPortInit.enPinDrv = Pin_Drv_H; + stcPortInit.enPullUp = Disable;//无上拉 + stcPortInit.enPullUp = Enable;//上拉 PORT_Init(TWIN_BIT0_PORTx, TWIN_BIT0_PINx, &stcPortInit); PORT_Init(TWIN_BIT1_PORTx, TWIN_BIT1_PINx, &stcPortInit); SET_TWIN_BIT0(); SET_TWIN_BIT1(); } +void Bit0AndBit1_IN(void) +{ + stc_port_init_t stcPortInit; + MEM_ZERO_STRUCT(stcPortInit); + + /********************************** GPIO输入 ****************************************/ + stcPortInit.enPinMode = Pin_Mode_In; + stcPortInit.enPullUp = Disable;//无上拉 + + PORT_Init(TWIN_BIT0_PORTx, TWIN_BIT0_PINx, &stcPortInit); + PORT_Init(TWIN_BIT1_PORTx, TWIN_BIT1_PINx, &stcPortInit); +} /*********************************左上传感器 ******************************************************************************* ** \brief ExtInt00 callback function @@ -1429,6 +1459,7 @@ void BSP_Init(void) ExtiCh01_Init(); ExtiCh06_Init(); ExtiCh04_Init(); + Bit0AndBit1_OUT(); DbgUart_Config(700000); #if(USE_USART3 == 1) diff --git a/Project/HC32F460/source/User/Src/main.c b/Project/HC32F460/source/User/Src/main.c index 590b557..3d81cc9 100644 --- a/Project/HC32F460/source/User/Src/main.c +++ b/Project/HC32F460/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) @@ -42,7 +43,7 @@ void LEFTUP_CS1237AinADataProcessCallBack(int32_t AD) // } if(App.MotorPara.CaliFlag == true) { - AD = AD + App.MotorPara.LU_HeightStress[(App.MotorPara.Current_height)-720]; + AD = AD + App.MotorPara.LU_HeightStress[(App.MotorPara.Current_height)-725]; for(int i = OSC_CNT-1; i > 0; i--) { App.LeftUpCS1237_TrendArray[i] = App.LeftUpCS1237_TrendArray[i - 1]; @@ -50,10 +51,10 @@ void LEFTUP_CS1237AinADataProcessCallBack(int32_t 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]; -// } + 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); @@ -61,14 +62,13 @@ void LEFTUP_CS1237AinADataProcessCallBack(int32_t AD) } void LEFTLOW_CS1237AinADataProcessCallBack(int32_t AD) { - // if(abs(AD - App.LeftLowCS1237_TrendArray[0]) > 2000000) // { // AD = App.LeftLowCS1237_TrendArray[0]; // } if(App.MotorPara.CaliFlag == true) { - AD = AD + App.MotorPara.LL_HeightStress[(App.MotorPara.Current_height)-720]; + AD = AD + App.MotorPara.LL_HeightStress[(App.MotorPara.Current_height)-725]; for(int i = OSC_CNT-1; i > 0; i--) { App.LeftLowCS1237_TrendArray[i] = App.LeftLowCS1237_TrendArray[i - 1]; @@ -94,7 +94,7 @@ void RIGHTUP_CS1237AinADataProcessCallBack(int32_t AD) // } if(App.MotorPara.CaliFlag == true) { - AD = AD + App.MotorPara.RU_HeightStress[(App.MotorPara.Current_height)-720]; + AD = AD + App.MotorPara.RU_HeightStress[(App.MotorPara.Current_height)-725]; for(int i = OSC_CNT-1; i > 0; i--) { App.RightUpCS1237_TrendArray[i] = App.RightUpCS1237_TrendArray[i - 1]; @@ -120,7 +120,7 @@ void RIGHTLOW_CS1237AinADataProcessCallBack(int32_t AD) // } if(App.MotorPara.CaliFlag == true) { - AD = AD + App.MotorPara.RL_HeightStress[(App.MotorPara.Current_height)-720]; + AD = AD + App.MotorPara.RL_HeightStress[(App.MotorPara.Current_height)-725]; for(int i = OSC_CNT-1; i > 0; i--) { App.RightLowCS1237_TrendArray[i] = App.RightLowCS1237_TrendArray[i - 1]; @@ -147,10 +147,6 @@ void GradeControl(ControlLevel_m level) App.ADTrack.LeftLowVPT = 5000; App.ADTrack.RightUpVPT = 5000; App.ADTrack.RightLowVPT = 5000; - App.ADTrack.LeftUpShieldVPT = SHIELD_VPT; - App.ADTrack.LeftLowShieldVPT = SHIELD_VPT; - App.ADTrack.RightUpShieldVPT = SHIELD_VPT; - App.ADTrack.RightLowShieldVPT = SHIELD_VPT; App.ADTrack.Stop_VPT = 4000; break;} @@ -160,24 +156,16 @@ void GradeControl(ControlLevel_m level) App.ADTrack.LeftLowVPT = 10000; App.ADTrack.RightUpVPT = 10000; App.ADTrack.RightLowVPT = 10000; - App.ADTrack.LeftUpShieldVPT = SHIELD_VPT; - App.ADTrack.LeftLowShieldVPT = SHIELD_VPT; - App.ADTrack.RightUpShieldVPT = SHIELD_VPT; - App.ADTrack.RightLowShieldVPT = SHIELD_VPT; App.ADTrack.Stop_VPT = 8000; break;} case LEVEL_3:{ App.ADTrack.Slope_VPT = SLOPE_VPT; - App.ADTrack.LeftUpVPT = 20000; - App.ADTrack.LeftLowVPT = 20000; - App.ADTrack.RightUpVPT = 20000; - App.ADTrack.RightLowVPT = 20000; - App.ADTrack.LeftUpShieldVPT = SHIELD_VPT; - App.ADTrack.LeftLowShieldVPT = SHIELD_VPT; - App.ADTrack.RightUpShieldVPT = SHIELD_VPT; - App.ADTrack.RightLowShieldVPT = SHIELD_VPT; - App.ADTrack.Stop_VPT = 14000; + App.ADTrack.LeftUpVPT = 18000; + App.ADTrack.LeftLowVPT = 18000; + App.ADTrack.RightUpVPT = 18000; + App.ADTrack.RightLowVPT = 18000; + App.ADTrack.Stop_VPT = 15000; break;} case LEVEL_4:{ @@ -186,10 +174,6 @@ void GradeControl(ControlLevel_m level) 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;} @@ -199,10 +183,6 @@ void GradeControl(ControlLevel_m level) 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; break;} } @@ -277,10 +257,10 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.RightUpCS1237_SlopeArray[0] = ru_slope; App.RightLowCS1237_SlopeArray[0] = rl_slope; - PeakValleyCheck lu_K = check_peaks_valleys(App.LeftUpCS1237_SlopeArray, SLOPE_CNT); - PeakValleyCheck ll_K = check_peaks_valleys(App.LeftLowCS1237_SlopeArray, SLOPE_CNT); - PeakValleyCheck ru_K = check_peaks_valleys(App.RightUpCS1237_SlopeArray, SLOPE_CNT); - PeakValleyCheck rl_K = check_peaks_valleys(App.RightLowCS1237_SlopeArray, SLOPE_CNT); +// PeakValleyCheck lu_K = check_peaks_valleys(App.LeftUpCS1237_SlopeArray, SLOPE_CNT); +// PeakValleyCheck ll_K = check_peaks_valleys(App.LeftLowCS1237_SlopeArray, SLOPE_CNT); +// PeakValleyCheck ru_K = check_peaks_valleys(App.RightUpCS1237_SlopeArray, SLOPE_CNT); +// PeakValleyCheck rl_K = check_peaks_valleys(App.RightLowCS1237_SlopeArray, SLOPE_CNT); int32_t LeftSlopeSum = abs(App.LeftUpCS1237_SlopeArray[0]) + abs(App.LeftLowCS1237_SlopeArray[0]) + abs(App.LeftUpCS1237_SlopeArray[1]) + abs(App.LeftLowCS1237_SlopeArray[1]) + abs(App.LeftUpCS1237_SlopeArray[2]) + abs(App.LeftLowCS1237_SlopeArray[2]); int32_t RightSlopeSum = abs(App.RightUpCS1237_SlopeArray[0]) + abs(App.RightLowCS1237_SlopeArray[0]) + abs(App.RightUpCS1237_SlopeArray[1]) + abs(App.RightLowCS1237_SlopeArray[1]) + abs(App.RightUpCS1237_SlopeArray[2]) + abs(App.RightLowCS1237_SlopeArray[2]); @@ -319,106 +299,186 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ App.LeftRunFalg = false; App.RightRunFalg = false; App.CentreRunFalg = false; - if(((lu_ad[0] < App.ADTrack.LeftUp_Org_AdZero-CENTRE_VPT)//判断中间操作 - && (ll_ad[0] > App.ADTrack.LeftLow_Org_AdZero+CENTRE_VPT) - && (ru_ad[0] < App.ADTrack.RightUp_Org_AdZero-CENTRE_VPT) - && (rl_ad[0] > App.ADTrack.RightLow_Org_AdZero+CENTRE_VPT) - && (lu_ad[1] < App.ADTrack.LeftUp_Org_AdZero-CENTRE_VPT) - && (ll_ad[1] > App.ADTrack.LeftLow_Org_AdZero+CENTRE_VPT) - && (ru_ad[1] < App.ADTrack.RightUp_Org_AdZero-CENTRE_VPT) - && (rl_ad[1] > App.ADTrack.RightLow_Org_AdZero+CENTRE_VPT) - && (lu_ad[2] < App.ADTrack.LeftUp_Org_AdZero-CENTRE_VPT) - && (ll_ad[2] > App.ADTrack.LeftLow_Org_AdZero+CENTRE_VPT) - && (ru_ad[2] < App.ADTrack.RightUp_Org_AdZero-CENTRE_VPT) - && (rl_ad[2] > App.ADTrack.RightLow_Org_AdZero+CENTRE_VPT)) - || ((lu_ad[0] > App.ADTrack.LeftUp_Org_AdZero+CENTRE_VPT) - && (ll_ad[0] < App.ADTrack.LeftLow_Org_AdZero-CENTRE_VPT) - && (ru_ad[0] > App.ADTrack.RightUp_Org_AdZero+CENTRE_VPT) - && (rl_ad[0] < App.ADTrack.RightLow_Org_AdZero-CENTRE_VPT) - && (lu_ad[1] > App.ADTrack.LeftUp_Org_AdZero+CENTRE_VPT) - && (ll_ad[1] < App.ADTrack.LeftLow_Org_AdZero-CENTRE_VPT) - && (ru_ad[1] > App.ADTrack.RightUp_Org_AdZero+CENTRE_VPT) - && (rl_ad[1] < App.ADTrack.RightLow_Org_AdZero-CENTRE_VPT) - && (lu_ad[2] > App.ADTrack.LeftUp_Org_AdZero+CENTRE_VPT) - && (ll_ad[2] < App.ADTrack.LeftLow_Org_AdZero-CENTRE_VPT) - && (ru_ad[2] > App.ADTrack.RightUp_Org_AdZero+CENTRE_VPT) - && (rl_ad[2] < App.ADTrack.RightLow_Org_AdZero-CENTRE_VPT))) + if(((lu_ad[0] < App.ADTrack.LeftUp_Org_AdZero-App.ddd)//判断中间操作 + && (ru_ad[0] < App.ADTrack.RightUp_Org_AdZero-App.ddd) + && (lu_ad[1] < App.ADTrack.LeftUp_Org_AdZero-App.ddd) + && (ru_ad[1] < App.ADTrack.RightUp_Org_AdZero-App.ddd) + && (lu_ad[2] < App.ADTrack.LeftUp_Org_AdZero-App.ddd) + && (ru_ad[2] < App.ADTrack.RightUp_Org_AdZero-App.ddd) + && (ll_ad[0] > App.ADTrack.LeftLow_Org_AdZero+App.ddd)//判断中间操作 + && (rl_ad[0] > App.ADTrack.RightLow_Org_AdZero+App.ddd) + && (ll_ad[1] > App.ADTrack.LeftLow_Org_AdZero+App.ddd) + && (rl_ad[1] > App.ADTrack.RightLow_Org_AdZero+App.ddd) + && (ll_ad[2] > App.ADTrack.LeftLow_Org_AdZero+App.ddd) + && (rl_ad[2] > App.ADTrack.RightLow_Org_AdZero+App.ddd)) + || ((lu_ad[0] > App.ADTrack.LeftUp_Org_AdZero+App.ddd)//判断中间操作 + && (ru_ad[0] > App.ADTrack.RightUp_Org_AdZero+App.ddd) + && (lu_ad[1] > App.ADTrack.LeftUp_Org_AdZero+App.ddd) + && (ru_ad[1] > App.ADTrack.RightUp_Org_AdZero+App.ddd) + && (lu_ad[2] > App.ADTrack.LeftUp_Org_AdZero+App.ddd) + && (ru_ad[2] > App.ADTrack.RightUp_Org_AdZero+App.ddd) + && (ll_ad[0] < App.ADTrack.LeftLow_Org_AdZero-App.ddd)//判断中间操作 + && (rl_ad[0] < App.ADTrack.RightLow_Org_AdZero-App.ddd) + && (ll_ad[1] < App.ADTrack.LeftLow_Org_AdZero-App.ddd) + && (rl_ad[1] < App.ADTrack.RightLow_Org_AdZero-App.ddd) + && (ll_ad[2] < App.ADTrack.LeftLow_Org_AdZero-App.ddd) + && (rl_ad[2] < App.ADTrack.RightLow_Org_AdZero-App.ddd)) + ) { App.CentreRunFalg = true; - if(LeftDiff > RightDiff) + App.CentCollDelay1mSCnt = CENT_TIME_MS; + if(abs(Oll_diff1) > abs(Orl_diff1) + && abs(Oll_diff2) > abs(Orl_diff2) + && abs(Oll_diff3) > abs(Orl_diff3) + && abs(Olu_diff1) > abs(Oru_diff1) + && abs(Olu_diff2) > abs(Oru_diff2) + && abs(Olu_diff3) > abs(Oru_diff3) + )//判断偏左还是偏右 { App.LeftRunFalg = true; - if((Olu_diff1+Oru_diff1) < -(App.ADTrack.Stop_VPT) - && ((Olu_diff2+Oru_diff2) < -(App.ADTrack.Stop_VPT)) - && ((Olu_diff3+Oru_diff3) < -(App.ADTrack.Stop_VPT)) + 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(((CompareAndCountAbs(ll_ad,lu_ad,App.aaa,\ +// App.ADTrack.LeftLow_Org_AdZero,App.ADTrack.LeftUp_Org_AdZero,\ +// App.ccc) \ +// + \ +// CompareAndCountAbs(rl_ad,ru_ad,App.aaa,\ +// App.ADTrack.RightLow_Org_AdZero,App.ADTrack.RightUp_Org_AdZero,\ +// App.ccc) +// ) >= App.bbb) +// )//判断是否不是正面压 +// if((abs(Oru_diff1) < abs(Orl_diff1)*2)//判断是否不是正面压 +// && (abs(Oru_diff2) < abs(Orl_diff2)*2) +// && (abs(Oru_diff3) < abs(Orl_diff3)*2)) { - 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) + 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(((CompareAndCountAbs(lu_ad,ll_ad,App.aaa,\ +// App.ADTrack.LeftUp_Org_AdZero,App.ADTrack.LeftLow_Org_AdZero,\ +// App.ccc) \ +// + \ +// CompareAndCountAbs(ru_ad,rl_ad,App.aaa,\ +// App.ADTrack.RightUp_Org_AdZero,App.ADTrack.RightLow_Org_AdZero,\ +// App.ccc) +// ) >= App.bbb) +// )//判断是否不是正面压 +// if((abs(Oru_diff1) > abs(Orl_diff1)*2)//判断是否不是正面压 +// && (abs(Oru_diff2) > abs(Orl_diff2)*2) +// && (abs(Oru_diff3) > abs(Orl_diff3)*2)) { - 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) + else if(abs(Oll_diff1) < abs(Orl_diff1) + && abs(Oll_diff2) < abs(Orl_diff2) + && abs(Oll_diff3) < abs(Orl_diff3) + && abs(Olu_diff1) < abs(Oru_diff1) + && abs(Olu_diff2) < abs(Oru_diff2) + && abs(Olu_diff3) < abs(Oru_diff3) + )//判断偏左还是偏右 { 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(((//|arr1[i] - zero1| + offset > |arr2[i] - zero2| +// CompareAndCountAbs(ll_ad,lu_ad,App.aaa,\ +// App.ADTrack.LeftLow_Org_AdZero,App.ADTrack.LeftUp_Org_AdZero,\ +// App.ccc) + \ +// CompareAndCountAbs(rl_ad,ru_ad,App.aaa,\ +// App.ADTrack.RightLow_Org_AdZero,App.ADTrack.RightUp_Org_AdZero,\ +// App.ccc)) >= App.bbb) +// )//判断是否不是正面压 +// if((abs(Olu_diff1) < abs(Oll_diff1)*2)//判断是否不是正面压 +// && (abs(Olu_diff2) < abs(Oll_diff2)*2) +// && (abs(Olu_diff3) < abs(Oll_diff3)*2)) { - 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) + 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(((//|arr1[i] - zero1| + offset > |arr2[i] - zero2| +// CompareAndCountAbs(lu_ad,ll_ad,App.aaa,\ +// App.ADTrack.LeftUp_Org_AdZero,App.ADTrack.LeftLow_Org_AdZero,\ +// App.ccc) + \ +// CompareAndCountAbs(ru_ad,rl_ad,App.aaa,\ +// App.ADTrack.RightUp_Org_AdZero,App.ADTrack.RightLow_Org_AdZero,\ +// App.ccc)) >= App.bbb) +// )//判断是否不是正面压 +// if((abs(Olu_diff1) > abs(Oll_diff1)*2)//判断是否不是正面压 +// && (abs(Olu_diff2) > abs(Oll_diff2)*2) +// && (abs(Olu_diff3) > abs(Oll_diff3)*2)) { - 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((LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum))//判断左边操作 + else if((LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum && App.CentCollDelay1mSCnt <= 0))//判断左边操作 { App.LeftRunFalg = true; if((Olu_diff1 >= (App.ADTrack.LeftUpVPT*5/6)) //通过差值判断上下拉方向 @@ -428,43 +488,52 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (Oll_diff2 <= -(App.ADTrack.LeftLowVPT*5/6)) && (Oll_diff3 <= -(App.ADTrack.LeftLowVPT*5/6))) { - if((App.LeftUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压 + if(((App.LeftUpCS1237_SlopeArray[0] <= -PRESS_VPT)//判断是否不是正面压 && (App.LeftUpCS1237_SlopeArray[1] <= -PRESS_VPT) && (App.LeftUpCS1237_SlopeArray[2] <= -PRESS_VPT) && (App.LeftLowCS1237_SlopeArray[0] >= PRESS_VPT) && (App.LeftLowCS1237_SlopeArray[1] >= PRESS_VPT) - && (App.LeftLowCS1237_SlopeArray[2] >= PRESS_VPT)) + && (App.LeftLowCS1237_SlopeArray[2] >= PRESS_VPT) + && (App.RightUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压 + && (App.RightUpCS1237_SlopeArray[1] >= PRESS_VPT) + && (App.RightUpCS1237_SlopeArray[2] >= PRESS_VPT) + && (App.RightLowCS1237_SlopeArray[0] <= -PRESS_VPT) + && (App.RightLowCS1237_SlopeArray[1] <= -PRESS_VPT) + && (App.RightLowCS1237_SlopeArray[2] <= -PRESS_VPT)) + || ((App.LeftUpCS1237_SlopeArray[0] <= -PRESS_VPT)//判断是否不是正面压 + && (App.LeftUpCS1237_SlopeArray[1] <= -PRESS_VPT) + && (App.LeftUpCS1237_SlopeArray[2] <= -PRESS_VPT) + && (App.LeftLowCS1237_SlopeArray[0] >= PRESS_VPT) + && (App.LeftLowCS1237_SlopeArray[1] >= PRESS_VPT) + && (App.LeftLowCS1237_SlopeArray[2] >= PRESS_VPT) + && (App.RightUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压 + && (App.RightUpCS1237_SlopeArray[1] <= -PRESS_VPT) + && (App.RightUpCS1237_SlopeArray[2] <= -PRESS_VPT) + && (App.RightLowCS1237_SlopeArray[0] <= -PRESS_VPT) + && (App.RightLowCS1237_SlopeArray[1] <= -PRESS_VPT) + && (App.RightLowCS1237_SlopeArray[2] <= -PRESS_VPT)) + ) { - if(App.CaliZeroFlag == true)//判断是否校零 +// if(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(Olu_diff1) > abs(Oru_diff1) * SHIELD_VPT)//判断是否不是侧面推拉 + && (abs(Olu_diff2) > abs(Oru_diff2) * SHIELD_VPT) + && (abs(Olu_diff3) > abs(Oru_diff3) * SHIELD_VPT) + && (abs(Oll_diff1) > abs(Orl_diff1) * SHIELD_VPT) + && (abs(Oll_diff3) > abs(Orl_diff2) * SHIELD_VPT) + && (abs(Oll_diff3) > abs(Orl_diff3) * SHIELD_VPT) + ) { - if(App.MotorPara.Current_height > 730) + if(App.MotorPara.Current_height < 1195) { - 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(); + BDC_UP(); App.CaliZeroFlag = false; - App.UpFlag = false; - App.DownFlag = true; + App.UpFlag = true; + App.DownFlag = false; App.StopFlag = false; App.EmeFlag = false; App.RunCollDelay1mSCnt = RUN_TIME_MS; + } } } } @@ -477,49 +546,58 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (Oll_diff3 >= (App.ADTrack.LeftLowVPT*5/6))) ) { - if((App.LeftUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压 + if(((App.LeftUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压 && (App.LeftUpCS1237_SlopeArray[1] >= PRESS_VPT) && (App.LeftUpCS1237_SlopeArray[2] >= PRESS_VPT) && (App.LeftLowCS1237_SlopeArray[0] <= -PRESS_VPT) && (App.LeftLowCS1237_SlopeArray[1] <= -PRESS_VPT) - && (App.LeftLowCS1237_SlopeArray[2] <= -PRESS_VPT)) + && (App.LeftLowCS1237_SlopeArray[2] <= -PRESS_VPT) + &&(App.RightUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压 + && (App.RightUpCS1237_SlopeArray[1] <= -PRESS_VPT) + && (App.RightUpCS1237_SlopeArray[2] <= -PRESS_VPT) + && (App.RightLowCS1237_SlopeArray[0] >= PRESS_VPT) + && (App.RightLowCS1237_SlopeArray[1] >= PRESS_VPT) + && (App.RightLowCS1237_SlopeArray[2] >= PRESS_VPT)) + ||((App.LeftUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压 + && (App.LeftUpCS1237_SlopeArray[1] >= PRESS_VPT) + && (App.LeftUpCS1237_SlopeArray[2] >= PRESS_VPT) + && (App.LeftLowCS1237_SlopeArray[0] <= -PRESS_VPT) + && (App.LeftLowCS1237_SlopeArray[1] <= -PRESS_VPT) + && (App.LeftLowCS1237_SlopeArray[2] <= -PRESS_VPT) + &&(App.RightUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压 + && (App.RightUpCS1237_SlopeArray[1] >= PRESS_VPT) + && (App.RightUpCS1237_SlopeArray[2] >= PRESS_VPT) + && (App.RightLowCS1237_SlopeArray[0] >= PRESS_VPT) + && (App.RightLowCS1237_SlopeArray[1] >= PRESS_VPT) + && (App.RightLowCS1237_SlopeArray[2] >= PRESS_VPT)) + ) { // if(App.CaliZeroFlag == true)//判断是否校零 { - if((abs(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(Olu_diff1) > abs(Oru_diff1) * SHIELD_VPT)//判断是否不是侧面推拉 + && (abs(Olu_diff2) > abs(Oru_diff2) * SHIELD_VPT) + && (abs(Olu_diff3) > abs(Oru_diff3) * SHIELD_VPT) + && (abs(Oll_diff1) > abs(Orl_diff1) * SHIELD_VPT) + && (abs(Oll_diff3) > abs(Orl_diff2) * SHIELD_VPT) + && (abs(Oll_diff3) > abs(Orl_diff3) * SHIELD_VPT) + ) { - if(App.MotorPara.Current_height < 1200) + if(App.MotorPara.Current_height > 725) { - BDC_UP(); - App.CaliZeroFlag = false; - App.UpFlag = true; - App.DownFlag = false; - App.StopFlag = false; - App.EmeFlag = false; - App.RunCollDelay1mSCnt = RUN_TIME_MS; + 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 < 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)) //通过差值判断上下拉方向 @@ -529,21 +607,100 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (Orl_diff2 <= -(App.ADTrack.RightLowVPT*5/6)) && (Orl_diff3 <= -(App.ADTrack.RightLowVPT*5/6))) { - if((App.RightUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压 + if(((App.RightUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压 && (App.RightUpCS1237_SlopeArray[1] <= -PRESS_VPT) && (App.RightUpCS1237_SlopeArray[2] <= -PRESS_VPT) && (App.RightLowCS1237_SlopeArray[0] >= PRESS_VPT) && (App.RightLowCS1237_SlopeArray[1] >= PRESS_VPT) - && (App.RightLowCS1237_SlopeArray[2] >= PRESS_VPT)) + && (App.RightLowCS1237_SlopeArray[2] >= PRESS_VPT) + && (App.LeftUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压 + && (App.LeftUpCS1237_SlopeArray[1] >= PRESS_VPT) + && (App.LeftUpCS1237_SlopeArray[2] >= PRESS_VPT) + && (App.LeftLowCS1237_SlopeArray[0] <= -PRESS_VPT) + && (App.LeftLowCS1237_SlopeArray[1] <= -PRESS_VPT) + && (App.LeftLowCS1237_SlopeArray[2] <= -PRESS_VPT)) + || ((App.RightUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压 + && (App.RightUpCS1237_SlopeArray[1] <= -PRESS_VPT) + && (App.RightUpCS1237_SlopeArray[2] <= -PRESS_VPT) + && (App.RightLowCS1237_SlopeArray[0] >= PRESS_VPT) + && (App.RightLowCS1237_SlopeArray[1] >= PRESS_VPT) + && (App.RightLowCS1237_SlopeArray[2] >= PRESS_VPT) + && (App.LeftUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压 + && (App.LeftUpCS1237_SlopeArray[1] <= -PRESS_VPT) + && (App.LeftUpCS1237_SlopeArray[2] <= -PRESS_VPT) + && (App.LeftLowCS1237_SlopeArray[0] <= -PRESS_VPT) + && (App.LeftLowCS1237_SlopeArray[1] <= -PRESS_VPT) + && (App.LeftLowCS1237_SlopeArray[2] <= -PRESS_VPT)) + ) { // if(App.CaliZeroFlag == true)//判断是否校零 { - if((abs(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) * SHIELD_VPT)//判断是否不是侧面推拉 + && (abs(Oru_diff2) > abs(Olu_diff2) * SHIELD_VPT) + && (abs(Oru_diff3) > abs(Olu_diff3) * SHIELD_VPT) + && (abs(Orl_diff1) > abs(Oll_diff1) * SHIELD_VPT) + && (abs(Orl_diff3) > abs(Oll_diff2) * SHIELD_VPT) + && (abs(Orl_diff3) > abs(Oll_diff3) * SHIELD_VPT) + ) { - if(App.MotorPara.Current_height > 730) + if(App.MotorPara.Current_height < 1195) + { + BDC_UP(); + App.CaliZeroFlag = false; + App.UpFlag = true; + App.StopFlag = false; + App.DownFlag = false; + App.EmeFlag = false; + App.RunCollDelay1mSCnt = RUN_TIME_MS; + } + } + } + } + } + else if((Oru_diff1 <= -(App.ADTrack.RightUpVPT*5/6)) //通过差值判断上下拉方向 + && (Oru_diff2 <= -(App.ADTrack.RightUpVPT*5/6)) + && (Oru_diff3 <= -(App.ADTrack.RightUpVPT*5/6)) + && (Orl_diff1 >= (App.ADTrack.RightLowVPT*5/6)) + && (Orl_diff2 >= (App.ADTrack.RightLowVPT*5/6)) + && (Orl_diff3 >= (App.ADTrack.RightLowVPT*5/6))) + { + if(((App.RightUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压 + && (App.RightUpCS1237_SlopeArray[1] >= PRESS_VPT) + && (App.RightUpCS1237_SlopeArray[2] >= PRESS_VPT) + && (App.RightLowCS1237_SlopeArray[0] <= -PRESS_VPT) + && (App.RightLowCS1237_SlopeArray[1] <= -PRESS_VPT) + && (App.RightLowCS1237_SlopeArray[2] <= -PRESS_VPT) + && (App.LeftUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压 + && (App.LeftUpCS1237_SlopeArray[1] <= -PRESS_VPT) + && (App.LeftUpCS1237_SlopeArray[2] <= -PRESS_VPT) + && (App.LeftLowCS1237_SlopeArray[0] >= PRESS_VPT) + && (App.LeftLowCS1237_SlopeArray[1] >= PRESS_VPT) + && (App.LeftLowCS1237_SlopeArray[2] >= PRESS_VPT)) + || ((App.RightUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压 + && (App.RightUpCS1237_SlopeArray[1] >= PRESS_VPT) + && (App.RightUpCS1237_SlopeArray[2] >= PRESS_VPT) + && (App.RightLowCS1237_SlopeArray[0] <= -PRESS_VPT) + && (App.RightLowCS1237_SlopeArray[1] <= -PRESS_VPT) + && (App.RightLowCS1237_SlopeArray[2] <= -PRESS_VPT) + && (App.LeftUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压 + && (App.LeftUpCS1237_SlopeArray[1] >= PRESS_VPT) + && (App.LeftUpCS1237_SlopeArray[2] >= PRESS_VPT) + && (App.LeftLowCS1237_SlopeArray[0] >= PRESS_VPT) + && (App.LeftLowCS1237_SlopeArray[1] >= PRESS_VPT) + && (App.LeftLowCS1237_SlopeArray[2] >= PRESS_VPT)) + ) + { +// if(App.CaliZeroFlag == true)//判断是否校零 + { + if((abs(Oru_diff1) > abs(Olu_diff1) * SHIELD_VPT)//判断是否不是侧面推拉 + && (abs(Oru_diff2) > abs(Olu_diff2) * SHIELD_VPT) + && (abs(Oru_diff3) > abs(Olu_diff3) * SHIELD_VPT) + && (abs(Orl_diff1) > abs(Oll_diff1) * SHIELD_VPT) + && (abs(Orl_diff3) > abs(Oll_diff2) * SHIELD_VPT) + && (abs(Orl_diff3) > abs(Oll_diff3) * SHIELD_VPT) + ) + { + if(App.MotorPara.Current_height > 725) { BDC_DOWN(); App.CaliZeroFlag = false; @@ -555,67 +712,6 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ } } } -// 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)) //通过差值判断上下拉方向 - && (Oru_diff2 <= -(App.ADTrack.RightUpVPT*5/6)) - && (Oru_diff3 <= -(App.ADTrack.RightUpVPT*5/6)) - && (Orl_diff1 >= (App.ADTrack.RightLowVPT*5/6)) - && (Orl_diff2 >= (App.ADTrack.RightLowVPT*5/6)) - && (Orl_diff3 >= (App.ADTrack.RightLowVPT*5/6))) - { - if((App.RightUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压 - && (App.RightUpCS1237_SlopeArray[1] >= PRESS_VPT) - && (App.RightUpCS1237_SlopeArray[2] >= PRESS_VPT) - && (App.RightLowCS1237_SlopeArray[0] <= -PRESS_VPT) - && (App.RightLowCS1237_SlopeArray[1] <= -PRESS_VPT) - && (App.RightLowCS1237_SlopeArray[2] <= -PRESS_VPT)) - { -// if(App.CaliZeroFlag == true)//判断是否校零 - { - 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; - } - } - } -// 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; -// } -// } } } } @@ -624,24 +720,26 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ { if(App.LeftRunFalg == true) { - if(((App.CentreRunFalg == false ) - && (Olu_diff1 < App.ADTrack.Stop_VPT) - && (Oll_diff1 > -App.ADTrack.Stop_VPT) - && (Olu_diff2 < App.ADTrack.Stop_VPT) - && (Oll_diff2 > -App.ADTrack.Stop_VPT) - && (Olu_diff3 < App.ADTrack.Stop_VPT) - && (Oll_diff3 > -App.ADTrack.Stop_VPT) - && (Olu_diff4 < App.ADTrack.Stop_VPT) - && (Oll_diff4 > -App.ADTrack.Stop_VPT) - && (Olu_diff5 < App.ADTrack.Stop_VPT) - && (Oll_diff5 > -App.ADTrack.Stop_VPT) - && (Olu_diff6 < App.ADTrack.Stop_VPT) - && (Oll_diff6 > -App.ADTrack.Stop_VPT)) - || ((App.CentreRunFalg == true) - && ((Olu_diff1 + Oru_diff1) < (App.ADTrack.Stop_VPT*2/3)) - && ((Olu_diff2 + Oru_diff2) < (App.ADTrack.Stop_VPT*2/3)) - && ((Olu_diff3 + Oru_diff3) < (App.ADTrack.Stop_VPT*2/3))) - ) + if(((App.CentreRunFalg == false) + && (Olu_diff1 > -App.ADTrack.Stop_VPT) + && (Oll_diff1 < App.ADTrack.Stop_VPT) + && (Olu_diff2 > -App.ADTrack.Stop_VPT) + && (Oll_diff2 < App.ADTrack.Stop_VPT) + && (Olu_diff3 > -App.ADTrack.Stop_VPT) + && (Oll_diff3 < App.ADTrack.Stop_VPT) + && (Olu_diff4 > -App.ADTrack.Stop_VPT) + && (Oll_diff4 < App.ADTrack.Stop_VPT) + && (Olu_diff5 > -App.ADTrack.Stop_VPT) + && (Oll_diff5 < App.ADTrack.Stop_VPT) + && (Olu_diff6 > -App.ADTrack.Stop_VPT) + && (Oll_diff6 < App.ADTrack.Stop_VPT)) + || ((App.CentreRunFalg == true) + && ((Olu_diff1 + Oru_diff1) > -(App.ADTrack.Stop_VPT*2/3)) + && ((Olu_diff2 + Oru_diff2) > -(App.ADTrack.Stop_VPT*2/3)) + && ((Olu_diff3 + Oru_diff3) > -(App.ADTrack.Stop_VPT*2/3))) + || App.MotorPara.Current_height >= 1195 + || App.MotorPara.Current_height <= 725 + ) { BDC_STOP(); App.StopFlag = true; @@ -668,23 +766,25 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ else if(App.RightRunFalg == true) { if(((App.CentreRunFalg == false ) - && (Oru_diff1 < App.ADTrack.Stop_VPT) - && (Orl_diff1 > -App.ADTrack.Stop_VPT) - && (Oru_diff2 < App.ADTrack.Stop_VPT) - && (Orl_diff2 > -App.ADTrack.Stop_VPT) - && (Oru_diff3 < App.ADTrack.Stop_VPT) - && (Orl_diff3 > -App.ADTrack.Stop_VPT) - && (Oru_diff4 < App.ADTrack.Stop_VPT) - && (Orl_diff4 > -App.ADTrack.Stop_VPT) - && (Oru_diff5 < App.ADTrack.Stop_VPT) - && (Orl_diff5 > -App.ADTrack.Stop_VPT) - && (Oru_diff6 < App.ADTrack.Stop_VPT) - && (Orl_diff6 > -App.ADTrack.Stop_VPT)) - || ((App.CentreRunFalg == true) - && ((Olu_diff1 + Oru_diff1) < (App.ADTrack.Stop_VPT*2/3)) - && ((Olu_diff2 + Oru_diff2) < (App.ADTrack.Stop_VPT*2/3)) - && ((Olu_diff3 + Oru_diff3) < (App.ADTrack.Stop_VPT*2/3))) - ) + && (Oru_diff1 > -App.ADTrack.Stop_VPT) + && (Orl_diff1 < App.ADTrack.Stop_VPT) + && (Oru_diff2 > -App.ADTrack.Stop_VPT) + && (Orl_diff2 < App.ADTrack.Stop_VPT) + && (Oru_diff3 > -App.ADTrack.Stop_VPT) + && (Orl_diff3 < App.ADTrack.Stop_VPT) + && (Oru_diff4 > -App.ADTrack.Stop_VPT) + && (Orl_diff4 < App.ADTrack.Stop_VPT) + && (Oru_diff5 > -App.ADTrack.Stop_VPT) + && (Orl_diff5 < App.ADTrack.Stop_VPT) + && (Oru_diff6 > -App.ADTrack.Stop_VPT) + && (Orl_diff6 < App.ADTrack.Stop_VPT)) + || ((App.CentreRunFalg == true) + && ((Olu_diff1 + Oru_diff1) > -(App.ADTrack.Stop_VPT*2/3)) + && ((Olu_diff2 + Oru_diff2) > -(App.ADTrack.Stop_VPT*2/3)) + && ((Olu_diff3 + Oru_diff3) > -(App.ADTrack.Stop_VPT*2/3))) + || App.MotorPara.Current_height >= 1195 + || App.MotorPara.Current_height <= 725 + ) { BDC_STOP(); App.StopFlag = true; @@ -714,23 +814,25 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ if(App.LeftRunFalg == true) { if(((App.CentreRunFalg == false) - && (Olu_diff1 > -App.ADTrack.Stop_VPT) - && (Oll_diff1 < App.ADTrack.Stop_VPT) - && (Olu_diff2 > -App.ADTrack.Stop_VPT) - && (Oll_diff2 < App.ADTrack.Stop_VPT) - && (Olu_diff3 > -App.ADTrack.Stop_VPT) - && (Oll_diff3 < App.ADTrack.Stop_VPT) - && (Olu_diff4 > -App.ADTrack.Stop_VPT) - && (Oll_diff4 < App.ADTrack.Stop_VPT) - && (Olu_diff5 > -App.ADTrack.Stop_VPT) - && (Oll_diff5 < App.ADTrack.Stop_VPT) - && (Olu_diff6 > -App.ADTrack.Stop_VPT) - && (Oll_diff6 < App.ADTrack.Stop_VPT)) - || ((App.CentreRunFalg == true) - && ((Olu_diff1 + Oru_diff1) > -(App.ADTrack.Stop_VPT*2/3)) - && ((Olu_diff2 + Oru_diff2) > -(App.ADTrack.Stop_VPT*2/3)) - && ((Olu_diff3 + Oru_diff3) > -(App.ADTrack.Stop_VPT*2/3))) - ) + && (Olu_diff1 < App.ADTrack.Stop_VPT) + && (Oll_diff1 > -App.ADTrack.Stop_VPT) + && (Olu_diff2 < App.ADTrack.Stop_VPT) + && (Oll_diff2 > -App.ADTrack.Stop_VPT) + && (Olu_diff3 < App.ADTrack.Stop_VPT) + && (Oll_diff3 > -App.ADTrack.Stop_VPT) + && (Olu_diff4 < App.ADTrack.Stop_VPT) + && (Oll_diff4 > -App.ADTrack.Stop_VPT) + && (Olu_diff5 < App.ADTrack.Stop_VPT) + && (Oll_diff5 > -App.ADTrack.Stop_VPT) + && (Olu_diff6 < App.ADTrack.Stop_VPT) + && (Oll_diff6 > -App.ADTrack.Stop_VPT)) + || ((App.CentreRunFalg == true) + && ((Olu_diff1 + Oru_diff1) < (App.ADTrack.Stop_VPT*2/3)) + && ((Olu_diff2 + Oru_diff2) < (App.ADTrack.Stop_VPT*2/3)) + && ((Olu_diff3 + Oru_diff3) < (App.ADTrack.Stop_VPT*2/3))) + || App.MotorPara.Current_height >= 1195 + || App.MotorPara.Current_height <= 725 + ) { BDC_STOP(); App.StopFlag = true; @@ -756,24 +858,26 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ } else if(App.RightRunFalg == true) { - if(((App.CentreRunFalg == false ) - && (Oru_diff1 > -App.ADTrack.Stop_VPT) - && (Orl_diff1 < App.ADTrack.Stop_VPT) - && (Oru_diff2 > -App.ADTrack.Stop_VPT) - && (Orl_diff2 < App.ADTrack.Stop_VPT) - && (Oru_diff3 > -App.ADTrack.Stop_VPT) - && (Orl_diff3 < App.ADTrack.Stop_VPT) - && (Oru_diff4 > -App.ADTrack.Stop_VPT) - && (Orl_diff4 < App.ADTrack.Stop_VPT) - && (Oru_diff5 > -App.ADTrack.Stop_VPT) - && (Orl_diff5 < App.ADTrack.Stop_VPT) - && (Oru_diff6 > -App.ADTrack.Stop_VPT) - && (Orl_diff6 < App.ADTrack.Stop_VPT)) - || ((App.CentreRunFalg == true) - && ((Olu_diff1 + Oru_diff1) > -(App.ADTrack.Stop_VPT*2/3)) - && ((Olu_diff2 + Oru_diff2) > -(App.ADTrack.Stop_VPT*2/3)) - && ((Olu_diff3 + Oru_diff3) > -(App.ADTrack.Stop_VPT*2/3))) - ) + if(((App.CentreRunFalg == false ) + && (Oru_diff1 < App.ADTrack.Stop_VPT) + && (Orl_diff1 > -App.ADTrack.Stop_VPT) + && (Oru_diff2 < App.ADTrack.Stop_VPT) + && (Orl_diff2 > -App.ADTrack.Stop_VPT) + && (Oru_diff3 < App.ADTrack.Stop_VPT) + && (Orl_diff3 > -App.ADTrack.Stop_VPT) + && (Oru_diff4 < App.ADTrack.Stop_VPT) + && (Orl_diff4 > -App.ADTrack.Stop_VPT) + && (Oru_diff5 < App.ADTrack.Stop_VPT) + && (Orl_diff5 > -App.ADTrack.Stop_VPT) + && (Oru_diff6 < App.ADTrack.Stop_VPT) + && (Orl_diff6 > -App.ADTrack.Stop_VPT)) + || ((App.CentreRunFalg == true) + && ((Olu_diff1 + Oru_diff1) < (App.ADTrack.Stop_VPT*2/3)) + && ((Olu_diff2 + Oru_diff2) < (App.ADTrack.Stop_VPT*2/3)) + && ((Olu_diff3 + Oru_diff3) < (App.ADTrack.Stop_VPT*2/3))) + || App.MotorPara.Current_height >= 1195 + || App.MotorPara.Current_height <= 725 + ) { BDC_STOP(); App.StopFlag = true; @@ -835,8 +939,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ { if(App.StopFlag == true && App.StopCollDelay1mSCnt <= 0 - && App.MotorPara.Current_height <= 1200 - && App.MotorPara.Current_height >= 722){ + && App.MotorPara.Current_height <= 1195 + && App.MotorPara.Current_height >= 725){ App.ADTrack.LeftUp_Org_AdZero = lu_ad[0]; App.ADTrack.LeftLow_Org_AdZero = ll_ad[0]; App.ADTrack.RightUp_Org_AdZero = ru_ad[0]; @@ -878,13 +982,10 @@ static void AppInit(void) RIGHTLOW_CS1237_Init(); BDC_STOP(); - -// App.Status = APP_STATUS_WAIT; + LED_RED_OFF(); + LED_GREEN_ON(); 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; GradeControl(App.Para.Layout.ControlLevel);//等级控制 @@ -1004,6 +1105,9 @@ static void AppLoopHandler(void) break;} case APP_STATUS_MOTOR_INIT:{ + Beeper_Start(200,0,1); + LED_RED_ON(); + LED_GREEN_OFF(); BDC_UP(); Ddl_Delay1ms(1000); BDC_DOWN(); @@ -1017,6 +1121,8 @@ static void AppLoopHandler(void) if(App.TD1mSDelayCnt <= 0) { BDC_STOP(); + LED_RED_OFF(); + LED_GREEN_ON(); App.StopFlag = true; App.DownFlag = false; App.UpFlag = false; @@ -1025,6 +1131,8 @@ static void AppLoopHandler(void) if(App.MotorPara.Current_height <= 722 && App.MotorPara.Current_height >= 720) { BDC_STOP(); + LED_RED_OFF(); + LED_GREEN_ON(); App.StopFlag = true; App.DownFlag = false; App.UpFlag = false; @@ -1054,6 +1162,8 @@ static void AppLoopHandler(void) App.StopFlag = false; App.UpFlag = true; BDC_UP(); + LED_RED_ON(); + LED_GREEN_OFF(); App.CailUp1mSCnt = 0; App.MotorPara.LoopOne = false; } @@ -1076,6 +1186,8 @@ static void AppLoopHandler(void) else if(App.MotorPara.CaliNum >= 480 && App.MotorPara.LoopOne == true) { BDC_STOP(); + LED_RED_OFF(); + LED_GREEN_ON(); App.StopFlag = true; App.DownFlag = false; App.UpFlag = false; @@ -1114,6 +1226,8 @@ static void AppLoopHandler(void) else if(App.MotorPara.CaliNum >= 480 && App.MotorPara.LoopOne == true) { BDC_STOP(); + LED_RED_OFF(); + LED_GREEN_ON(); App.StopFlag = true; App.DownFlag = false; App.UpFlag = false; @@ -1137,6 +1251,7 @@ static void AppLoopHandler(void) case APP_STATUS_WAIT_STEADY:{ if(App.TD1mSDelayCnt <= 0) { + Beeper_Start(200,200,2); App.SideFlag = false; App.CaliZeroFlag = false; App.UpFlag = false; @@ -1149,22 +1264,26 @@ static void AppLoopHandler(void) break;} case APP_STATUS_HEIGHT_CONTROL:{ - if(App.MotorPara.Current_height < 722 + if(App.MotorPara.Current_height <= 725 && App.MotorPara.RestFlag == false - && App.HeightContRolCnt >= 500) + && App.HeightContRolCnt >= 500 + && GET_BIT0_DOUT() == 1 + && GET_BIT1_DOUT() == 1) { App.HeightContRolCnt = 0; BDC_UP(); Ddl_Delay1ms(500); BDC_STOP(); } - else if(App.MotorPara.Current_height > 1200 + else if(App.MotorPara.Current_height >= 1195 && App.MotorPara.RestFlag == false - && App.HeightContRolCnt >= 500) + && App.HeightContRolCnt >= 500 + && GET_BIT0_DOUT() == 1 + && GET_BIT1_DOUT() == 1) { App.HeightContRolCnt = 0; BDC_DOWN(); - Ddl_Delay1ms(400); + Ddl_Delay1ms(500); BDC_STOP(); } break;} @@ -1188,9 +1307,11 @@ static void App1mSRoutine(void) App.EmergencyDelay1mSCnt--; if(App.SlopeCollDelay1mSCnt > 0) App.SlopeCollDelay1mSCnt--; + if(App.CentCollDelay1mSCnt > 0) + App.CentCollDelay1mSCnt--; if(App.MotorPara.InitialFlag == true) App.CailUp1mSCnt++; - if((App.MotorPara.Current_height < 722 || App.MotorPara.Current_height > 1200)) + if((App.MotorPara.Current_height < 725 || App.MotorPara.Current_height > 1195)) { App.HeightContRolCnt++; } @@ -1211,6 +1332,12 @@ int main(void) { BSP_Init(); AppInit(); + + App.aaa = CENTRE_FRONT_CNT; + App.bbb = CENTRE_FRONT_SUM; + App.ccc = CENTRE_FRONT_VPT; + App.ddd = CENTRE_VPT; + while(1) { #if(USE_WDT == 1) FeedDogHandler(); @@ -1230,6 +1357,7 @@ int main(void) #endif ReadLoopHandler(); DcmotorLoopHandler(); + BeeperLoopHandler(); AdcLoopHander(); AppLoopHandler(); } @@ -1249,6 +1377,7 @@ void SysTick_IrqHandler(void) LEFTLOW_CS12371mSRoutine(); RIGHTUP_CS12371mSRoutine(); RIGHTLOW_CS12371mSRoutine(); + Beeper_ClearDelay(); Dcmotor1msRoutine(); Adc1mSRoutine(); } @@ -1319,7 +1448,7 @@ void Usart3RxIrqCallback(void) { App.MotorPara.Current_height = h; } - if(App.MotorPara.InitialFlag == true && h >=720 && h <= 1208) + if(App.MotorPara.InitialFlag == true && h >=720 && h <= 1200) {//缓存每个高度点对于零点应力的差值，对不同高度点的应力进行补偿 if(App.MotorPara.LU_HeightStress[(App.MotorPara.Current_height)-720] == 0 || App.MotorPara.LL_HeightStress[(App.MotorPara.Current_height)-720] == 0