YuanHongbin/DoubleColumn_Trolley
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

适配110V美标控制盒：高度量程改为1000~1430mm，431点补偿表采样410点完成校准

Browse Source
This commit is contained in:
DESKTOP-UB3FFU5\袁洪彬
2026-09-09 13:58:03 +08:00
parent 41021cf95c
commit 130904e0a2
3 changed files with 60 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Project/HC32F460/MDK/HC32F460.uvoptx
+2 -2
View File
@@ -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("") -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> <Name>-X"Any" -UAny -O206 -S9 -C0 -P00000000 -N00("") -D00(00000000) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO31 -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>
@@ -430,7 +430,7 @@
<EnableFlashSeq>1</EnableFlashSeq> <EnableFlashSeq>1</EnableFlashSeq>
<EnableLog>0</EnableLog> <EnableLog>0</EnableLog>
<Protocol>2</Protocol> <Protocol>2</Protocol>
<DbgClock>1000000</DbgClock> <DbgClock>5000000</DbgClock>
</DebugDescription> </DebugDescription>
</TargetOption> </TargetOption>
</Target> </Target>
Project/HC32F460/source/User/Inc/main.h
+10 -7
View File
@@ -30,8 +30,11 @@
#define TRAVEL_TIME_MS 50000 #define TRAVEL_TIME_MS 50000
#define PEAK 1200 //1200mm #define PEAK 1430 //最高高度mm
#define VALLEY 720 //720mm #define VALLEY 1000 //最低高度mm
/*高度转应力表索引: 补偿表431点覆盖1000~1430mm(430个步进), 接收范围已保证不越界*/
#define HEIGHT_TO_IDX(h) ((h) - 1000)
#define BDC_STOP() (SET_TWIN_BIT0(),SET_TWIN_BIT1()) #define BDC_STOP() (SET_TWIN_BIT0(),SET_TWIN_BIT1())
#define BDC_DOWN() (SET_TWIN_BIT0(),CLR_TWIN_BIT1()) #define BDC_DOWN() (SET_TWIN_BIT0(),CLR_TWIN_BIT1())
@@ -75,12 +78,12 @@ typedef struct{//电机参数
bool RestFlag; bool RestFlag;
bool CaliFlag; bool CaliFlag;
bool LoopOne;//循环一次标志位 bool LoopOne;//循环一次标志位
int16_t Current_height;//当前高度mm int16_t Current_height;//当前高度mm(控制盒上报1000~1430)
int16_t CaliNum;//标校数量 int16_t CaliNum;//标校数量
int32_t LU_HeightStress[488];//左上传感器对应高度应力 int32_t LU_HeightStress[431];//左上传感器对应高度应力
int32_t LL_HeightStress[488];//左下传感器对应高度应力 int32_t LL_HeightStress[431];//左下传感器对应高度应力
int32_t RU_HeightStress[488];//右上传感器对应高度应力 int32_t RU_HeightStress[431];//右上传感器对应高度应力
int32_t RL_HeightStress[488];//右上传感器对应高度应力 int32_t RL_HeightStress[431];//右下传感器对应高度应力
}MotorPara_t; }MotorPara_t;
/*数据读取状态机*/ /*数据读取状态机*/
typedef enum { typedef enum {
Project/HC32F460/source/User/Src/main.c
+48 -48
View File
@@ -43,7 +43,7 @@ void LEFTUP_CS1237AinADataProcessCallBack(int32_t AD)
// } // }
if(App.MotorPara.CaliFlag == true) if(App.MotorPara.CaliFlag == true)
{ {
AD = AD + App.MotorPara.LU_HeightStress[(App.MotorPara.Current_height)-725]; AD = AD + App.MotorPara.LU_HeightStress[HEIGHT_TO_IDX(App.MotorPara.Current_height)];
for(int i = OSC_CNT-1; i > 0; i--) for(int i = OSC_CNT-1; i > 0; i--)
{ {
App.LeftUpCS1237_TrendArray[i] = App.LeftUpCS1237_TrendArray[i - 1]; App.LeftUpCS1237_TrendArray[i] = App.LeftUpCS1237_TrendArray[i - 1];
@@ -68,7 +68,7 @@ void LEFTLOW_CS1237AinADataProcessCallBack(int32_t AD)
// } // }
if(App.MotorPara.CaliFlag == true) if(App.MotorPara.CaliFlag == true)
{ {
AD = AD + App.MotorPara.LL_HeightStress[(App.MotorPara.Current_height)-725]; AD = AD + App.MotorPara.LL_HeightStress[HEIGHT_TO_IDX(App.MotorPara.Current_height)];
for(int i = OSC_CNT-1; i > 0; i--) for(int i = OSC_CNT-1; i > 0; i--)
{ {
App.LeftLowCS1237_TrendArray[i] = App.LeftLowCS1237_TrendArray[i - 1]; App.LeftLowCS1237_TrendArray[i] = App.LeftLowCS1237_TrendArray[i - 1];
@@ -94,7 +94,7 @@ void RIGHTUP_CS1237AinADataProcessCallBack(int32_t AD)
// } // }
if(App.MotorPara.CaliFlag == true) if(App.MotorPara.CaliFlag == true)
{ {
AD = AD + App.MotorPara.RU_HeightStress[(App.MotorPara.Current_height)-725]; AD = AD + App.MotorPara.RU_HeightStress[HEIGHT_TO_IDX(App.MotorPara.Current_height)];
for(int i = OSC_CNT-1; i > 0; i--) for(int i = OSC_CNT-1; i > 0; i--)
{ {
App.RightUpCS1237_TrendArray[i] = App.RightUpCS1237_TrendArray[i - 1]; App.RightUpCS1237_TrendArray[i] = App.RightUpCS1237_TrendArray[i - 1];
@@ -120,7 +120,7 @@ void RIGHTLOW_CS1237AinADataProcessCallBack(int32_t AD)
// } // }
if(App.MotorPara.CaliFlag == true) if(App.MotorPara.CaliFlag == true)
{ {
AD = AD + App.MotorPara.RL_HeightStress[(App.MotorPara.Current_height)-725]; AD = AD + App.MotorPara.RL_HeightStress[HEIGHT_TO_IDX(App.MotorPara.Current_height)];
for(int i = OSC_CNT-1; i > 0; i--) for(int i = OSC_CNT-1; i > 0; i--)
{ {
App.RightLowCS1237_TrendArray[i] = App.RightLowCS1237_TrendArray[i - 1]; App.RightLowCS1237_TrendArray[i] = App.RightLowCS1237_TrendArray[i - 1];
@@ -331,7 +331,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
{ {
// if(App.CaliZeroFlag == true)//判断是否校零 // if(App.CaliZeroFlag == true)//判断是否校零
{ {
if(App.MotorPara.Current_height < 1195) if(App.MotorPara.Current_height < 1425)
{ {
BDC_UP(); BDC_UP();
App.CaliZeroFlag = false; App.CaliZeroFlag = false;
@@ -353,7 +353,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
{ {
// if(App.CaliZeroFlag == true)//判断是否校零 // if(App.CaliZeroFlag == true)//判断是否校零
{ {
if(App.MotorPara.Current_height > 725) if(App.MotorPara.Current_height > 1005)
{ {
BDC_DOWN(); BDC_DOWN();
App.CaliZeroFlag = false; App.CaliZeroFlag = false;
@@ -379,7 +379,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
{ {
// if(App.CaliZeroFlag == true)//判断是否校零 // if(App.CaliZeroFlag == true)//判断是否校零
{ {
if(App.MotorPara.Current_height < 1195) if(App.MotorPara.Current_height < 1425)
{ {
BDC_UP(); BDC_UP();
App.CaliZeroFlag = false; App.CaliZeroFlag = false;
@@ -401,7 +401,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
{ {
// if(App.CaliZeroFlag == true)//判断是否校零 // if(App.CaliZeroFlag == true)//判断是否校零
{ {
if(App.MotorPara.Current_height > 725) if(App.MotorPara.Current_height > 1005)
{ {
BDC_DOWN(); BDC_DOWN();
App.CaliZeroFlag = false; App.CaliZeroFlag = false;
@@ -443,7 +443,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (abs(Oll_diff3) > abs(Orl_diff3) * SHIELD_VPT) && (abs(Oll_diff3) > abs(Orl_diff3) * SHIELD_VPT)
) )
{ {
if(App.MotorPara.Current_height < 1195) if(App.MotorPara.Current_height < 1425)
{ {
BDC_UP(); BDC_UP();
App.CaliZeroFlag = false; App.CaliZeroFlag = false;
@@ -483,7 +483,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (abs(Oll_diff3) > abs(Orl_diff3) * SHIELD_VPT) && (abs(Oll_diff3) > abs(Orl_diff3) * SHIELD_VPT)
) )
{ {
if(App.MotorPara.Current_height > 725) if(App.MotorPara.Current_height > 1005)
{ {
BDC_DOWN(); BDC_DOWN();
App.CaliZeroFlag = false; App.CaliZeroFlag = false;
@@ -526,7 +526,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (abs(Orl_diff3) > abs(Oll_diff3) * SHIELD_VPT) && (abs(Orl_diff3) > abs(Oll_diff3) * SHIELD_VPT)
) )
{ {
if(App.MotorPara.Current_height < 1195) if(App.MotorPara.Current_height < 1425)
{ {
BDC_UP(); BDC_UP();
App.CaliZeroFlag = false; App.CaliZeroFlag = false;
@@ -565,7 +565,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (abs(Orl_diff3) > abs(Oll_diff3) * SHIELD_VPT) && (abs(Orl_diff3) > abs(Oll_diff3) * SHIELD_VPT)
) )
{ {
if(App.MotorPara.Current_height > 725) if(App.MotorPara.Current_height > 1005)
{ {
BDC_DOWN(); BDC_DOWN();
App.CaliZeroFlag = false; App.CaliZeroFlag = false;
@@ -602,8 +602,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& ((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 || App.MotorPara.Current_height >= 1425
|| App.MotorPara.Current_height <= 725 || App.MotorPara.Current_height <= 1005
) )
{ {
BDC_STOP(); BDC_STOP();
@@ -647,8 +647,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& ((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 || App.MotorPara.Current_height >= 1425
|| App.MotorPara.Current_height <= 725 || App.MotorPara.Current_height <= 1005
) )
{ {
BDC_STOP(); BDC_STOP();
@@ -695,8 +695,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& ((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 || App.MotorPara.Current_height >= 1425
|| App.MotorPara.Current_height <= 725 || App.MotorPara.Current_height <= 1005
) )
{ {
BDC_STOP(); BDC_STOP();
@@ -740,8 +740,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& ((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 || App.MotorPara.Current_height >= 1425
|| App.MotorPara.Current_height <= 725 || App.MotorPara.Current_height <= 1005
) )
{ {
BDC_STOP(); BDC_STOP();
@@ -804,8 +804,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 <= 1195 && App.MotorPara.Current_height <= 1425
&& App.MotorPara.Current_height >= 725){ && App.MotorPara.Current_height >= 1005){
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];
@@ -999,7 +999,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 <= 722 && App.MotorPara.Current_height >= 720) if(App.MotorPara.Current_height <= 1002 && App.MotorPara.Current_height >= 1000)
{ {
BDC_STOP(); BDC_STOP();
App.StopFlag = true; App.StopFlag = true;
@@ -1036,7 +1036,7 @@ static void AppLoopHandler(void)
break;} break;}
case APP_STATUS_WAIT_MOTOR_CALI_UP:{ case APP_STATUS_WAIT_MOTOR_CALI_UP:{
if(App.MotorPara.Current_height >= 1200 && App.CailUp1mSCnt >= 17000) if(App.MotorPara.Current_height >= 1430 && App.CailUp1mSCnt >= 17000)
{ {
App.CailUp1mSCnt = 0; App.CailUp1mSCnt = 0;
BDC_STOP(); BDC_STOP();
@@ -1049,7 +1049,7 @@ 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 >= 480 && App.MotorPara.LoopOne == true) else if(App.MotorPara.CaliNum >= 410 && App.MotorPara.LoopOne == true)
{ {
BDC_STOP(); BDC_STOP();
App.StopFlag = true; App.StopFlag = true;
@@ -1059,13 +1059,13 @@ static void AppLoopHandler(void)
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");
weightedMovingAverageWithEnhance(App.MotorPara.LU_HeightStress,480,7,380,480,4,0); weightedMovingAverageWithEnhance(App.MotorPara.LU_HeightStress,431,7,341,430,4,0);
DBG_LOG("===================LL Data monitoring====================\r\n"); DBG_LOG("===================LL Data monitoring====================\r\n");
weightedMovingAverageWithEnhance(App.MotorPara.LL_HeightStress,480,7,380,480,4,0); weightedMovingAverageWithEnhance(App.MotorPara.LL_HeightStress,431,7,341,430,4,0);
DBG_LOG("===================RU Data monitoring====================\r\n"); DBG_LOG("===================RU Data monitoring====================\r\n");
weightedMovingAverageWithEnhance(App.MotorPara.RU_HeightStress,480,7,380,480,4,0); weightedMovingAverageWithEnhance(App.MotorPara.RU_HeightStress,431,7,341,430,4,0);
DBG_LOG("===================RL Data monitoring====================\r\n"); DBG_LOG("===================RL Data monitoring====================\r\n");
weightedMovingAverageWithEnhance(App.MotorPara.RL_HeightStress,480,7,380,480,4,0); weightedMovingAverageWithEnhance(App.MotorPara.RL_HeightStress,431,7,341,430,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;
@@ -1073,7 +1073,7 @@ 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 <= 725) if(App.MotorPara.Current_height <= 1005)
{ {
BDC_STOP(); BDC_STOP();
App.StopFlag = true; App.StopFlag = true;
@@ -1087,7 +1087,7 @@ static void AppLoopHandler(void)
BDC_UP(); BDC_UP();
App.CailUp1mSCnt = 0; App.CailUp1mSCnt = 0;
} }
else if(App.MotorPara.CaliNum >= 480 && App.MotorPara.LoopOne == true) else if(App.MotorPara.CaliNum >= 410 && App.MotorPara.LoopOne == true)
{ {
BDC_STOP(); BDC_STOP();
App.StopFlag = true; App.StopFlag = true;
@@ -1097,13 +1097,13 @@ static void AppLoopHandler(void)
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");
weightedMovingAverageWithEnhance(App.MotorPara.LU_HeightStress,480,7,380,480,4,0); weightedMovingAverageWithEnhance(App.MotorPara.LU_HeightStress,431,7,341,430,4,0);
DBG_LOG("===================LL Data monitoring====================\r\n"); DBG_LOG("===================LL Data monitoring====================\r\n");
weightedMovingAverageWithEnhance(App.MotorPara.LL_HeightStress,480,7,380,480,4,0); weightedMovingAverageWithEnhance(App.MotorPara.LL_HeightStress,431,7,341,430,4,0);
DBG_LOG("===================RU Data monitoring====================\r\n"); DBG_LOG("===================RU Data monitoring====================\r\n");
weightedMovingAverageWithEnhance(App.MotorPara.RU_HeightStress,480,7,380,480,4,0); weightedMovingAverageWithEnhance(App.MotorPara.RU_HeightStress,431,7,341,430,4,0);
DBG_LOG("===================RL Data monitoring====================\r\n"); DBG_LOG("===================RL Data monitoring====================\r\n");
weightedMovingAverageWithEnhance(App.MotorPara.RL_HeightStress,480,7,380,480,4,0); weightedMovingAverageWithEnhance(App.MotorPara.RL_HeightStress,431,7,341,430,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;
@@ -1132,7 +1132,7 @@ static void AppLoopHandler(void)
break;} break;}
case APP_STATUS_HEIGHT_CONTROL:{ case APP_STATUS_HEIGHT_CONTROL:{
if(App.MotorPara.Current_height <= 725 if(App.MotorPara.Current_height <= 1005
&& App.MotorPara.RestFlag == false && App.MotorPara.RestFlag == false
&& App.HeightContRolCnt >= 500 && App.HeightContRolCnt >= 500
&& GET_BIT0_DOUT() == 1 && GET_BIT0_DOUT() == 1
@@ -1143,7 +1143,7 @@ static void AppLoopHandler(void)
Ddl_Delay1ms(500); Ddl_Delay1ms(500);
BDC_STOP(); BDC_STOP();
} }
else if(App.MotorPara.Current_height >= 1195 else if(App.MotorPara.Current_height >= 1425
&& App.MotorPara.RestFlag == false && App.MotorPara.RestFlag == false
&& App.HeightContRolCnt >= 500 && App.HeightContRolCnt >= 500
&& GET_BIT0_DOUT() == 1 && GET_BIT0_DOUT() == 1
@@ -1179,7 +1179,7 @@ static void App1mSRoutine(void)
App.CentCollDelay1mSCnt--; App.CentCollDelay1mSCnt--;
if(App.MotorPara.InitialFlag == true) if(App.MotorPara.InitialFlag == true)
App.CailUp1mSCnt++; App.CailUp1mSCnt++;
if((App.MotorPara.Current_height < 725 || App.MotorPara.Current_height > 1195)) if((App.MotorPara.Current_height < 1005 || App.MotorPara.Current_height > 1425))
{ {
App.HeightContRolCnt++; App.HeightContRolCnt++;
} }
@@ -1307,23 +1307,23 @@ void Usart3RxIrqCallback(void)
{ {
App.MotorPara.RestFlag = false; 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 >= 1000 && h <= 1430)
{ {
App.MotorPara.Current_height = h; App.MotorPara.Current_height = h;
} }
if(App.MotorPara.InitialFlag == true && h >=720 && h <= 1200) if(App.MotorPara.InitialFlag == true && h >= 1000 && h <= 1430)
{//缓存每个高度点对于零点应力的差值，对不同高度点的应力进行补偿 {//缓存每个高度点对于零点应力的差值，对不同高度点的应力进行补偿
if(App.MotorPara.LU_HeightStress[(App.MotorPara.Current_height)-720] == 0 if(App.MotorPara.LU_HeightStress[HEIGHT_TO_IDX(App.MotorPara.Current_height)] == 0
|| App.MotorPara.LL_HeightStress[(App.MotorPara.Current_height)-720] == 0 || App.MotorPara.LL_HeightStress[HEIGHT_TO_IDX(App.MotorPara.Current_height)] == 0
|| App.MotorPara.RU_HeightStress[(App.MotorPara.Current_height)-720] == 0 || App.MotorPara.RU_HeightStress[HEIGHT_TO_IDX(App.MotorPara.Current_height)] == 0
|| App.MotorPara.RL_HeightStress[(App.MotorPara.Current_height)-720] == 0) || App.MotorPara.RL_HeightStress[HEIGHT_TO_IDX(App.MotorPara.Current_height)] == 0)
{ {
App.MotorPara.CaliNum++; 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[HEIGHT_TO_IDX(App.MotorPara.Current_height)] = 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[HEIGHT_TO_IDX(App.MotorPara.Current_height)] = 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[HEIGHT_TO_IDX(App.MotorPara.Current_height)] = 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[HEIGHT_TO_IDX(App.MotorPara.Current_height)] = App.ADTrack.RightLow_Org_AdZero - App.RightLowCS1237_TrendArray[0];
} }
} }
memset(App.RxUart3Data.RxBuff, 0, sizeof(App.RxUart3Data.RxBuff)); memset(App.RxUart3Data.RxBuff, 0, sizeof(App.RxUart3Data.RxBuff));