From 18c48e59a11889b50e4f9b9b325cb574c1fd4296 Mon Sep 17 00:00:00 2001 From: "DESKTOP-KQHDCM4\\97555" <975559679@qq.com> Date: Mon, 30 Mar 2026 14:46:47 +0800 Subject: [PATCH] =?UTF-8?q?2026=E5=B9=B43=E6=9C=8824=E6=97=A5=EF=BC=8C?= =?UTF-8?q?=E5=85=B4=E5=85=88=E8=BE=BE=E8=AE=BE=E5=A4=87=E8=B0=83=E8=AF=95?= =?UTF-8?q?=E7=89=88=E6=9C=AC=EF=BC=8C=E6=A0=A1=E5=87=86=E7=AE=97=E6=B3=95?= =?UTF-8?q?=E5=88=9D=E7=89=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Project/HC32F460/MDK/HC32F460.uvoptx | 49 ++- Project/HC32F460/source/User/Inc/Algorithm.h | 6 +- Project/HC32F460/source/User/Inc/bsp.h | 2 + Project/HC32F460/source/User/Inc/main.h | 7 +- Project/HC32F460/source/User/Src/Algorithm.c | 304 ++++++++++--- Project/HC32F460/source/User/Src/main.c | 441 ++++++++++--------- 6 files changed, 524 insertions(+), 285 deletions(-) diff --git a/Project/HC32F460/MDK/HC32F460.uvoptx b/Project/HC32F460/MDK/HC32F460.uvoptx index fe7fab1..a29ea74 100644 --- a/Project/HC32F460/MDK/HC32F460.uvoptx +++ b/Project/HC32F460/MDK/HC32F460.uvoptx @@ -103,7 +103,7 @@ 1 0 0 - 3 + 4 @@ -114,7 +114,7 @@ - BIN\CMSIS_AGDI.dll + Segger\JL2CM3.dll @@ -145,7 +145,7 @@ 0 CMSIS_AGDI - -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) + -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) 0 @@ -243,7 +243,7 @@ 17 1 - App.MotorPara.RL_HeightStress + App.MotorPara.RL_HeightStress,0x0A 18 @@ -263,17 +263,52 @@ 21 1 - App.MotorPara.Current_height,0x0A + App.Status 22 1 - App.Status + App.MotorPara.CaliNum,0x0A 23 1 - App.MotorPara.CaliNum,0x0A + 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 diff --git a/Project/HC32F460/source/User/Inc/Algorithm.h b/Project/HC32F460/source/User/Inc/Algorithm.h index f9ca1b7..60dd0b4 100644 --- a/Project/HC32F460/source/User/Inc/Algorithm.h +++ b/Project/HC32F460/source/User/Inc/Algorithm.h @@ -49,7 +49,11 @@ bool count_smaller(int32_t *arr, uint8_t size, int32_t target); uint16_t CRC_Modbus(uint16_t wBase, uint8_t *para, uint16_t length); uint8_t CRC_Sum(uint8_t *_pbuff, uint16_t _cmdLen); bool isAllZero(uint8_t *arr, int size); -int fixWithMedianWindow(int arr[], int size, int windowSize, int threshold); +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); #endif diff --git a/Project/HC32F460/source/User/Inc/bsp.h b/Project/HC32F460/source/User/Inc/bsp.h index 9c11293..df63b87 100644 --- a/Project/HC32F460/source/User/Inc/bsp.h +++ b/Project/HC32F460/source/User/Inc/bsp.h @@ -204,12 +204,14 @@ #define TWIN_BIT0_PINx (Pin12) #define SET_TWIN_BIT0() PORT_SetBits(TWIN_BIT0_PORTx, TWIN_BIT0_PINx)//拉高 #define CLR_TWIN_BIT0() PORT_ResetBits(TWIN_BIT0_PORTx, TWIN_BIT0_PINx)//拉低 +#define GET_BIT0_DOUT() PORT_GetBit(TWIN_BIT0_PORTx, TWIN_BIT0_PINx) //< BIT1 #define TWIN_BIT1_PORTx (PortB) #define TWIN_BIT1_PINx (Pin13) #define SET_TWIN_BIT1() PORT_SetBits(TWIN_BIT1_PORTx, TWIN_BIT1_PINx)//拉高 #define CLR_TWIN_BIT1() PORT_ResetBits(TWIN_BIT1_PORTx, TWIN_BIT1_PINx)//拉低 +#define GET_BIT1_DOUT() PORT_GetBit(TWIN_BIT1_PORTx, TWIN_BIT1_PINx) typedef struct { int32_t ControlLevel;//控制等级 diff --git a/Project/HC32F460/source/User/Inc/main.h b/Project/HC32F460/source/User/Inc/main.h index 213641a..943c989 100644 --- a/Project/HC32F460/source/User/Inc/main.h +++ b/Project/HC32F460/source/User/Inc/main.h @@ -31,7 +31,7 @@ #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_STOP() Motor_Control(MOTOR_CMD_STOP) //#define BDC_UP() Motor_Control(MOTOR_CMD_FORWARD) //#define BDC_DOWN() Motor_Control(MOTOR_CMD_REVERSE) @@ -67,7 +67,9 @@ typedef struct{ }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];//左上传感器对应高度应力 @@ -101,6 +103,7 @@ typedef enum { APP_STATUS_WAIT_MOTOR_CALI_DOWN, APP_STATUS_WAIT_STEADY, APP_STATUS_ZERO_MARKING, + APP_STATUS_HEIGHT_CONTROL, }AppStatus_m; typedef struct{ @@ -134,6 +137,8 @@ typedef struct { uint32_t RunCollDelay1mSCnt; uint32_t EmergencyDelay1mSCnt; uint32_t SlopeCollDelay1mSCnt; + uint32_t CailUp1mSCnt; + uint32_t HeightContRolCnt; uint8_t FeedDogDlyCnt; bool CaliZeroFlag;//校零标志位 diff --git a/Project/HC32F460/source/User/Src/Algorithm.c b/Project/HC32F460/source/User/Src/Algorithm.c index ca7235f..beeb12c 100644 --- a/Project/HC32F460/source/User/Src/Algorithm.c +++ b/Project/HC32F460/source/User/Src/Algorithm.c @@ -3,7 +3,137 @@ #include "bsp.h" #include "Algorithm.h" #include "Debug.h" - + +/** + * 加强特定区域的平滑处理（使用更小的权重衰减，更平滑的效果） + * + * @param arr 待处理的int32_t数组 + * @param size 数组的大小 + * @param windowSize 滑动窗口大小 + * @param enhanceStart 加强区域的起始索引（包含） + * @param enhanceEnd 加强区域的结束索引（包含） + * @param enhanceFactor 加强因子（>1表示更强的平滑，建议2-5） + * @param verbose 是否打印详细调试信息 + * @return 返回修正的次数 + */ +int weightedMovingAverageWithEnhance(int32_t arr[], int size, int windowSize, + int enhanceStart, int enhanceEnd, + double enhanceFactor, int verbose) { + if (size <= 1 || windowSize < 2) return 0; + + int32_t *temp = (int32_t*)malloc(size * sizeof(int32_t)); + if (temp == NULL) { + DBG_LOG("内存分配失败！\n"); + return 0; + } + + int halfWindow = windowSize / 2; + int fixCount = 0; + + if (verbose) { + DBG_LOG("加权移动平均平滑 (窗口大小: %d)\n", windowSize); + DBG_LOG("加强平滑区域: [%d - %d], 加强因子: %.1f\n", enhanceStart, enhanceEnd, enhanceFactor); + DBG_LOG("========================================\n"); + } + + for (int i = 0; i < size; i++) { + // 判断是否在加强区域内 + int isEnhanced = (i >= enhanceStart && i <= enhanceEnd); + + // 计算窗口边界 + int start = (i - halfWindow) < 0 ? 0 : (i - halfWindow); + int end = (i + halfWindow) >= size ? (size - 1) : (i + halfWindow); + + // 计算加权平均值 + double weightedSum = 0; + double weightSum = 0; + + for (int j = start; j <= end; j++) { + // 权重：距离越近权重越大 + int distance = abs(j - i); + double weight = 1.0 / (distance + 1); + + // 如果在加强区域内，且当前点不是中心点，可以调整权重 + if (isEnhanced && j != i) { + // 加强区域内的参考点权重提高，使平滑效果更强 + weight *= enhanceFactor; + } + + weightedSum += arr[j] * weight; + weightSum += weight; + } + + int newValue = (int)(weightedSum / weightSum + 0.5); + + if (newValue != arr[i]) { + if (verbose && fixCount < 50) { + DBG_LOG("位置 [%d]: %d -> %d %s\n", i, arr[i], newValue, + isEnhanced ? "[加强区域]" : ""); + } + temp[i] = newValue; + fixCount++; + } else { + temp[i] = arr[i]; + } + } + + // 写回原数组 + memcpy(arr, temp, size * sizeof(int32_t)); + free(temp); + + if (verbose) { + DBG_LOG("========================================\n"); + DBG_LOG("共修正 %d 个点（加强区域修正了更多点）\n", fixCount); + } + + return fixCount; +} +int finalCheck(int arr[], int size, int threshold, int verbose) { + int problemCount = 0; + + if (verbose) { + DBG_LOG("\n========== 最终检查 ==========\n"); + } + + for (int i = 1; i < size - 1; i++) { + int diffPrev = abs(arr[i] - arr[i-1]); + int diffNext = abs(arr[i+1] - arr[i]); + + if (diffPrev > threshold && diffNext > threshold) { + problemCount++; + if (verbose) { + DBG_LOG("警告: 位置 [%d] 仍有突变问题 (前差%d, 后差%d)\n", i, diffPrev, diffNext); + } + } + } + + // 检查首尾 + if (size >= 2) { + if (abs(arr[0] - arr[1]) > threshold * 2) { + problemCount++; + if (verbose) { + DBG_LOG("警告: 位置 [0] 仍有突变问题 (与第二元素差%d)\n", abs(arr[0] - arr[1])); + } + } + if (abs(arr[size-1] - arr[size-2]) > threshold * 2) { + problemCount++; + if (verbose) { + DBG_LOG("警告: 位置 [%d] 仍有突变问题 (与前元素差%d)\n", size-1, abs(arr[size-1] - arr[size-2])); + } + } + } + + if (verbose) { + if (problemCount == 0) { + DBG_LOG("检查通过！没有发现突变点\n"); + } else { + DBG_LOG("发现 %d 个问题点\n", problemCount); + } + DBG_LOG("========================================\n"); + } + + return problemCount; +} /** * 使用滑动窗口修正突变值，并打印修改信息 * @@ -13,102 +143,138 @@ * @param threshold 突变阈值 * @return 返回修正的次数 */ -int fixWithMedianWindow(int arr[], int size, int windowSize, int threshold) { - if (size <= 2 || windowSize < 3) { +int fixWithMedianIterative(int arr[], int size, int windowSize, int threshold, int maxIterations, int verbose) { + if (arr == NULL || size <= 2 || windowSize < 3) { return 0; } - int fixCount = 0; + int totalFixCount = 0; + int iteration = 0; + int changed = 1; + int halfWindow = windowSize / 2; + int *temp = (int*)malloc(size * sizeof(int)); if (temp == NULL) { DBG_LOG("内存分配失败！\n"); return 0; } - memcpy(temp, arr, size * sizeof(int)); + if (verbose) { + DBG_LOG("========== 开始迭代修正 (中位数窗口) ==========\n"); + DBG_LOG("数组大小: %d, 窗口大小: %d, 阈值: %d, 最大迭代: %d\n", + size, windowSize, threshold, maxIterations); + DBG_LOG("========================================\n"); + } - DBG_LOG("开始检测并修正突变点 (使用中位数窗口)...\n"); - DBG_LOG("数组大小: %d, 窗口大小: %d, 阈值: %d\n", size, windowSize, threshold); - DBG_LOG("========================================\n"); - - // 检测并修正 - for (int i = 0; i < size; i++) { - // 计算窗口边界 - int halfWindow = windowSize / 2; - int start = (i - halfWindow) < 0 ? 0 : (i - halfWindow); - int end = (i + halfWindow) >= size ? (size - 1) : (i + halfWindow); - int windowActualSize = end - start + 1; + while (changed && iteration < maxIterations) { + changed = 0; + int iterFixCount = 0; + memcpy(temp, arr, size * sizeof(int)); - // 检查当前点是否为突变点 - int isMutated = 0; - if (i > 0 && i < size - 1) { - int diffPrev = abs(arr[i] - arr[i-1]); - int diffNext = abs(arr[i+1] - arr[i]); - if (diffPrev > threshold && diffNext > threshold) { - isMutated = 1; - } + if (verbose) { + DBG_LOG("\n第 %d 次迭代:\n", iteration + 1); + DBG_LOG("----------------------------------------\n"); } - if (isMutated || (i > 0 && abs(arr[i] - arr[i-1]) > threshold * 2)) { - // 收集窗口内的值 - int *window = (int*)malloc(windowActualSize * sizeof(int)); - if (window == NULL) continue; + // 处理所有点 + for (int i = 0; i < size; i++) { + int isAbnormal = 0; - int idx = 0; - for (int j = start; j <= end; j++) { - if (j != i) { // 排除自身 - window[idx++] = arr[j]; + // 判断是否为异常点 + if (i > 0 && i < size - 1) { + // 中间点：检查前后突变 + int diffPrev = abs(arr[i] - arr[i-1]); + int diffNext = abs(arr[i+1] - arr[i]); + if (diffPrev > threshold && diffNext > threshold) { + isAbnormal = 1; + } + } else if (i == 0 && size >= 2) { + // 首元素：检查与第二个元素的差异 + if (abs(arr[0] - arr[1]) > threshold * 2) { + isAbnormal = 1; + } + } else if (i == size - 1 && size >= 2) { + // 尾元素：检查与倒数第二个元素的差异 + if (abs(arr[size-1] - arr[size-2]) > threshold * 2) { + isAbnormal = 1; } } - // 如果窗口内有效值太少，使用前后值平均 - if (idx < 2) { - if (i > 0 && i < size - 1) { - int correctedValue = (arr[i-1] + arr[i+1]) / 2; - DBG_LOG("位置 [%d]: 原值 %d -> 新值 %d (窗口内无足够参考值)\n", - i, arr[i], correctedValue); - temp[i] = correctedValue; - fixCount++; - } - free(window); - continue; - } - - // 排序找中位数（简单冒泡排序，因为窗口通常不大） - for (int m = 0; m < idx - 1; m++) { - for (int n = 0; n < idx - m - 1; n++) { - if (window[n] > window[n+1]) { - int tmp = window[n]; - window[n] = window[n+1]; - window[n+1] = tmp; + if (isAbnormal) { + // 计算窗口边界 + int start = (i - halfWindow) < 0 ? 0 : (i - halfWindow); + int end = (i + halfWindow) >= size ? (size - 1) : (i + halfWindow); + + // 收集窗口内的值（排除自身） + int windowActualSize = end - start + 1; + int *window = (int*)malloc(windowActualSize * sizeof(int)); + if (window == NULL) continue; + + int idx = 0; + for (int j = start; j <= end; j++) { + if (j != i) { + window[idx++] = arr[j]; } } + + int correctedValue; + if (idx > 0) { + // 排序找中位数 + for (int m = 0; m < idx - 1; m++) { + for (int n = 0; n < idx - m - 1; n++) { + if (window[n] > window[n+1]) { + int tmp = window[n]; + window[n] = window[n+1]; + window[n+1] = tmp; + } + } + } + correctedValue = window[idx / 2]; + } else { + // 没有参考值，使用相邻点平均 + if (i > 0 && i < size - 1) { + correctedValue = (arr[i-1] + arr[i+1]) / 2; + } else if (i == 0) { + correctedValue = arr[1]; + } else { + correctedValue = arr[size-2]; + } + } + + if (correctedValue != arr[i]) { + if (verbose) { + DBG_LOG("位置 [%d]: %d -> %d (窗口[%d-%d], 中位数:%d)\n", + i, arr[i], correctedValue, start, end, correctedValue); + } + temp[i] = correctedValue; + iterFixCount++; + changed = 1; + } + + free(window); } - - // 取中位数 - int correctedValue = window[idx / 2]; - - DBG_LOG("位置 [%d]: 原值 %d -> 新值 %d\n", i, arr[i], correctedValue); - DBG_LOG(" 窗口范围: [%d-%d], 使用中位数: %d\n", start, end, correctedValue); - - temp[i] = correctedValue; - fixCount++; - free(window); } + + if (changed) { + memcpy(arr, temp, size * sizeof(int)); + totalFixCount += iterFixCount; + if (verbose) { + DBG_LOG("本次修正了 %d 个点\n", iterFixCount); + } + } + + iteration++; } - // 将修正后的值写回原数组 - memcpy(arr, temp, size * sizeof(int)); free(temp); - if (fixCount == 0) { - DBG_LOG("未发现突变点\n"); - } else { + if (verbose) { + DBG_LOG("========================================\n"); + DBG_LOG("迭代完成！共进行了 %d 次迭代，总修正 %d 个点\n", iteration, totalFixCount); DBG_LOG("========================================\n"); - DBG_LOG("共修正了 %d 个突变点\n", fixCount); } - return fixCount; + return totalFixCount; } /***************************************************************************************** * 函数名称: check_peaks_valleys_ratio diff --git a/Project/HC32F460/source/User/Src/main.c b/Project/HC32F460/source/User/Src/main.c index e2e4168..590b557 100644 --- a/Project/HC32F460/source/User/Src/main.c +++ b/Project/HC32F460/source/User/Src/main.c @@ -151,7 +151,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 = 5000*4/5; + App.ADTrack.Stop_VPT = 4000; break;} case LEVEL_2:{ @@ -164,23 +164,10 @@ void GradeControl(ControlLevel_m level) App.ADTrack.LeftLowShieldVPT = SHIELD_VPT; App.ADTrack.RightUpShieldVPT = SHIELD_VPT; App.ADTrack.RightLowShieldVPT = SHIELD_VPT; - App.ADTrack.Stop_VPT = 10000*4/5; + App.ADTrack.Stop_VPT = 8000; break;} case LEVEL_3:{ - App.ADTrack.Slope_VPT = SLOPE_VPT; - 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_4:{ App.ADTrack.Slope_VPT = SLOPE_VPT; App.ADTrack.LeftUpVPT = 20000; App.ADTrack.LeftLowVPT = 20000; @@ -190,10 +177,10 @@ void GradeControl(ControlLevel_m level) App.ADTrack.LeftLowShieldVPT = SHIELD_VPT; App.ADTrack.RightUpShieldVPT = SHIELD_VPT; App.ADTrack.RightLowShieldVPT = SHIELD_VPT; - App.ADTrack.Stop_VPT = 15000; + App.ADTrack.Stop_VPT = 14000; break;} - case LEVEL_5:{ + case LEVEL_4:{ App.ADTrack.Slope_VPT = SLOPE_VPT; App.ADTrack.LeftUpVPT = 25000; App.ADTrack.LeftLowVPT = 25000; @@ -203,30 +190,58 @@ void GradeControl(ControlLevel_m level) App.ADTrack.LeftLowShieldVPT = SHIELD_VPT; App.ADTrack.RightUpShieldVPT = SHIELD_VPT; App.ADTrack.RightLowShieldVPT = SHIELD_VPT; - App.ADTrack.Stop_VPT = 20000; + 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; break;} } } #if(USE_AGC == 1) void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_t *rl_ad) { - int32_t lu_diff[OSC_CNT], Olu_diff1, Olu_diff2, Olu_diff3; - int32_t ll_diff[OSC_CNT], Oll_diff1, Oll_diff2, Oll_diff3; - int32_t ru_diff[OSC_CNT], Oru_diff1, Oru_diff2, Oru_diff3; - int32_t rl_diff[OSC_CNT], Orl_diff1, Orl_diff2, Orl_diff3; + int32_t lu_diff[OSC_CNT], Olu_diff1, Olu_diff2, Olu_diff3, Olu_diff4, Olu_diff5, Olu_diff6; + int32_t ll_diff[OSC_CNT], Oll_diff1, Oll_diff2, Oll_diff3, Oll_diff4, Oll_diff5, Oll_diff6; + int32_t ru_diff[OSC_CNT], Oru_diff1, Oru_diff2, Oru_diff3, Oru_diff4, Oru_diff5, Oru_diff6; + int32_t rl_diff[OSC_CNT], Orl_diff1, Orl_diff2, Orl_diff3, Orl_diff4 ,Orl_diff5, Orl_diff6; Olu_diff1 = lu_ad[0] - App.ADTrack.LeftUp_Org_AdZero; Olu_diff2 = lu_ad[1] - App.ADTrack.LeftUp_Org_AdZero; Olu_diff3 = lu_ad[2] - App.ADTrack.LeftUp_Org_AdZero; + Olu_diff4 = lu_ad[3] - App.ADTrack.LeftUp_Org_AdZero; + Olu_diff5 = lu_ad[4] - App.ADTrack.LeftUp_Org_AdZero; + Olu_diff6 = lu_ad[5] - App.ADTrack.LeftUp_Org_AdZero; + Oll_diff1 = ll_ad[0] - App.ADTrack.LeftLow_Org_AdZero; Oll_diff2 = ll_ad[1] - App.ADTrack.LeftLow_Org_AdZero; Oll_diff3 = ll_ad[2] - App.ADTrack.LeftLow_Org_AdZero; + Oll_diff4 = ll_ad[3] - App.ADTrack.LeftLow_Org_AdZero; + Oll_diff5 = ll_ad[4] - App.ADTrack.LeftLow_Org_AdZero; + Oll_diff6 = ll_ad[5] - App.ADTrack.LeftLow_Org_AdZero; + Oru_diff1 = ru_ad[0] - App.ADTrack.RightUp_Org_AdZero; Oru_diff2 = ru_ad[1] - App.ADTrack.RightUp_Org_AdZero; Oru_diff3 = ru_ad[2] - App.ADTrack.RightUp_Org_AdZero; + Oru_diff4 = ru_ad[3] - App.ADTrack.RightUp_Org_AdZero; + Oru_diff5 = ru_ad[4] - App.ADTrack.RightUp_Org_AdZero; + Oru_diff6 = ru_ad[5] - App.ADTrack.RightUp_Org_AdZero; + Orl_diff1 = rl_ad[0] - App.ADTrack.RightLow_Org_AdZero; Orl_diff2 = rl_ad[1] - App.ADTrack.RightLow_Org_AdZero; Orl_diff3 = rl_ad[2] - App.ADTrack.RightLow_Org_AdZero; + Orl_diff4 = rl_ad[3] - App.ADTrack.RightLow_Org_AdZero; + Orl_diff5 = rl_ad[4] - App.ADTrack.RightLow_Org_AdZero; + Orl_diff6 = rl_ad[5] - App.ADTrack.RightLow_Org_AdZero; for(int i = 0; i < OSC_CNT; i++) { @@ -338,7 +353,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && ((Olu_diff3+Oru_diff3) < -(App.ADTrack.Stop_VPT)) ) { - if(App.MotorPara.Current_height < 1195) + if(App.MotorPara.Current_height < 1200) { BDC_UP(); App.CaliZeroFlag = false; @@ -354,7 +369,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (Olu_diff3+Oru_diff3) > (App.ADTrack.Stop_VPT) ) { - if(App.MotorPara.Current_height > 725) + if(App.MotorPara.Current_height > 730) { BDC_DOWN(); App.CaliZeroFlag = false; @@ -374,7 +389,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (Olu_diff3+Oru_diff3) < -(App.ADTrack.Stop_VPT) ) { - if(App.MotorPara.Current_height < 1195) + if(App.MotorPara.Current_height < 1200) { BDC_UP(); App.CaliZeroFlag = false; @@ -390,7 +405,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (Olu_diff3+Oru_diff3) > (App.ADTrack.Stop_VPT) ) { - if(App.MotorPara.Current_height > 725) + if(App.MotorPara.Current_height > 730) { BDC_DOWN(); App.CaliZeroFlag = false; @@ -406,17 +421,9 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ else if((LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum))//判断左边操作 { App.LeftRunFalg = true; - if(( -// (App.SlopeCollDelay1mSCnt <= 0) -// && (App.LeftUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) //通过斜率判断上下拉方向 -// && (App.LeftUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) -// && (App.LeftUpCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT) -// && (App.LeftLowCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) -// && (App.LeftLowCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) -// && (App.LeftLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)) || ( - (Olu_diff1 >= (App.ADTrack.LeftUpVPT*5/6)) //通过差值判断上下拉方向 + if((Olu_diff1 >= (App.ADTrack.LeftUpVPT*5/6)) //通过差值判断上下拉方向 && (Olu_diff2 >= (App.ADTrack.LeftUpVPT*5/6)) - && (Olu_diff3 >= (App.ADTrack.LeftUpVPT*5/6))) + && (Olu_diff3 >= (App.ADTrack.LeftUpVPT*5/6)) && (Oll_diff1 <= -(App.ADTrack.LeftLowVPT*5/6)) && (Oll_diff2 <= -(App.ADTrack.LeftLowVPT*5/6)) && (Oll_diff3 <= -(App.ADTrack.LeftLowVPT*5/6))) @@ -435,7 +442,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ && (abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT) && (abs(Olu_diff1) > abs(Oru_diff1) + App.ADTrack.LeftUpShieldVPT)) { - if(App.MotorPara.Current_height > 725) + if(App.MotorPara.Current_height > 730) { BDC_DOWN(); App.CaliZeroFlag = false; @@ -449,7 +456,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ } else { - if(App.MotorPara.Current_height > 725) + if(App.MotorPara.Current_height > 730) { BDC_DOWN(); App.CaliZeroFlag = false; @@ -462,15 +469,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ } } } - else if(( -// (App.SlopeCollDelay1mSCnt <= 0) -// && (App.LeftUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) //通过斜率判断上下拉方向 -// && (App.LeftUpCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) -// && (App.LeftUpCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT) -// && (App.LeftLowCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) -// && (App.LeftLowCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) -// && (App.LeftLowCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)) || ( - (Olu_diff1 <= -(App.ADTrack.LeftUpVPT*5/6))//通过差值判断上下拉方向 + else if(((Olu_diff1 <= -(App.ADTrack.LeftUpVPT*5/6))//通过差值判断上下拉方向 && (Olu_diff2 <= -(App.ADTrack.LeftUpVPT*5/6)) && (Olu_diff3 <= -(App.ADTrack.LeftUpVPT*5/6)) && (Oll_diff1 >= (App.ADTrack.LeftLowVPT*5/6)) @@ -485,14 +484,14 @@ 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(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(App.MotorPara.Current_height < 1195) + if(App.MotorPara.Current_height < 1200) { BDC_UP(); App.CaliZeroFlag = false; @@ -504,38 +503,30 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ } } } - else - { - if(App.MotorPara.Current_height < 1195) - { - BDC_UP(); - App.CaliZeroFlag = false; - App.UpFlag = true; - App.DownFlag = false; - App.StopFlag = false; - App.EmeFlag = false; - App.RunCollDelay1mSCnt = RUN_TIME_MS; - } - } +// else +// { +// 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)//判断右边操作 { App.RightRunFalg = true; - if(( -// (App.SlopeCollDelay1mSCnt <= 0) -// && (App.RightUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) //通过斜率判断上下拉方向 -// && (App.RightUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) -// && (App.RightUpCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT) -// && (App.RightLowCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) -// && (App.RightLowCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) -// && (App.RightLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)) || ( - (Oru_diff1 >= (App.ADTrack.RightUpVPT*5/6)) //通过差值判断上下拉方向 + if((Oru_diff1 >= (App.ADTrack.RightUpVPT*5/6)) //通过差值判断上下拉方向 && (Oru_diff2 >= (App.ADTrack.RightUpVPT*5/6)) - && (Oru_diff3 >= (App.ADTrack.RightUpVPT*5/6))) + && (Oru_diff3 >= (App.ADTrack.RightUpVPT*5/6)) && (Orl_diff1 <= -(App.ADTrack.RightLowVPT*5/6)) - && (Orl_diff2 >= -(App.ADTrack.RightLowVPT*5/6)) + && (Orl_diff2 <= -(App.ADTrack.RightLowVPT*5/6)) && (Orl_diff3 <= -(App.ADTrack.RightLowVPT*5/6))) { if((App.RightUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压 @@ -545,14 +536,14 @@ 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(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(App.MotorPara.Current_height > 725) + if(App.MotorPara.Current_height > 730) { BDC_DOWN(); App.CaliZeroFlag = false; @@ -564,32 +555,24 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ } } } - else - { - if(App.MotorPara.Current_height > 725) - { - BDC_DOWN(); - App.CaliZeroFlag = false; - App.DownFlag = true; - App.StopFlag = false; - App.UpFlag = false; - App.EmeFlag = false; - App.RunCollDelay1mSCnt = RUN_TIME_MS; - } - } +// else +// { +// if(App.MotorPara.Current_height > 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(( -// (App.SlopeCollDelay1mSCnt <= 0) -// && (App.RightUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT) //通过斜率判断上下拉方向 -// && (App.RightUpCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT) -// && (App.RightUpCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT) -// && (App.RightLowCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT) -// && (App.RightLowCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT) -// && (App.RightLowCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT))|| ( - (Oru_diff1 <= -(App.ADTrack.RightUpVPT*5/6)) //通过差值判断上下拉方向 + else if((Oru_diff1 <= -(App.ADTrack.RightUpVPT*5/6)) //通过差值判断上下拉方向 && (Oru_diff2 <= -(App.ADTrack.RightUpVPT*5/6)) - && (Oru_diff3 <= -(App.ADTrack.RightUpVPT*5/6))) + && (Oru_diff3 <= -(App.ADTrack.RightUpVPT*5/6)) && (Orl_diff1 >= (App.ADTrack.RightLowVPT*5/6)) && (Orl_diff2 >= (App.ADTrack.RightLowVPT*5/6)) && (Orl_diff3 >= (App.ADTrack.RightLowVPT*5/6))) @@ -601,14 +584,14 @@ 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(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 < 1195) + if(App.MotorPara.Current_height < 1200) { BDC_UP(); App.CaliZeroFlag = false; @@ -620,19 +603,19 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ } } } - else - { - if(App.MotorPara.Current_height < 1195) - { - BDC_UP(); - App.CaliZeroFlag = false; - App.UpFlag = true; - App.StopFlag = false; - App.DownFlag = false; - App.EmeFlag = false; - App.RunCollDelay1mSCnt = RUN_TIME_MS; - } - } +// else +// { +// if(App.MotorPara.Current_height < 1190) +// { +// BDC_UP(); +// App.CaliZeroFlag = false; +// App.UpFlag = true; +// App.StopFlag = false; +// App.DownFlag = false; +// App.EmeFlag = false; +// App.RunCollDelay1mSCnt = RUN_TIME_MS; +// } +// } } } } @@ -641,25 +624,24 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ { if(App.LeftRunFalg == true) { - if(((App.CentreRunFalg == false ) && (lu_slope > App.ADTrack.Slope_VPT)) - || ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0)) - || (App.MotorPara.Current_height <= 725)) - { - BDC_STOP(); - 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(((App.CentreRunFalg == false ) && (Olu_diff1 < App.ADTrack.Stop_VPT) && (Olu_diff2 < App.ADTrack.Stop_VPT) && (Olu_diff3 < App.ADTrack.Stop_VPT)) + 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 <= 725)) + ) { BDC_STOP(); App.StopFlag = true; @@ -685,25 +667,24 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ } else if(App.RightRunFalg == true) { - if(((App.CentreRunFalg == false ) && (ru_slope > App.ADTrack.Slope_VPT)) - || ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0)) - || (App.MotorPara.Current_height <= 725)) - { - BDC_STOP(); - 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(((App.CentreRunFalg == false ) && (Oru_diff1 < App.ADTrack.Stop_VPT) && (Oru_diff2 < App.ADTrack.Stop_VPT) && (Oru_diff3 < App.ADTrack.Stop_VPT)) + 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 <= 725)) + ) { BDC_STOP(); App.StopFlag = true; @@ -732,25 +713,24 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ { if(App.LeftRunFalg == true) { - if(((App.CentreRunFalg == false ) && (lu_slope < -App.ADTrack.Slope_VPT)) - || ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0)) - || (App.MotorPara.Current_height >= 1195)) - { - BDC_STOP(); - 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(((App.CentreRunFalg == false ) && (Olu_diff1 > -App.ADTrack.Stop_VPT) && (Olu_diff2 > -App.ADTrack.Stop_VPT) && (Olu_diff3 > -App.ADTrack.Stop_VPT)) + 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)) + ) { BDC_STOP(); App.StopFlag = true; @@ -776,25 +756,24 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ } else if(App.RightRunFalg == true) { - if(((App.CentreRunFalg == false ) && (ru_slope < -App.ADTrack.Slope_VPT)) - || ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0)) - || (App.MotorPara.Current_height >= 1195)) - { - BDC_STOP(); - 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(((App.CentreRunFalg == false ) && (Oru_diff1 > -App.ADTrack.Stop_VPT) && (Oru_diff2 > -App.ADTrack.Stop_VPT) && (Oru_diff3 > -App.ADTrack.Stop_VPT)) + 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)) + ) { BDC_STOP(); App.StopFlag = true; @@ -820,7 +799,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_ } } - if ( + if( // lu_K.peaks_positive // && lu_K.valleys_negative App.LeftUpCS1237_SlopeArray[0] < FOLLOW_VPT @@ -856,8 +835,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 <= 1995 - && App.MotorPara.Current_height >= 725){ + && App.MotorPara.Current_height <= 1200 + && App.MotorPara.Current_height >= 722){ App.ADTrack.LeftUp_Org_AdZero = lu_ad[0]; App.ADTrack.LeftLow_Org_AdZero = ll_ad[0]; App.ADTrack.RightUp_Org_AdZero = ru_ad[0]; @@ -1043,7 +1022,7 @@ static void AppLoopHandler(void) App.UpFlag = false; App.Status = APP_STATUS_IDLE; } - if(App.MotorPara.Current_height <= 721 && App.MotorPara.Current_height >= 720) + if(App.MotorPara.Current_height <= 722 && App.MotorPara.Current_height >= 720) { BDC_STOP(); App.StopFlag = true; @@ -1073,15 +1052,17 @@ static void AppLoopHandler(void) App.MotorPara.CaliNum = 0; App.TD1mSDelayCnt = TRAVEL_TIME_MS; App.StopFlag = false; - App.UpFlag = true; + App.UpFlag = true; BDC_UP(); - //App.Status = APP_STATUS_WAIT_STEADY; + App.CailUp1mSCnt = 0; + App.MotorPara.LoopOne = false; } break;} case APP_STATUS_WAIT_MOTOR_CALI_UP:{ - if(App.MotorPara.Current_height >= 1204) + if(App.MotorPara.Current_height >= 1200 && App.CailUp1mSCnt >= 17000) { + App.CailUp1mSCnt = 0; BDC_STOP(); App.StopFlag = true; App.DownFlag = false; @@ -1092,22 +1073,23 @@ static void AppLoopHandler(void) App.Status = APP_STATUS_WAIT_MOTOR_CALI_DOWN; BDC_DOWN(); } - else if(App.MotorPara.CaliNum >= 488) + 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===================="); - fixWithMedianWindow(App.MotorPara.LU_HeightStress,sizeof(App.MotorPara.LU_HeightStress)/4,7,2000); - DBG_LOG("===================LL Data monitoring===================="); - fixWithMedianWindow(App.MotorPara.LL_HeightStress,sizeof(App.MotorPara.LL_HeightStress)/4,7,2000); - DBG_LOG("===================RU Data monitoring===================="); - fixWithMedianWindow(App.MotorPara.RU_HeightStress,sizeof(App.MotorPara.RU_HeightStress)/4,7,2000); - DBG_LOG("===================RL Data monitoring===================="); - fixWithMedianWindow(App.MotorPara.RL_HeightStress,sizeof(App.MotorPara.RL_HeightStress)/4,7,2000); + DBG_LOG("===================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; @@ -1115,34 +1097,37 @@ static void AppLoopHandler(void) break;} case APP_STATUS_WAIT_MOTOR_CALI_DOWN:{ - if(App.MotorPara.Current_height <= 721 && App.MotorPara.Current_height >= 720) + 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 >= 488) + 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"); - fixWithMedianWindow(App.MotorPara.LU_HeightStress,sizeof(App.MotorPara.LU_HeightStress)/4,7,2000); + weightedMovingAverageWithEnhance(App.MotorPara.LU_HeightStress,480,7,380,480,4,0); DBG_LOG("===================LL Data monitoring====================\r\n"); - fixWithMedianWindow(App.MotorPara.LL_HeightStress,sizeof(App.MotorPara.LL_HeightStress)/4,7,2000); + weightedMovingAverageWithEnhance(App.MotorPara.LL_HeightStress,480,7,380,480,4,0); DBG_LOG("===================RU Data monitoring====================\r\n"); - fixWithMedianWindow(App.MotorPara.RU_HeightStress,sizeof(App.MotorPara.RU_HeightStress)/4,7,2000); + weightedMovingAverageWithEnhance(App.MotorPara.RU_HeightStress,480,7,380,480,4,0); DBG_LOG("===================RL Data monitoring====================\r\n"); - fixWithMedianWindow(App.MotorPara.RL_HeightStress,sizeof(App.MotorPara.RL_HeightStress)/4,7,2000); + 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; @@ -1159,7 +1144,28 @@ static void AppLoopHandler(void) App.EmeFlag = false; App.StopFlag = true; App.WaitFlag = true; - App.Status = APP_STATUS_IDLE; + App.Status = APP_STATUS_HEIGHT_CONTROL; + } + 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(); } break;} } @@ -1182,6 +1188,16 @@ static void App1mSRoutine(void) 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; + } App.FeedDogDlyCnt++; } void FeedDogHandler(void) @@ -1271,40 +1287,51 @@ void Usart3RxIrqCallback(void) App.RxUart3Data.RxLen = 0; App.RxUart3Data.RxBuff[App.RxUart3Data.RxLen++] = Data; } + else if(App.RxUart3Data.RxLen == 0) + { + App.RxUart3Data.HeaderFrame = true; + App.RxUart3Data.RxLen = 0; + App.RxUart3Data.RxBuff[App.RxUart3Data.RxLen++] = Data; + } } else { - if(App.RxUart3Data.RxLen == 1 && Data != 0x01) - { - App.RxUart3Data.HeaderFrame = false; - return; - } +// if(App.RxUart3Data.RxLen == 1 && Data != 0x01) +// { +// App.RxUart3Data.HeaderFrame = false; +// return; +// } // 如果RxLen小于128，则将数据添加到RxBuff中 if(App.RxUart3Data.RxLen < 128) { App.RxUart3Data.RxBuff[App.RxUart3Data.RxLen++] = Data; } if(App.RxUart3Data.RxLen >= 4) { - if(App.RxUart3Data.RxBuff[0] == 0x01 && App.RxUart3Data.RxBuff[1] == 0x01) + if(App.RxUart3Data.RxBuff[0] == 0x01 && App.RxUart3Data.RxBuff[1] == 0x04) { + App.MotorPara.RestFlag = true; + } + 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) { App.MotorPara.Current_height = h; } - if(App.MotorPara.InitialFlag == true && h >=720 && h <= 1210) + 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.LU_HeightStress[(App.MotorPara.Current_height)-720] = App.ADTrack.LeftUp_Org_AdZero - App.LeftUpCS1237_TrendArray[0]; - App.MotorPara.LL_HeightStress[(App.MotorPara.Current_height)-720] = App.ADTrack.LeftLow_Org_AdZero - App.LeftLowCS1237_TrendArray[0]; - App.MotorPara.RU_HeightStress[(App.MotorPara.Current_height)-720] = App.ADTrack.RightUp_Org_AdZero - App.RightUpCS1237_TrendArray[0]; - App.MotorPara.RL_HeightStress[(App.MotorPara.Current_height)-720] = App.ADTrack.RightLow_Org_AdZero - App.RightLowCS1237_TrendArray[0]; App.MotorPara.CaliNum++; } + 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]; } } memset(App.RxUart3Data.RxBuff, 0, sizeof(App.RxUart3Data.RxBuff));