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/source/User/Inc/main.h
+10 -7
View File
@@ -30,8 +30,11 @@
#define TRAVEL_TIME_MS 50000
#define PEAK 1200 //1200mm
#define VALLEY 720 //720mm
#define PEAK 1430 //最高高度mm
#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_DOWN() (SET_TWIN_BIT0(),CLR_TWIN_BIT1())
@@ -75,12 +78,12 @@ typedef struct{//电机参数
bool RestFlag;
bool CaliFlag;
bool LoopOne;//循环一次标志位
int16_t Current_height;//当前高度mm
int16_t Current_height;//当前高度mm(控制盒上报1000~1430)
int16_t CaliNum;//标校数量
int32_t LU_HeightStress[488];//左上传感器对应高度应力
int32_t LL_HeightStress[488];//左下传感器对应高度应力
int32_t RU_HeightStress[488];//右上传感器对应高度应力
int32_t RL_HeightStress[488];//右上传感器对应高度应力
int32_t LU_HeightStress[431];//左上传感器对应高度应力
int32_t LL_HeightStress[431];//左下传感器对应高度应力
int32_t RU_HeightStress[431];//右上传感器对应高度应力
int32_t RL_HeightStress[431];//右下传感器对应高度应力
}MotorPara_t;
/*数据读取状态机*/
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)
{
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--)
{
App.LeftUpCS1237_TrendArray[i] = App.LeftUpCS1237_TrendArray[i - 1];
@@ -68,7 +68,7 @@ void LEFTLOW_CS1237AinADataProcessCallBack(int32_t AD)
// }
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--)
{
App.LeftLowCS1237_TrendArray[i] = App.LeftLowCS1237_TrendArray[i - 1];
@@ -94,7 +94,7 @@ void RIGHTUP_CS1237AinADataProcessCallBack(int32_t AD)
// }
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--)
{
App.RightUpCS1237_TrendArray[i] = App.RightUpCS1237_TrendArray[i - 1];
@@ -120,7 +120,7 @@ void RIGHTLOW_CS1237AinADataProcessCallBack(int32_t AD)
// }
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--)
{
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.MotorPara.Current_height < 1195)
if(App.MotorPara.Current_height < 1425)
{
BDC_UP();
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.MotorPara.Current_height > 725)
if(App.MotorPara.Current_height > 1005)
{
BDC_DOWN();
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.MotorPara.Current_height < 1195)
if(App.MotorPara.Current_height < 1425)
{
BDC_UP();
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.MotorPara.Current_height > 725)
if(App.MotorPara.Current_height > 1005)
{
BDC_DOWN();
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)
)
{
if(App.MotorPara.Current_height < 1195)
if(App.MotorPara.Current_height < 1425)
{
BDC_UP();
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)
)
{
if(App.MotorPara.Current_height > 725)
if(App.MotorPara.Current_height > 1005)
{
BDC_DOWN();
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)
)
{
if(App.MotorPara.Current_height < 1195)
if(App.MotorPara.Current_height < 1425)
{
BDC_UP();
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)
)
{
if(App.MotorPara.Current_height > 725)
if(App.MotorPara.Current_height > 1005)
{
BDC_DOWN();
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_diff2 + Oru_diff2) > -(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 <= 725
|| App.MotorPara.Current_height >= 1425
|| App.MotorPara.Current_height <= 1005
)
{
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_diff2 + Oru_diff2) > -(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 <= 725
|| App.MotorPara.Current_height >= 1425
|| App.MotorPara.Current_height <= 1005
)
{
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_diff2 + Oru_diff2) < (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 <= 725
|| App.MotorPara.Current_height >= 1425
|| App.MotorPara.Current_height <= 1005
)
{
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_diff2 + Oru_diff2) < (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 <= 725
|| App.MotorPara.Current_height >= 1425
|| App.MotorPara.Current_height <= 1005
)
{
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
&& App.StopCollDelay1mSCnt <= 0
&& App.MotorPara.Current_height <= 1195
&& App.MotorPara.Current_height >= 725){
&& App.MotorPara.Current_height <= 1425
&& App.MotorPara.Current_height >= 1005){
App.ADTrack.LeftUp_Org_AdZero = lu_ad[0];
App.ADTrack.LeftLow_Org_AdZero = ll_ad[0];
App.ADTrack.RightUp_Org_AdZero = ru_ad[0];
@@ -999,7 +999,7 @@ static void AppLoopHandler(void)
App.UpFlag = false;
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();
App.StopFlag = true;
@@ -1036,7 +1036,7 @@ static void AppLoopHandler(void)
break;}
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;
BDC_STOP();
@@ -1049,7 +1049,7 @@ static void AppLoopHandler(void)
App.Status = APP_STATUS_WAIT_MOTOR_CALI_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();
App.StopFlag = true;
@@ -1059,13 +1059,13 @@ static void AppLoopHandler(void)
App.MotorPara.InitialFlag = false;
App.MotorPara.CaliFlag = true;
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");
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");
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");
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");
App.TD1mSDelayCnt = 1000;
App.Status = APP_STATUS_WAIT_STEADY;
@@ -1073,7 +1073,7 @@ static void AppLoopHandler(void)
break;}
case APP_STATUS_WAIT_MOTOR_CALI_DOWN:{
if(App.MotorPara.Current_height <= 725)
if(App.MotorPara.Current_height <= 1005)
{
BDC_STOP();
App.StopFlag = true;
@@ -1087,7 +1087,7 @@ static void AppLoopHandler(void)
BDC_UP();
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();
App.StopFlag = true;
@@ -1097,13 +1097,13 @@ static void AppLoopHandler(void)
App.MotorPara.InitialFlag = false;
App.MotorPara.CaliFlag = true;
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");
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");
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");
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");
App.TD1mSDelayCnt = 1000;
App.Status = APP_STATUS_WAIT_STEADY;
@@ -1132,7 +1132,7 @@ static void AppLoopHandler(void)
break;}
case APP_STATUS_HEIGHT_CONTROL:{
if(App.MotorPara.Current_height <= 725
if(App.MotorPara.Current_height <= 1005
&& App.MotorPara.RestFlag == false
&& App.HeightContRolCnt >= 500
&& GET_BIT0_DOUT() == 1
@@ -1143,7 +1143,7 @@ static void AppLoopHandler(void)
Ddl_Delay1ms(500);
BDC_STOP();
}
else if(App.MotorPara.Current_height >= 1195
else if(App.MotorPara.Current_height >= 1425
&& App.MotorPara.RestFlag == false
&& App.HeightContRolCnt >= 500
&& GET_BIT0_DOUT() == 1
@@ -1179,7 +1179,7 @@ static void App1mSRoutine(void)
App.CentCollDelay1mSCnt--;
if(App.MotorPara.InitialFlag == true)
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++;
}
@@ -1307,23 +1307,23 @@ void Usart3RxIrqCallback(void)
{
App.MotorPara.RestFlag = false;
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;
}
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
|| 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)
if(App.MotorPara.LU_HeightStress[HEIGHT_TO_IDX(App.MotorPara.Current_height)] == 0
|| App.MotorPara.LL_HeightStress[HEIGHT_TO_IDX(App.MotorPara.Current_height)] == 0
|| App.MotorPara.RU_HeightStress[HEIGHT_TO_IDX(App.MotorPara.Current_height)] == 0
|| App.MotorPara.RL_HeightStress[HEIGHT_TO_IDX(App.MotorPara.Current_height)] == 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];
App.MotorPara.LU_HeightStress[HEIGHT_TO_IDX(App.MotorPara.Current_height)] = App.ADTrack.LeftUp_Org_AdZero - App.LeftUpCS1237_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[HEIGHT_TO_IDX(App.MotorPara.Current_height)] = App.ADTrack.RightUp_Org_AdZero - App.RightUpCS1237_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));