YuanHongbin/Acc_Support_29V_HV2.x
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

2026年3月24日，兴先达设备调试版本，校准算法初版

Browse Source
This commit is contained in:
DESKTOP-KQHDCM4\97555
2026-03-30 14:46:47 +08:00
parent 2657c1ca27
commit 18c48e59a1
6 changed files with 524 additions and 285 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Project/HC32F460/MDK/HC32F460.uvoptx
+42 -7
View File
@@ -103,7 +103,7 @@
<bEvRecOn>1</bEvRecOn> <bEvRecOn>1</bEvRecOn>
<bSchkAxf>0</bSchkAxf> <bSchkAxf>0</bSchkAxf>
<bTchkAxf>0</bTchkAxf> <bTchkAxf>0</bTchkAxf>
<nTsel>3</nTsel> <nTsel>4</nTsel>
<sDll></sDll> <sDll></sDll>
<sDllPa></sDllPa> <sDllPa></sDllPa>
<sDlgDll></sDlgDll> <sDlgDll></sDlgDll>
@@ -114,7 +114,7 @@
<tDlgDll></tDlgDll> <tDlgDll></tDlgDll>
<tDlgPa></tDlgPa> <tDlgPa></tDlgPa>
<tIfile></tIfile> <tIfile></tIfile>
<pMon>BIN\CMSIS_AGDI.dll</pMon> <pMon>Segger\JL2CM3.dll</pMon>
</DebugOpt> </DebugOpt>
<TargetDriverDllRegistry> <TargetDriverDllRegistry>
<SetRegEntry> <SetRegEntry>
@@ -145,7 +145,7 @@
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>CMSIS_AGDI</Key> <Key>CMSIS_AGDI</Key>
<Name>-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)</Name> <Name>-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)</Name>
</SetRegEntry> </SetRegEntry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
@@ -243,7 +243,7 @@
<Ww> <Ww>
<count>17</count> <count>17</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>App.MotorPara.RL_HeightStress</ItemText> <ItemText>App.MotorPara.RL_HeightStress,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>18</count> <count>18</count>
@@ -263,17 +263,52 @@
<Ww> <Ww>
<count>21</count> <count>21</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>App.MotorPara.Current_height,0x0A</ItemText> <ItemText>App.Status</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>22</count> <count>22</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>App.Status</ItemText> <ItemText>App.MotorPara.CaliNum,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>23</count> <count>23</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>App.MotorPara.CaliNum,0x0A</ItemText> <ItemText>App.CailUp1mSCnt,0x0A</ItemText>
</Ww>
<Ww>
<count>24</count>
<WinNumber>1</WinNumber>
<ItemText>Oru_diff1,0x0A</ItemText>
</Ww>
<Ww>
<count>25</count>
<WinNumber>1</WinNumber>
<ItemText>Orl_diff1,0x0A</ItemText>
</Ww>
<Ww>
<count>26</count>
<WinNumber>1</WinNumber>
<ItemText>App.ADTrack.Stop_VPT,0x0A</ItemText>
</Ww>
<Ww>
<count>27</count>
<WinNumber>1</WinNumber>
<ItemText>ru_ad[0],0x0A</ItemText>
</Ww>
<Ww>
<count>28</count>
<WinNumber>1</WinNumber>
<ItemText>App.MotorPara.RestFlag,0x0A</ItemText>
</Ww>
<Ww>
<count>29</count>
<WinNumber>1</WinNumber>
<ItemText>App.MotorPara.Current_height,0x0A</ItemText>
</Ww>
<Ww>
<count>30</count>
<WinNumber>1</WinNumber>
<ItemText>App.HeightContRolCnt,0x0A</ItemText>
</Ww> </Ww>
</WatchWindow1> </WatchWindow1>
<Tracepoint> <Tracepoint>
Project/HC32F460/source/User/Inc/Algorithm.h
+5 -1
View File
@@ -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); uint16_t CRC_Modbus(uint16_t wBase, uint8_t *para, uint16_t length);
uint8_t CRC_Sum(uint8_t *_pbuff, uint16_t _cmdLen); uint8_t CRC_Sum(uint8_t *_pbuff, uint16_t _cmdLen);
bool isAllZero(uint8_t *arr, int size); 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 #endif
Project/HC32F460/source/User/Inc/bsp.h
+2
View File
@@ -204,12 +204,14 @@
#define TWIN_BIT0_PINx (Pin12) #define TWIN_BIT0_PINx (Pin12)
#define SET_TWIN_BIT0() PORT_SetBits(TWIN_BIT0_PORTx, TWIN_BIT0_PINx)//拉高 #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 CLR_TWIN_BIT0() PORT_ResetBits(TWIN_BIT0_PORTx, TWIN_BIT0_PINx)//拉低
#define GET_BIT0_DOUT() PORT_GetBit(TWIN_BIT0_PORTx, TWIN_BIT0_PINx)
//< BIT1 //< BIT1
#define TWIN_BIT1_PORTx (PortB) #define TWIN_BIT1_PORTx (PortB)
#define TWIN_BIT1_PINx (Pin13) #define TWIN_BIT1_PINx (Pin13)
#define SET_TWIN_BIT1() PORT_SetBits(TWIN_BIT1_PORTx, TWIN_BIT1_PINx)//拉高 #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 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 { typedef struct {
int32_t ControlLevel;//控制等级 int32_t ControlLevel;//控制等级
Project/HC32F460/source/User/Inc/main.h
+5
View File
@@ -67,7 +67,9 @@ typedef struct{
}ADTrack_t; }ADTrack_t;
typedef struct{//电机参数 typedef struct{//电机参数
bool InitialFlag;//初始标志位 bool InitialFlag;//初始标志位
bool RestFlag;
bool CaliFlag; bool CaliFlag;
bool LoopOne;//循环一次标志位
int16_t Current_height;//当前高度mm int16_t Current_height;//当前高度mm
int16_t CaliNum;//标校数量 int16_t CaliNum;//标校数量
int32_t LU_HeightStress[488];//左上传感器对应高度应力 int32_t LU_HeightStress[488];//左上传感器对应高度应力
@@ -101,6 +103,7 @@ typedef enum {
APP_STATUS_WAIT_MOTOR_CALI_DOWN, APP_STATUS_WAIT_MOTOR_CALI_DOWN,
APP_STATUS_WAIT_STEADY, APP_STATUS_WAIT_STEADY,
APP_STATUS_ZERO_MARKING, APP_STATUS_ZERO_MARKING,
APP_STATUS_HEIGHT_CONTROL,
}AppStatus_m; }AppStatus_m;
typedef struct{ typedef struct{
@@ -134,6 +137,8 @@ typedef struct {
uint32_t RunCollDelay1mSCnt; uint32_t RunCollDelay1mSCnt;
uint32_t EmergencyDelay1mSCnt; uint32_t EmergencyDelay1mSCnt;
uint32_t SlopeCollDelay1mSCnt; uint32_t SlopeCollDelay1mSCnt;
uint32_t CailUp1mSCnt;
uint32_t HeightContRolCnt;
uint8_t FeedDogDlyCnt; uint8_t FeedDogDlyCnt;
bool CaliZeroFlag;//校零标志位 bool CaliZeroFlag;//校零标志位
Project/HC32F460/source/User/Src/Algorithm.c
+214 -48
View File
@@ -4,6 +4,136 @@
#include "Algorithm.h" #include "Algorithm.h"
#include "Debug.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,68 +143,83 @@
* @param threshold 突变阈值 * @param threshold 突变阈值
* @return 返回修正的次数 * @return 返回修正的次数
*/ */
int fixWithMedianWindow(int arr[], int size, int windowSize, int threshold) { int fixWithMedianIterative(int arr[], int size, int windowSize, int threshold, int maxIterations, int verbose) {
if (size <= 2 || windowSize < 3) { if (arr == NULL || size <= 2 || windowSize < 3) {
return 0; 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)); int *temp = (int*)malloc(size * sizeof(int));
if (temp == NULL) { if (temp == NULL) {
DBG_LOG("内存分配失败！\n"); DBG_LOG("内存分配失败！\n");
return 0; return 0;
} }
if (verbose) {
DBG_LOG("========== 开始迭代修正 (中位数窗口) ==========\n");
DBG_LOG("数组大小: %d, 窗口大小: %d, 阈值: %d, 最大迭代: %d\n",
size, windowSize, threshold, maxIterations);
DBG_LOG("========================================\n");
}
while (changed && iteration < maxIterations) {
changed = 0;
int iterFixCount = 0;
memcpy(temp, arr, size * sizeof(int)); memcpy(temp, arr, size * sizeof(int));
DBG_LOG("开始检测并修正突变点 (使用中位数窗口)...\n"); if (verbose) {
DBG_LOG("数组大小: %d, 窗口大小: %d, 阈值: %d\n", size, windowSize, threshold); DBG_LOG("\n第 %d 次迭代:\n", iteration + 1);
DBG_LOG("========================================\n"); DBG_LOG("----------------------------------------\n");
}
// 检测并修正 // 处理所有点
for (int i = 0; i < size; i++) { for (int i = 0; i < size; i++) {
// 计算窗口边界 int isAbnormal = 0;
int halfWindow = windowSize / 2;
int start = (i - halfWindow) < 0 ? 0 : (i - halfWindow);
int end = (i + halfWindow) >= size ? (size - 1) : (i + halfWindow);
int windowActualSize = end - start + 1;
// 检查当前点是否为突变点 // 判断是否为异常点
int isMutated = 0;
if (i > 0 && i < size - 1) { if (i > 0 && i < size - 1) {
// 中间点：检查前后突变
int diffPrev = abs(arr[i] - arr[i-1]); int diffPrev = abs(arr[i] - arr[i-1]);
int diffNext = abs(arr[i+1] - arr[i]); int diffNext = abs(arr[i+1] - arr[i]);
if (diffPrev > threshold && diffNext > threshold) { if (diffPrev > threshold && diffNext > threshold) {
isMutated = 1; 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 (isMutated || (i > 0 && abs(arr[i] - arr[i-1]) > threshold * 2)) { 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)); int *window = (int*)malloc(windowActualSize * sizeof(int));
if (window == NULL) continue; if (window == NULL) continue;
int idx = 0; int idx = 0;
for (int j = start; j <= end; j++) { for (int j = start; j <= end; j++) {
if (j != i) { // 排除自身 if (j != i) {
window[idx++] = arr[j]; window[idx++] = arr[j];
} }
} }
// 如果窗口内有效值太少，使用前后值平均 int correctedValue;
if (idx < 2) { if (idx > 0) {
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 m = 0; m < idx - 1; m++) {
for (int n = 0; n < idx - m - 1; n++) { for (int n = 0; n < idx - m - 1; n++) {
if (window[n] > window[n+1]) { if (window[n] > window[n+1]) {
@@ -84,31 +229,52 @@ int fixWithMedianWindow(int arr[], int size, int windowSize, int threshold) {
} }
} }
} }
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]) {
int correctedValue = window[idx / 2]; if (verbose) {
DBG_LOG("位置 [%d]: %d -> %d (窗口[%d-%d], 中位数:%d)\n",
DBG_LOG("位置 [%d]: 原值 %d -> 新值 %d\n", i, arr[i], correctedValue); i, arr[i], correctedValue, start, end, correctedValue);
DBG_LOG(" 窗口范围: [%d-%d], 使用中位数: %d\n", start, end, correctedValue); }
temp[i] = correctedValue; temp[i] = correctedValue;
fixCount++; iterFixCount++;
changed = 1;
}
free(window); free(window);
} }
} }
// 将修正后的值写回原数组 if (changed) {
memcpy(arr, temp, size * sizeof(int)); memcpy(arr, temp, size * sizeof(int));
free(temp); totalFixCount += iterFixCount;
if (verbose) {
if (fixCount == 0) { DBG_LOG("本次修正了 %d 个点\n", iterFixCount);
DBG_LOG("未发现突变点\n"); }
} else {
DBG_LOG("========================================\n");
DBG_LOG("共修正了 %d 个突变点\n", fixCount);
} }
return fixCount; iteration++;
}
free(temp);
if (verbose) {
DBG_LOG("========================================\n");
DBG_LOG("迭代完成！共进行了 %d 次迭代，总修正 %d 个点\n", iteration, totalFixCount);
DBG_LOG("========================================\n");
}
return totalFixCount;
} }
/***************************************************************************************** /*****************************************************************************************
* 函数名称: check_peaks_valleys_ratio * 函数名称: check_peaks_valleys_ratio
Project/HC32F460/source/User/Src/main.c
+231 -204
View File
@@ -151,7 +151,7 @@ void GradeControl(ControlLevel_m level)
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT; App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT; App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT; App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 5000*4/5; App.ADTrack.Stop_VPT = 4000;
break;} break;}
case LEVEL_2:{ case LEVEL_2:{
@@ -164,23 +164,10 @@ void GradeControl(ControlLevel_m level)
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT; App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT; App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT; App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 10000*4/5; App.ADTrack.Stop_VPT = 8000;
break;} break;}
case LEVEL_3:{ 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.Slope_VPT = SLOPE_VPT;
App.ADTrack.LeftUpVPT = 20000; App.ADTrack.LeftUpVPT = 20000;
App.ADTrack.LeftLowVPT = 20000; App.ADTrack.LeftLowVPT = 20000;
@@ -190,10 +177,10 @@ void GradeControl(ControlLevel_m level)
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT; App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT; App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT; App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
App.ADTrack.Stop_VPT = 15000; App.ADTrack.Stop_VPT = 14000;
break;} break;}
case LEVEL_5:{ case LEVEL_4:{
App.ADTrack.Slope_VPT = SLOPE_VPT; App.ADTrack.Slope_VPT = SLOPE_VPT;
App.ADTrack.LeftUpVPT = 25000; App.ADTrack.LeftUpVPT = 25000;
App.ADTrack.LeftLowVPT = 25000; App.ADTrack.LeftLowVPT = 25000;
@@ -203,30 +190,58 @@ void GradeControl(ControlLevel_m level)
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT; App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT; App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = 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;} break;}
} }
} }
#if(USE_AGC == 1) #if(USE_AGC == 1)
void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_t *rl_ad) 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 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; 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; 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; 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_diff1 = lu_ad[0] - App.ADTrack.LeftUp_Org_AdZero;
Olu_diff2 = lu_ad[1] - 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_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_diff1 = ll_ad[0] - App.ADTrack.LeftLow_Org_AdZero;
Oll_diff2 = ll_ad[1] - 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_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_diff1 = ru_ad[0] - App.ADTrack.RightUp_Org_AdZero;
Oru_diff2 = ru_ad[1] - 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_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_diff1 = rl_ad[0] - App.ADTrack.RightLow_Org_AdZero;
Orl_diff2 = rl_ad[1] - 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_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++) 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)) && ((Olu_diff3+Oru_diff3) < -(App.ADTrack.Stop_VPT))
) )
{ {
if(App.MotorPara.Current_height < 1195) if(App.MotorPara.Current_height < 1200)
{ {
BDC_UP(); BDC_UP();
App.CaliZeroFlag = false; 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) && (Olu_diff3+Oru_diff3) > (App.ADTrack.Stop_VPT)
) )
{ {
if(App.MotorPara.Current_height > 725) if(App.MotorPara.Current_height > 730)
{ {
BDC_DOWN(); BDC_DOWN();
App.CaliZeroFlag = false; 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) && (Olu_diff3+Oru_diff3) < -(App.ADTrack.Stop_VPT)
) )
{ {
if(App.MotorPara.Current_height < 1195) if(App.MotorPara.Current_height < 1200)
{ {
BDC_UP(); BDC_UP();
App.CaliZeroFlag = false; 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) && (Olu_diff3+Oru_diff3) > (App.ADTrack.Stop_VPT)
) )
{ {
if(App.MotorPara.Current_height > 725) if(App.MotorPara.Current_height > 730)
{ {
BDC_DOWN(); BDC_DOWN();
App.CaliZeroFlag = false; 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))//判断左边操作 else if((LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum))//判断左边操作
{ {
App.LeftRunFalg = true; App.LeftRunFalg = true;
if(( if((Olu_diff1 >= (App.ADTrack.LeftUpVPT*5/6)) //通过差值判断上下拉方向
// (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)) //通过差值判断上下拉方向
&& (Olu_diff2 >= (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_diff1 <= -(App.ADTrack.LeftLowVPT*5/6))
&& (Oll_diff2 <= -(App.ADTrack.LeftLowVPT*5/6)) && (Oll_diff2 <= -(App.ADTrack.LeftLowVPT*5/6))
&& (Oll_diff3 <= -(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(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT)
&& (abs(Olu_diff1) > abs(Oru_diff1) + App.ADTrack.LeftUpShieldVPT)) && (abs(Olu_diff1) > abs(Oru_diff1) + App.ADTrack.LeftUpShieldVPT))
{ {
if(App.MotorPara.Current_height > 725) if(App.MotorPara.Current_height > 730)
{ {
BDC_DOWN(); BDC_DOWN();
App.CaliZeroFlag = false; App.CaliZeroFlag = false;
@@ -449,7 +456,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
} }
else else
{ {
if(App.MotorPara.Current_height > 725) if(App.MotorPara.Current_height > 730)
{ {
BDC_DOWN(); BDC_DOWN();
App.CaliZeroFlag = false; App.CaliZeroFlag = false;
@@ -462,15 +469,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
} }
} }
} }
else if(( else if(((Olu_diff1 <= -(App.ADTrack.LeftUpVPT*5/6))//通过差值判断上下拉方向
// (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))//通过差值判断上下拉方向
&& (Olu_diff2 <= -(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_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[1] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] <= -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)//判断是否不是侧面推拉 if((abs(Oll_diff1) > abs(Orl_diff1) + App.ADTrack.LeftLowShieldVPT)//判断是否不是侧面推拉
&& (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT) && (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT)
&& (abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT) && (abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT)
&& (abs(Olu_diff1) > abs(Oru_diff1) + App.ADTrack.LeftUpShieldVPT)) && (abs(Olu_diff1) > abs(Oru_diff1) + App.ADTrack.LeftUpShieldVPT))
{ {
if(App.MotorPara.Current_height < 1195) if(App.MotorPara.Current_height < 1200)
{ {
BDC_UP(); BDC_UP();
App.CaliZeroFlag = false; App.CaliZeroFlag = false;
@@ -504,38 +503,30 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
} }
} }
} }
else // else
{ // {
if(App.MotorPara.Current_height < 1195) // if(App.MotorPara.Current_height < 1190)
{ // {
BDC_UP(); // BDC_UP();
App.CaliZeroFlag = false; // App.CaliZeroFlag = false;
App.UpFlag = true; // App.UpFlag = true;
App.DownFlag = false; // App.DownFlag = false;
App.StopFlag = false; // App.StopFlag = false;
App.EmeFlag = false; // App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; // App.RunCollDelay1mSCnt = RUN_TIME_MS;
} // }
} // }
} }
} }
} }
else if(LeftSlopeSum < RightSlopeSum && LeftDiffSum < RightDiffSum)//判断右边操作 else if(LeftSlopeSum < RightSlopeSum && LeftDiffSum < RightDiffSum)//判断右边操作
{ {
App.RightRunFalg = true; App.RightRunFalg = true;
if(( if((Oru_diff1 >= (App.ADTrack.RightUpVPT*5/6)) //通过差值判断上下拉方向
// (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)) //通过差值判断上下拉方向
&& (Oru_diff2 >= (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_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))) && (Orl_diff3 <= -(App.ADTrack.RightLowVPT*5/6)))
{ {
if((App.RightUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压 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[1] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] >= 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)//判断是否不是侧面推拉 if((abs(Orl_diff1) > abs(Oll_diff1) + App.ADTrack.RightLowShieldVPT)//判断是否不是侧面推拉
&& (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT) && (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT)
&& (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT) && (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT)
&& (abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT)) && (abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT))
{ {
if(App.MotorPara.Current_height > 725) if(App.MotorPara.Current_height > 730)
{ {
BDC_DOWN(); BDC_DOWN();
App.CaliZeroFlag = false; App.CaliZeroFlag = false;
@@ -564,32 +555,24 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
} }
} }
} }
else // else
{ // {
if(App.MotorPara.Current_height > 725) // if(App.MotorPara.Current_height > 730)
{ // {
BDC_DOWN(); // BDC_DOWN();
App.CaliZeroFlag = false; // App.CaliZeroFlag = false;
App.DownFlag = true; // App.DownFlag = true;
App.StopFlag = false; // App.StopFlag = false;
App.UpFlag = false; // App.UpFlag = false;
App.EmeFlag = false; // App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; // App.RunCollDelay1mSCnt = RUN_TIME_MS;
// }
// }
} }
} }
} else if((Oru_diff1 <= -(App.ADTrack.RightUpVPT*5/6)) //通过差值判断上下拉方向
}
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)) //通过差值判断上下拉方向
&& (Oru_diff2 <= -(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_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))) && (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[1] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] <= -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)//判断是否不是侧面推拉 if((abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT)//判断是否不是侧面推拉
&& (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT) && (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT)
&& (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT) && (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT)
&& (abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT)) && (abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT))
{ {
if(App.MotorPara.Current_height < 1195) if(App.MotorPara.Current_height < 1200)
{ {
BDC_UP(); BDC_UP();
App.CaliZeroFlag = false; App.CaliZeroFlag = false;
@@ -620,19 +603,19 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
} }
} }
} }
else // else
{ // {
if(App.MotorPara.Current_height < 1195) // if(App.MotorPara.Current_height < 1190)
{ // {
BDC_UP(); // BDC_UP();
App.CaliZeroFlag = false; // App.CaliZeroFlag = false;
App.UpFlag = true; // App.UpFlag = true;
App.StopFlag = false; // App.StopFlag = false;
App.DownFlag = false; // App.DownFlag = false;
App.EmeFlag = false; // App.EmeFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS; // 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.LeftRunFalg == true)
{ {
if(((App.CentreRunFalg == false ) && (lu_slope > App.ADTrack.Slope_VPT)) if(((App.CentreRunFalg == false )
|| ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0)) && (Olu_diff1 < App.ADTrack.Stop_VPT)
|| (App.MotorPara.Current_height <= 725)) && (Oll_diff1 > -App.ADTrack.Stop_VPT)
{ && (Olu_diff2 < App.ADTrack.Stop_VPT)
BDC_STOP(); && (Oll_diff2 > -App.ADTrack.Stop_VPT)
App.StopFlag = true; && (Olu_diff3 < App.ADTrack.Stop_VPT)
App.DownFlag = false; && (Oll_diff3 > -App.ADTrack.Stop_VPT)
App.UpFlag = false; && (Olu_diff4 < App.ADTrack.Stop_VPT)
App.EmeFlag = false; && (Oll_diff4 > -App.ADTrack.Stop_VPT)
App.StopCollDelay1mSCnt = STOP_TIME_MS; && (Olu_diff5 < App.ADTrack.Stop_VPT)
App.SlopeCollDelay1mSCnt = SLOPE_START_MS; && (Oll_diff5 > -App.ADTrack.Stop_VPT)
App.EmergencyDelay1mSCnt = EME_START_MS; && (Olu_diff6 < App.ADTrack.Stop_VPT)
} && (Oll_diff6 > -App.ADTrack.Stop_VPT))
else if(((App.CentreRunFalg == false ) && (Olu_diff1 < App.ADTrack.Stop_VPT) && (Olu_diff2 < App.ADTrack.Stop_VPT) && (Olu_diff3 < App.ADTrack.Stop_VPT))
|| ((App.CentreRunFalg == true) || ((App.CentreRunFalg == true)
&& ((Olu_diff1 + Oru_diff1) < (App.ADTrack.Stop_VPT*2/3)) && ((Olu_diff1 + Oru_diff1) < (App.ADTrack.Stop_VPT*2/3))
&& ((Olu_diff2 + Oru_diff2) < (App.ADTrack.Stop_VPT*2/3)) && ((Olu_diff2 + Oru_diff2) < (App.ADTrack.Stop_VPT*2/3))
&& ((Olu_diff3 + Oru_diff3) < (App.ADTrack.Stop_VPT*2/3))) && ((Olu_diff3 + Oru_diff3) < (App.ADTrack.Stop_VPT*2/3)))
|| (App.MotorPara.Current_height <= 725)) )
{ {
BDC_STOP(); BDC_STOP();
App.StopFlag = true; 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) else if(App.RightRunFalg == true)
{ {
if(((App.CentreRunFalg == false ) && (ru_slope > App.ADTrack.Slope_VPT)) if(((App.CentreRunFalg == false )
|| ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0)) && (Oru_diff1 < App.ADTrack.Stop_VPT)
|| (App.MotorPara.Current_height <= 725)) && (Orl_diff1 > -App.ADTrack.Stop_VPT)
{ && (Oru_diff2 < App.ADTrack.Stop_VPT)
BDC_STOP(); && (Orl_diff2 > -App.ADTrack.Stop_VPT)
App.StopFlag = true; && (Oru_diff3 < App.ADTrack.Stop_VPT)
App.DownFlag = false; && (Orl_diff3 > -App.ADTrack.Stop_VPT)
App.UpFlag = false; && (Oru_diff4 < App.ADTrack.Stop_VPT)
App.EmeFlag = false; && (Orl_diff4 > -App.ADTrack.Stop_VPT)
App.StopCollDelay1mSCnt = STOP_TIME_MS; && (Oru_diff5 < App.ADTrack.Stop_VPT)
App.SlopeCollDelay1mSCnt = SLOPE_START_MS; && (Orl_diff5 > -App.ADTrack.Stop_VPT)
App.EmergencyDelay1mSCnt = EME_START_MS; && (Oru_diff6 < App.ADTrack.Stop_VPT)
} && (Orl_diff6 > -App.ADTrack.Stop_VPT))
else if(((App.CentreRunFalg == false ) && (Oru_diff1 < App.ADTrack.Stop_VPT) && (Oru_diff2 < App.ADTrack.Stop_VPT) && (Oru_diff3 < App.ADTrack.Stop_VPT))
|| ((App.CentreRunFalg == true) || ((App.CentreRunFalg == true)
&& ((Olu_diff1 + Oru_diff1) < (App.ADTrack.Stop_VPT*2/3)) && ((Olu_diff1 + Oru_diff1) < (App.ADTrack.Stop_VPT*2/3))
&& ((Olu_diff2 + Oru_diff2) < (App.ADTrack.Stop_VPT*2/3)) && ((Olu_diff2 + Oru_diff2) < (App.ADTrack.Stop_VPT*2/3))
&& ((Olu_diff3 + Oru_diff3) < (App.ADTrack.Stop_VPT*2/3))) && ((Olu_diff3 + Oru_diff3) < (App.ADTrack.Stop_VPT*2/3)))
|| (App.MotorPara.Current_height <= 725)) )
{ {
BDC_STOP(); BDC_STOP();
App.StopFlag = true; 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.LeftRunFalg == true)
{ {
if(((App.CentreRunFalg == false ) && (lu_slope < -App.ADTrack.Slope_VPT)) if(((App.CentreRunFalg == false)
|| ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0)) && (Olu_diff1 > -App.ADTrack.Stop_VPT)
|| (App.MotorPara.Current_height >= 1195)) && (Oll_diff1 < App.ADTrack.Stop_VPT)
{ && (Olu_diff2 > -App.ADTrack.Stop_VPT)
BDC_STOP(); && (Oll_diff2 < App.ADTrack.Stop_VPT)
App.StopFlag = true; && (Olu_diff3 > -App.ADTrack.Stop_VPT)
App.DownFlag = false; && (Oll_diff3 < App.ADTrack.Stop_VPT)
App.UpFlag = false; && (Olu_diff4 > -App.ADTrack.Stop_VPT)
App.EmeFlag = false; && (Oll_diff4 < App.ADTrack.Stop_VPT)
App.StopCollDelay1mSCnt = STOP_TIME_MS; && (Olu_diff5 > -App.ADTrack.Stop_VPT)
App.SlopeCollDelay1mSCnt = SLOPE_START_MS; && (Oll_diff5 < App.ADTrack.Stop_VPT)
App.EmergencyDelay1mSCnt = EME_START_MS; && (Olu_diff6 > -App.ADTrack.Stop_VPT)
} && (Oll_diff6 < App.ADTrack.Stop_VPT))
else if(((App.CentreRunFalg == false ) && (Olu_diff1 > -App.ADTrack.Stop_VPT) && (Olu_diff2 > -App.ADTrack.Stop_VPT) && (Olu_diff3 > -App.ADTrack.Stop_VPT))
|| ((App.CentreRunFalg == true) || ((App.CentreRunFalg == true)
&& ((Olu_diff1 + Oru_diff1) > -(App.ADTrack.Stop_VPT*2/3)) && ((Olu_diff1 + Oru_diff1) > -(App.ADTrack.Stop_VPT*2/3))
&& ((Olu_diff2 + Oru_diff2) > -(App.ADTrack.Stop_VPT*2/3)) && ((Olu_diff2 + Oru_diff2) > -(App.ADTrack.Stop_VPT*2/3))
&& ((Olu_diff3 + Oru_diff3) > -(App.ADTrack.Stop_VPT*2/3))) && ((Olu_diff3 + Oru_diff3) > -(App.ADTrack.Stop_VPT*2/3)))
|| (App.MotorPara.Current_height >= 1195)) )
{ {
BDC_STOP(); BDC_STOP();
App.StopFlag = true; 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) else if(App.RightRunFalg == true)
{ {
if(((App.CentreRunFalg == false ) && (ru_slope < -App.ADTrack.Slope_VPT)) if(((App.CentreRunFalg == false )
|| ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0)) && (Oru_diff1 > -App.ADTrack.Stop_VPT)
|| (App.MotorPara.Current_height >= 1195)) && (Orl_diff1 < App.ADTrack.Stop_VPT)
{ && (Oru_diff2 > -App.ADTrack.Stop_VPT)
BDC_STOP(); && (Orl_diff2 < App.ADTrack.Stop_VPT)
App.StopFlag = true; && (Oru_diff3 > -App.ADTrack.Stop_VPT)
App.DownFlag = false; && (Orl_diff3 < App.ADTrack.Stop_VPT)
App.UpFlag = false; && (Oru_diff4 > -App.ADTrack.Stop_VPT)
App.EmeFlag = false; && (Orl_diff4 < App.ADTrack.Stop_VPT)
App.StopCollDelay1mSCnt = STOP_TIME_MS; && (Oru_diff5 > -App.ADTrack.Stop_VPT)
App.SlopeCollDelay1mSCnt = SLOPE_START_MS; && (Orl_diff5 < App.ADTrack.Stop_VPT)
App.EmergencyDelay1mSCnt = EME_START_MS; && (Oru_diff6 > -App.ADTrack.Stop_VPT)
} && (Orl_diff6 < App.ADTrack.Stop_VPT))
else if(((App.CentreRunFalg == false ) && (Oru_diff1 > -App.ADTrack.Stop_VPT) && (Oru_diff2 > -App.ADTrack.Stop_VPT) && (Oru_diff3 > -App.ADTrack.Stop_VPT))
|| ((App.CentreRunFalg == true) || ((App.CentreRunFalg == true)
&& ((Olu_diff1 + Oru_diff1) > -(App.ADTrack.Stop_VPT*2/3)) && ((Olu_diff1 + Oru_diff1) > -(App.ADTrack.Stop_VPT*2/3))
&& ((Olu_diff2 + Oru_diff2) > -(App.ADTrack.Stop_VPT*2/3)) && ((Olu_diff2 + Oru_diff2) > -(App.ADTrack.Stop_VPT*2/3))
&& ((Olu_diff3 + Oru_diff3) > -(App.ADTrack.Stop_VPT*2/3))) && ((Olu_diff3 + Oru_diff3) > -(App.ADTrack.Stop_VPT*2/3)))
|| (App.MotorPara.Current_height >= 1195)) )
{ {
BDC_STOP(); BDC_STOP();
App.StopFlag = true; 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.peaks_positive
// && lu_K.valleys_negative // && lu_K.valleys_negative
App.LeftUpCS1237_SlopeArray[0] < FOLLOW_VPT 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 if(App.StopFlag == true
&& App.StopCollDelay1mSCnt <= 0 && App.StopCollDelay1mSCnt <= 0
&& App.MotorPara.Current_height <= 1995 && App.MotorPara.Current_height <= 1200
&& App.MotorPara.Current_height >= 725){ && App.MotorPara.Current_height >= 722){
App.ADTrack.LeftUp_Org_AdZero = lu_ad[0]; App.ADTrack.LeftUp_Org_AdZero = lu_ad[0];
App.ADTrack.LeftLow_Org_AdZero = ll_ad[0]; App.ADTrack.LeftLow_Org_AdZero = ll_ad[0];
App.ADTrack.RightUp_Org_AdZero = ru_ad[0]; App.ADTrack.RightUp_Org_AdZero = ru_ad[0];
@@ -1043,7 +1022,7 @@ static void AppLoopHandler(void)
App.UpFlag = false; App.UpFlag = false;
App.Status = APP_STATUS_IDLE; 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(); BDC_STOP();
App.StopFlag = true; App.StopFlag = true;
@@ -1075,13 +1054,15 @@ static void AppLoopHandler(void)
App.StopFlag = false; App.StopFlag = false;
App.UpFlag = true; App.UpFlag = true;
BDC_UP(); BDC_UP();
//App.Status = APP_STATUS_WAIT_STEADY; App.CailUp1mSCnt = 0;
App.MotorPara.LoopOne = false;
} }
break;} break;}
case APP_STATUS_WAIT_MOTOR_CALI_UP:{ 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(); BDC_STOP();
App.StopFlag = true; App.StopFlag = true;
App.DownFlag = false; App.DownFlag = false;
@@ -1092,22 +1073,23 @@ static void AppLoopHandler(void)
App.Status = APP_STATUS_WAIT_MOTOR_CALI_DOWN; App.Status = APP_STATUS_WAIT_MOTOR_CALI_DOWN;
BDC_DOWN(); BDC_DOWN();
} }
else if(App.MotorPara.CaliNum >= 488) else if(App.MotorPara.CaliNum >= 480 && App.MotorPara.LoopOne == true)
{ {
BDC_STOP(); BDC_STOP();
App.StopFlag = true; App.StopFlag = true;
App.DownFlag = false; App.DownFlag = false;
App.UpFlag = false; App.UpFlag = false;
App.MotorPara.LoopOne = false;
App.MotorPara.InitialFlag = false; App.MotorPara.InitialFlag = false;
App.MotorPara.CaliFlag = true; App.MotorPara.CaliFlag = true;
DBG_LOG("===================LU Data monitoring===================="); 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===================="); 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===================="); 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===================="); 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"); DBG_LOG("Motor Cali Ok\r\n");
App.TD1mSDelayCnt = 1000; App.TD1mSDelayCnt = 1000;
App.Status = APP_STATUS_WAIT_STEADY; App.Status = APP_STATUS_WAIT_STEADY;
@@ -1115,34 +1097,37 @@ static void AppLoopHandler(void)
break;} break;}
case APP_STATUS_WAIT_MOTOR_CALI_DOWN:{ 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(); BDC_STOP();
App.StopFlag = true; App.StopFlag = true;
App.DownFlag = false; App.DownFlag = false;
App.UpFlag = false; App.UpFlag = false;
App.MotorPara.LoopOne = true;
Ddl_Delay1ms(1000); Ddl_Delay1ms(1000);
App.UpFlag = true; App.UpFlag = true;
App.StopFlag = false; App.StopFlag = false;
App.Status = APP_STATUS_WAIT_MOTOR_CALI_UP; App.Status = APP_STATUS_WAIT_MOTOR_CALI_UP;
BDC_UP(); BDC_UP();
App.CailUp1mSCnt = 0;
} }
else if(App.MotorPara.CaliNum >= 488) else if(App.MotorPara.CaliNum >= 480 && App.MotorPara.LoopOne == true)
{ {
BDC_STOP(); BDC_STOP();
App.StopFlag = true; App.StopFlag = true;
App.DownFlag = false; App.DownFlag = false;
App.UpFlag = false; App.UpFlag = false;
App.MotorPara.LoopOne = false;
App.MotorPara.InitialFlag = false; App.MotorPara.InitialFlag = false;
App.MotorPara.CaliFlag = true; App.MotorPara.CaliFlag = true;
DBG_LOG("===================LU Data monitoring====================\r\n"); 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"); 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"); 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"); 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"); DBG_LOG("Motor Cali Ok\r\n");
App.TD1mSDelayCnt = 1000; App.TD1mSDelayCnt = 1000;
App.Status = APP_STATUS_WAIT_STEADY; App.Status = APP_STATUS_WAIT_STEADY;
@@ -1159,7 +1144,28 @@ static void AppLoopHandler(void)
App.EmeFlag = false; App.EmeFlag = false;
App.StopFlag = true; App.StopFlag = true;
App.WaitFlag = 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;} break;}
} }
@@ -1182,6 +1188,16 @@ static void App1mSRoutine(void)
App.EmergencyDelay1mSCnt--; App.EmergencyDelay1mSCnt--;
if(App.SlopeCollDelay1mSCnt > 0) if(App.SlopeCollDelay1mSCnt > 0)
App.SlopeCollDelay1mSCnt--; 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++; App.FeedDogDlyCnt++;
} }
void FeedDogHandler(void) void FeedDogHandler(void)
@@ -1271,40 +1287,51 @@ void Usart3RxIrqCallback(void)
App.RxUart3Data.RxLen = 0; App.RxUart3Data.RxLen = 0;
App.RxUart3Data.RxBuff[App.RxUart3Data.RxLen++] = Data; 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 else
{ {
if(App.RxUart3Data.RxLen == 1 && Data != 0x01) // if(App.RxUart3Data.RxLen == 1 && Data != 0x01)
{ // {
App.RxUart3Data.HeaderFrame = false; // App.RxUart3Data.HeaderFrame = false;
return; // return;
} // }
// 如果RxLen小于128，则将数据添加到RxBuff中 // 如果RxLen小于128，则将数据添加到RxBuff中
if(App.RxUart3Data.RxLen < 128) { if(App.RxUart3Data.RxLen < 128) {
App.RxUart3Data.RxBuff[App.RxUart3Data.RxLen++] = Data; App.RxUart3Data.RxBuff[App.RxUart3Data.RxLen++] = Data;
} }
if(App.RxUart3Data.RxLen >= 4) 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); int16_t h = ((App.RxUart3Data.RxBuff[2]<<8) & 0xff00) + (App.RxUart3Data.RxBuff[3] & 0x00ff);
if(h >=720 && h <= 1208) if(h >=720 && h <= 1208)
{ {
App.MotorPara.Current_height = h; 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 if(App.MotorPara.LU_HeightStress[(App.MotorPara.Current_height)-720] == 0
|| App.MotorPara.LL_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.RU_HeightStress[(App.MotorPara.Current_height)-720] == 0
|| App.MotorPara.RL_HeightStress[(App.MotorPara.Current_height)-720] == 0) || App.MotorPara.RL_HeightStress[(App.MotorPara.Current_height)-720] == 0)
{ {
App.MotorPara.CaliNum++;
}
App.MotorPara.LU_HeightStress[(App.MotorPara.Current_height)-720] = App.ADTrack.LeftUp_Org_AdZero - App.LeftUpCS1237_TrendArray[0]; App.MotorPara.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.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.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.RL_HeightStress[(App.MotorPara.Current_height)-720] = App.ADTrack.RightLow_Org_AdZero - App.RightLowCS1237_TrendArray[0];
App.MotorPara.CaliNum++;
}
} }
} }
memset(App.RxUart3Data.RxBuff, 0, sizeof(App.RxUart3Data.RxBuff)); memset(App.RxUart3Data.RxBuff, 0, sizeof(App.RxUart3Data.RxBuff));