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));