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>
<bSchkAxf>0</bSchkAxf>
<bTchkAxf>0</bTchkAxf>
<nTsel>3</nTsel>
<nTsel>4</nTsel>
<sDll></sDll>
<sDllPa></sDllPa>
<sDlgDll></sDlgDll>
@@ -114,7 +114,7 @@
<tDlgDll></tDlgDll>
<tDlgPa></tDlgPa>
<tIfile></tIfile>
<pMon>BIN\CMSIS_AGDI.dll</pMon>
<pMon>Segger\JL2CM3.dll</pMon>
</DebugOpt>
<TargetDriverDllRegistry>
<SetRegEntry>
@@ -145,7 +145,7 @@
<SetRegEntry>
<Number>0</Number>
<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>
<Number>0</Number>
@@ -243,7 +243,7 @@
<Ww>
<count>17</count>
<WinNumber>1</WinNumber>
<ItemText>App.MotorPara.RL_HeightStress</ItemText>
<ItemText>App.MotorPara.RL_HeightStress,0x0A</ItemText>
</Ww>
<Ww>
<count>18</count>
@@ -263,17 +263,52 @@
<Ww>
<count>21</count>
<WinNumber>1</WinNumber>
<ItemText>App.MotorPara.Current_height,0x0A</ItemText>
<ItemText>App.Status</ItemText>
</Ww>
<Ww>
<count>22</count>
<WinNumber>1</WinNumber>
<ItemText>App.Status</ItemText>
<ItemText>App.MotorPara.CaliNum,0x0A</ItemText>
</Ww>
<Ww>
<count>23</count>
<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>
</WatchWindow1>
<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);
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
Project/HC32F460/source/User/Inc/bsp.h
+2
View File
@@ -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;//控制等级
Project/HC32F460/source/User/Inc/main.h
+6 -1
View File
@@ -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;//校零标志位
Project/HC32F460/source/User/Src/Algorithm.c
+235 -69
View File
@@ -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
Project/HC32F460/source/User/Src/main.c
+234 -207
View File
@@ -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));