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

全功能无误版提交

Browse Source
This commit is contained in:
DESKTOP-KQHDCM4\97555
2026-07-12 12:10:24 +08:00
parent 53897050bc
commit 8200e7b8aa
3 changed files with 107 additions and 258 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Project/HC32F460/source/User/Inc/main.h
+10 -8
View File
@@ -13,13 +13,13 @@
#define SLOPE_VPT 1000
#define PRESS_VPT 3
#define FOLLOW_VPT 10
#define SHIELD_VPT 2
#define SHIELD_VPT 1.5
#define EME_STOP_VPT 4000
#define CENTRE_FRONT_CNT 0
#define CENTRE_FRONT_SUM 0
#define CENTRE_FRONT_VPT 0
#define CENTRE_VPT 0
#define CENTRE_VPT 2000
#define STOP_TIME_MS 500
#define CENT_TIME_MS 200
@@ -60,6 +60,11 @@ typedef struct{
int32_t RightUpVPT;//右上设备阈值
int32_t RightLowVPT;//右下设备阈值
int32_t LeftUpShieldVPT; //左上屏蔽阈值
int32_t LeftLowShieldVPT; //左下屏蔽阈值
int32_t RightUpShieldVPT; //右上屏蔽阈值
int32_t RightLowShieldVPT; //右下屏蔽阈值
int32_t LeftUp_Org_AdZero;//左上设备原始AD零点
int32_t LeftLow_Org_AdZero;//左下设备原始AD零点
int32_t RightUp_Org_AdZero;//右上设备原始AD零点
@@ -97,12 +102,14 @@ typedef enum {
APP_STATUS_IDLE,
APP_STATUS_ON,
APP_STATUS_OFF,
APP_STATUS_START_BEEP,
APP_STATUS_MOTOR_INIT,
APP_STATUS_ZERO_MARKING,
APP_STATUS_WAIT_MOTOR_INIT,
APP_STATUS_WAIT_MOTOR_CALI_UP,
APP_STATUS_WAIT_MOTOR_CALI_DOWN,
APP_STATUS_WAIT_STEADY,
APP_STATUS_ZERO_MARKING,
APP_STATUS_END_BEEP,
APP_STATUS_HEIGHT_CONTROL,
}AppStatus_m;
@@ -167,11 +174,6 @@ typedef struct {
int32_t RightUpCS1237_SlopeArray[SLOPE_CNT];
int32_t RightLowCS1237_SlopeArray[SLOPE_CNT];
int32_t aaa;
int32_t bbb;
int32_t ccc;
int32_t ddd;
bool WaitFlag;
}AppDetect_t;
extern AppDetect_t App;
Project/HC32F460/source/User/Src/main.c
+96 -232
View File
@@ -164,7 +164,7 @@ void GradeControl(ControlLevel_m level)
App.ADTrack.LeftUpVPT = 18000;
App.ADTrack.LeftLowVPT = 18000;
App.ADTrack.RightUpVPT = 18000;
App.ADTrack.RightLowVPT = 18000;
App.ADTrack.RightLowVPT = 18000;
App.ADTrack.Stop_VPT = 15000;
break;}
@@ -173,7 +173,7 @@ void GradeControl(ControlLevel_m level)
App.ADTrack.LeftUpVPT = 25000;
App.ADTrack.LeftLowVPT = 25000;
App.ADTrack.RightUpVPT = 25000;
App.ADTrack.RightLowVPT = 25000;
App.ADTrack.RightLowVPT = 25000;
App.ADTrack.Stop_VPT = 19000;
break;}
@@ -182,7 +182,7 @@ void GradeControl(ControlLevel_m level)
App.ADTrack.LeftUpVPT = 30000;
App.ADTrack.LeftLowVPT = 30000;
App.ADTrack.RightUpVPT = 30000;
App.ADTrack.RightLowVPT = 30000;
App.ADTrack.RightLowVPT = 30000;
App.ADTrack.Stop_VPT = 25000;
break;}
}
@@ -257,19 +257,12 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.RightUpCS1237_SlopeArray[0] = ru_slope;
App.RightLowCS1237_SlopeArray[0] = rl_slope;
// PeakValleyCheck lu_K = check_peaks_valleys(App.LeftUpCS1237_SlopeArray, SLOPE_CNT);
// PeakValleyCheck ll_K = check_peaks_valleys(App.LeftLowCS1237_SlopeArray, SLOPE_CNT);
// PeakValleyCheck ru_K = check_peaks_valleys(App.RightUpCS1237_SlopeArray, SLOPE_CNT);
// PeakValleyCheck rl_K = check_peaks_valleys(App.RightLowCS1237_SlopeArray, SLOPE_CNT);
int32_t LeftSlopeSum = abs(App.LeftUpCS1237_SlopeArray[0]) + abs(App.LeftLowCS1237_SlopeArray[0]) + abs(App.LeftUpCS1237_SlopeArray[1]) + abs(App.LeftLowCS1237_SlopeArray[1]) + abs(App.LeftUpCS1237_SlopeArray[2]) + abs(App.LeftLowCS1237_SlopeArray[2]);
int32_t RightSlopeSum = abs(App.RightUpCS1237_SlopeArray[0]) + abs(App.RightLowCS1237_SlopeArray[0]) + abs(App.RightUpCS1237_SlopeArray[1]) + abs(App.RightLowCS1237_SlopeArray[1]) + abs(App.RightUpCS1237_SlopeArray[2]) + abs(App.RightLowCS1237_SlopeArray[2]);
int32_t LeftDiffSum = abs(lu_diff[0]) + abs(lu_diff[1]) + abs(lu_diff[2]) + abs(ll_diff[0]) + abs(ll_diff[1]) + abs(ll_diff[2]);
int32_t RightDiffSum = abs(ru_diff[0]) + abs(ru_diff[1]) + abs(ru_diff[2]) + abs(rl_diff[0]) + abs(rl_diff[1]) + abs(rl_diff[2]);
int32_t LuSideDiff = lu_ad[0] - lu_ad[5];
int32_t LlSideDiff = ll_ad[0] - ll_ad[5];
int32_t RuSideDiff = ru_ad[0] - ru_ad[5];
int32_t RlSideDiff = rl_ad[0] - rl_ad[5];
int32_t LeftDiff = abs(Oll_diff1) + abs(Olu_diff1) + abs(Oll_diff2) + abs(Olu_diff2) + abs(Oll_diff3) + abs(Olu_diff3);
int32_t RightDiff = abs(Orl_diff1) + abs(Oru_diff1) + abs(Orl_diff2) + abs(Oru_diff2) + abs(Orl_diff3) + abs(Oru_diff3);
if(App.EmeFlag == true)
{
@@ -297,41 +290,35 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.LeftRunFalg = false;
App.RightRunFalg = false;
App.CentreRunFalg = false;
if(((lu_ad[0] < App.ADTrack.LeftUp_Org_AdZero-App.ddd)//判断中间操作
&& (ru_ad[0] < App.ADTrack.RightUp_Org_AdZero-App.ddd)
&& (lu_ad[1] < App.ADTrack.LeftUp_Org_AdZero-App.ddd)
&& (ru_ad[1] < App.ADTrack.RightUp_Org_AdZero-App.ddd)
&& (lu_ad[2] < App.ADTrack.LeftUp_Org_AdZero-App.ddd)
&& (ru_ad[2] < App.ADTrack.RightUp_Org_AdZero-App.ddd)
&& (ll_ad[0] > App.ADTrack.LeftLow_Org_AdZero+App.ddd)//判断中间操作
&& (rl_ad[0] > App.ADTrack.RightLow_Org_AdZero+App.ddd)
&& (ll_ad[1] > App.ADTrack.LeftLow_Org_AdZero+App.ddd)
&& (rl_ad[1] > App.ADTrack.RightLow_Org_AdZero+App.ddd)
&& (ll_ad[2] > App.ADTrack.LeftLow_Org_AdZero+App.ddd)
&& (rl_ad[2] > App.ADTrack.RightLow_Org_AdZero+App.ddd))
|| ((lu_ad[0] > App.ADTrack.LeftUp_Org_AdZero+App.ddd)//判断中间操作
&& (ru_ad[0] > App.ADTrack.RightUp_Org_AdZero+App.ddd)
&& (lu_ad[1] > App.ADTrack.LeftUp_Org_AdZero+App.ddd)
&& (ru_ad[1] > App.ADTrack.RightUp_Org_AdZero+App.ddd)
&& (lu_ad[2] > App.ADTrack.LeftUp_Org_AdZero+App.ddd)
&& (ru_ad[2] > App.ADTrack.RightUp_Org_AdZero+App.ddd)
&& (ll_ad[0] < App.ADTrack.LeftLow_Org_AdZero-App.ddd)//判断中间操作
&& (rl_ad[0] < App.ADTrack.RightLow_Org_AdZero-App.ddd)
&& (ll_ad[1] < App.ADTrack.LeftLow_Org_AdZero-App.ddd)
&& (rl_ad[1] < App.ADTrack.RightLow_Org_AdZero-App.ddd)
&& (ll_ad[2] < App.ADTrack.LeftLow_Org_AdZero-App.ddd)
&& (rl_ad[2] < App.ADTrack.RightLow_Org_AdZero-App.ddd))
if(((lu_ad[0] < App.ADTrack.LeftUp_Org_AdZero-CENTRE_VPT)//判断中间操作
&& (ru_ad[0] < App.ADTrack.RightUp_Org_AdZero-CENTRE_VPT)
&& (lu_ad[1] < App.ADTrack.LeftUp_Org_AdZero-CENTRE_VPT)
&& (ru_ad[1] < App.ADTrack.RightUp_Org_AdZero-CENTRE_VPT)
&& (lu_ad[2] < App.ADTrack.LeftUp_Org_AdZero-CENTRE_VPT)
&& (ru_ad[2] < App.ADTrack.RightUp_Org_AdZero-CENTRE_VPT)
&& (ll_ad[0] > App.ADTrack.LeftLow_Org_AdZero+CENTRE_VPT)//判断中间操作
&& (rl_ad[0] > App.ADTrack.RightLow_Org_AdZero+CENTRE_VPT)
&& (ll_ad[1] > App.ADTrack.LeftLow_Org_AdZero+CENTRE_VPT)
&& (rl_ad[1] > App.ADTrack.RightLow_Org_AdZero+CENTRE_VPT)
&& (ll_ad[2] > App.ADTrack.LeftLow_Org_AdZero+CENTRE_VPT)
&& (rl_ad[2] > App.ADTrack.RightLow_Org_AdZero+CENTRE_VPT))
|| ((lu_ad[0] > App.ADTrack.LeftUp_Org_AdZero+CENTRE_VPT)//判断中间操作
&& (ru_ad[0] > App.ADTrack.RightUp_Org_AdZero+CENTRE_VPT)
&& (lu_ad[1] > App.ADTrack.LeftUp_Org_AdZero+CENTRE_VPT)
&& (ru_ad[1] > App.ADTrack.RightUp_Org_AdZero+CENTRE_VPT)
&& (lu_ad[2] > App.ADTrack.LeftUp_Org_AdZero+CENTRE_VPT)
&& (ru_ad[2] > App.ADTrack.RightUp_Org_AdZero+CENTRE_VPT)
&& (ll_ad[0] < App.ADTrack.LeftLow_Org_AdZero-CENTRE_VPT)//判断中间操作
&& (rl_ad[0] < App.ADTrack.RightLow_Org_AdZero-CENTRE_VPT)
&& (ll_ad[1] < App.ADTrack.LeftLow_Org_AdZero-CENTRE_VPT)
&& (rl_ad[1] < App.ADTrack.RightLow_Org_AdZero-CENTRE_VPT)
&& (ll_ad[2] < App.ADTrack.LeftLow_Org_AdZero-CENTRE_VPT)
&& (rl_ad[2] < App.ADTrack.RightLow_Org_AdZero-CENTRE_VPT))
)
{
App.CentreRunFalg = true;
App.CentCollDelay1mSCnt = CENT_TIME_MS;
if(abs(Oll_diff1) + 100 > abs(Orl_diff1)
&& abs(Oll_diff2) + 100 > abs(Orl_diff2)
&& abs(Oll_diff3) + 100 > abs(Orl_diff3)
&& abs(Olu_diff1) + 100 > abs(Oru_diff1)
&& abs(Olu_diff2) + 100 > abs(Oru_diff2)
&& abs(Olu_diff3) + 100 > abs(Oru_diff3)
)//判断偏左还是偏右
if(LeftDiff > RightDiff)//判断偏左
{
App.LeftRunFalg = true;
if(((Olu_diff1+Oru_diff1) > (App.ADTrack.Stop_VPT))//判断上拉
@@ -342,18 +329,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& ((Oll_diff3+Orl_diff3) < -(App.ADTrack.Stop_VPT))
)
{
if((App.RightUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
&& (App.RightUpCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[0] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
&& (App.LeftUpCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] >= PRESS_VPT))
// if(App.CaliZeroFlag == true)//判断是否校零
{
if(App.MotorPara.Current_height < 1195)
{
@@ -375,18 +351,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& ((Oll_diff3+Orl_diff3) > (App.ADTrack.Stop_VPT))
)
{
if((App.RightUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
&& (App.RightUpCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[0] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
&& (App.LeftUpCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] <= -PRESS_VPT))
// if(App.CaliZeroFlag == true)//判断是否校零
{
if(App.MotorPara.Current_height > 725)
{
@@ -401,12 +366,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
}
}
}
else if(abs(Oll_diff1) + 100 < abs(Orl_diff1)//判断偏左
&& abs(Oll_diff2) < abs(Orl_diff2) + 100
&& abs(Oll_diff3) < abs(Orl_diff3) + 100
&& abs(Olu_diff1) < abs(Oru_diff1) + 100
&& abs(Olu_diff2) < abs(Oru_diff2) + 100
&& abs(Olu_diff3) < abs(Oru_diff3) + 100)
else if(RightDiff > LeftDiff)//判断偏右
{
App.RightRunFalg = true;
if(((Olu_diff1+Oru_diff1) > (App.ADTrack.Stop_VPT))//判断上拉
@@ -417,18 +377,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& ((Oll_diff3+Orl_diff3) < -(App.ADTrack.Stop_VPT))
)
{
if((App.RightUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
&& (App.RightUpCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[0] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
&& (App.LeftUpCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] >= PRESS_VPT))
// if(App.CaliZeroFlag == true)//判断是否校零
{
if(App.MotorPara.Current_height < 1195)
{
@@ -450,18 +399,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& ((Oll_diff3+Orl_diff3) > (App.ADTrack.Stop_VPT))
)
{
if((App.RightUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
&& (App.RightUpCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[0] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
&& (App.LeftUpCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] <= -PRESS_VPT))
// if(App.CaliZeroFlag == true)//判断是否校零
{
if(App.MotorPara.Current_height > 725)
{
@@ -492,36 +430,18 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (App.LeftUpCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
&& (App.RightUpCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[0] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] <= -PRESS_VPT))
|| ((App.LeftUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
&& (App.LeftUpCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
&& (App.RightUpCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[0] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] <= -PRESS_VPT))
&& (App.LeftLowCS1237_SlopeArray[2] >= PRESS_VPT))
)
{
// if(App.CaliZeroFlag == true)//判断是否校零
{
// if((abs(Olu_diff1) > abs(Oru_diff1) * SHIELD_VPT)//判断是否不是侧面推拉
// && (abs(Olu_diff2) > abs(Oru_diff2) * SHIELD_VPT)
// && (abs(Olu_diff3) > abs(Oru_diff3) * SHIELD_VPT)
// && (abs(Oll_diff1) > abs(Orl_diff1) * SHIELD_VPT)
// && (abs(Oll_diff3) > abs(Orl_diff2) * SHIELD_VPT)
// && (abs(Oll_diff3) > abs(Orl_diff3) * SHIELD_VPT)
// )
if((abs(Olu_diff1) > abs(Oru_diff1) * SHIELD_VPT)//判断是否不是侧面推拉
&& (abs(Olu_diff2) > abs(Oru_diff2) * SHIELD_VPT)
&& (abs(Olu_diff3) > abs(Oru_diff3) * SHIELD_VPT)
&& (abs(Oll_diff1) > abs(Orl_diff1) * SHIELD_VPT)
&& (abs(Oll_diff3) > abs(Orl_diff2) * SHIELD_VPT)
&& (abs(Oll_diff3) > abs(Orl_diff3) * SHIELD_VPT)
)
{
if(App.MotorPara.Current_height < 1195)
{
@@ -550,36 +470,18 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (App.LeftUpCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
&& (App.RightUpCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[0] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] >= PRESS_VPT))
|| ((App.LeftUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
&& (App.LeftUpCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
&& (App.RightUpCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[0] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] >= PRESS_VPT))
&& (App.LeftLowCS1237_SlopeArray[2] <= -PRESS_VPT))
)
{
// if(App.CaliZeroFlag == true)//判断是否校零
{
// if((abs(Olu_diff1) > abs(Oru_diff1) * SHIELD_VPT)//判断是否不是侧面推拉
// && (abs(Olu_diff2) > abs(Oru_diff2) * SHIELD_VPT)
// && (abs(Olu_diff3) > abs(Oru_diff3) * SHIELD_VPT)
// && (abs(Oll_diff1) > abs(Orl_diff1) * SHIELD_VPT)
// && (abs(Oll_diff3) > abs(Orl_diff2) * SHIELD_VPT)
// && (abs(Oll_diff3) > abs(Orl_diff3) * SHIELD_VPT)
// )
if((abs(Olu_diff1) > abs(Oru_diff1) * SHIELD_VPT)//判断是否不是侧面推拉
&& (abs(Olu_diff2) > abs(Oru_diff2) * SHIELD_VPT)
&& (abs(Olu_diff3) > abs(Oru_diff3) * SHIELD_VPT)
&& (abs(Oll_diff1) > abs(Orl_diff1) * SHIELD_VPT)
&& (abs(Oll_diff3) > abs(Orl_diff2) * SHIELD_VPT)
&& (abs(Oll_diff3) > abs(Orl_diff3) * SHIELD_VPT)
)
{
if(App.MotorPara.Current_height > 725)
{
@@ -611,36 +513,18 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (App.RightUpCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[0] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
&& (App.LeftUpCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] <= -PRESS_VPT))
|| ((App.RightUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
&& (App.RightUpCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[0] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
&& (App.LeftUpCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] >= PRESS_VPT))
&& (App.RightLowCS1237_SlopeArray[2] >= PRESS_VPT))
)
{
// if(App.CaliZeroFlag == true)//判断是否校零
{
// if((abs(Oru_diff1) > abs(Olu_diff1) * SHIELD_VPT)//判断是否不是侧面推拉
// && (abs(Oru_diff2) > abs(Olu_diff2) * SHIELD_VPT)
// && (abs(Oru_diff3) > abs(Olu_diff3) * SHIELD_VPT)
// && (abs(Orl_diff1) > abs(Oll_diff1) * SHIELD_VPT)
// && (abs(Orl_diff3) > abs(Oll_diff2) * SHIELD_VPT)
// && (abs(Orl_diff3) > abs(Oll_diff3) * SHIELD_VPT)
// )
if((abs(Oru_diff1) > abs(Olu_diff1) * SHIELD_VPT)//判断是否不是侧面推拉
&& (abs(Oru_diff2) > abs(Olu_diff2) * SHIELD_VPT)
&& (abs(Oru_diff3) > abs(Olu_diff3) * SHIELD_VPT)
&& (abs(Orl_diff1) > abs(Oll_diff1) * SHIELD_VPT)
&& (abs(Orl_diff3) > abs(Oll_diff2) * SHIELD_VPT)
&& (abs(Orl_diff3) > abs(Oll_diff3) * SHIELD_VPT)
)
{
if(App.MotorPara.Current_height < 1195)
{
@@ -668,36 +552,18 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
&& (App.RightUpCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[0] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
&& (App.LeftUpCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] >= PRESS_VPT))
|| ((App.RightUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
&& (App.RightUpCS1237_SlopeArray[1] >= PRESS_VPT)
&& (App.RightUpCS1237_SlopeArray[2] >= PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[0] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.RightLowCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
&& (App.LeftUpCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.LeftUpCS1237_SlopeArray[2] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[0] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[1] <= -PRESS_VPT)
&& (App.LeftLowCS1237_SlopeArray[2] <= -PRESS_VPT))
&& (App.RightLowCS1237_SlopeArray[2] <= -PRESS_VPT))
)
{
// if(App.CaliZeroFlag == true)//判断是否校零
{
// if((abs(Oru_diff1) > abs(Olu_diff1) * SHIELD_VPT)//判断是否不是侧面推拉
// && (abs(Oru_diff2) > abs(Olu_diff2) * SHIELD_VPT)
// && (abs(Oru_diff3) > abs(Olu_diff3) * SHIELD_VPT)
// && (abs(Orl_diff1) > abs(Oll_diff1) * SHIELD_VPT)
// && (abs(Orl_diff3) > abs(Oll_diff2) * SHIELD_VPT)
// && (abs(Orl_diff3) > abs(Oll_diff3) * SHIELD_VPT)
// )
if((abs(Oru_diff1) > abs(Olu_diff1) * SHIELD_VPT)//判断是否不是侧面推拉
&& (abs(Oru_diff2) > abs(Olu_diff2) * SHIELD_VPT)
&& (abs(Oru_diff3) > abs(Olu_diff3) * SHIELD_VPT)
&& (abs(Orl_diff1) > abs(Oll_diff1) * SHIELD_VPT)
&& (abs(Orl_diff3) > abs(Oll_diff2) * SHIELD_VPT)
&& (abs(Orl_diff3) > abs(Oll_diff3) * SHIELD_VPT)
)
{
if(App.MotorPara.Current_height > 725)
{
@@ -983,7 +849,7 @@ static void AppInit(void)
BDC_STOP();
LED_RED_OFF();
LED_GREEN_ON();
App.Status = APP_STATUS_MOTOR_INIT;
App.Status = APP_STATUS_START_BEEP;
App.MotorPara.InitialFlag = false;
App.RStatus = READ_STATUS_START;
GradeControl(App.Para.Layout.ControlLevel);//等级控制
@@ -1103,25 +969,31 @@ static void AppLoopHandler(void)
App.Status = APP_STATUS_IDLE;
break;}
case APP_STATUS_MOTOR_INIT:{
Beeper_Start(200,200,2);
case APP_STATUS_START_BEEP:{
LED_RED_ON();
LED_GREEN_OFF();
BDC_UP();
Ddl_Delay1ms(1000);
BDC_DOWN();
App.DownFlag = true;
App.StopFlag = false;
App.Status = APP_STATUS_WAIT_MOTOR_INIT;
App.TD1mSDelayCnt = TRAVEL_TIME_MS;
Beeper_Start(200,200,2);
App.TD1mSDelayCnt = 1000;
App.Status = APP_STATUS_MOTOR_INIT;
break;}
case APP_STATUS_MOTOR_INIT:{
if(App.TD1mSDelayCnt <= 0)
{
BDC_UP();
Ddl_Delay1ms(1000);
BDC_DOWN();
App.DownFlag = true;
App.StopFlag = false;
App.Status = APP_STATUS_WAIT_MOTOR_INIT;
App.TD1mSDelayCnt = TRAVEL_TIME_MS;
}
break;}
case APP_STATUS_WAIT_MOTOR_INIT:{
if(App.TD1mSDelayCnt <= 0)
{
BDC_STOP();
LED_RED_OFF();
LED_GREEN_ON();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
@@ -1130,13 +1002,11 @@ static void AppLoopHandler(void)
if(App.MotorPara.Current_height <= 722 && App.MotorPara.Current_height >= 720)
{
BDC_STOP();
LED_RED_OFF();
LED_GREEN_ON();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
App.MotorPara.CaliFlag = false;
App.TD1mSDelayCnt = 1000;
App.TD1mSDelayCnt = 1500;
App.Status = APP_STATUS_ZERO_MARKING;
}
break;}
@@ -1153,7 +1023,6 @@ static void AppLoopHandler(void)
App.ADTrack.LeftLow_Org_AdZero,
App.ADTrack.RightUp_Org_AdZero,
App.ADTrack.RightLow_Org_AdZero);
App.Status = APP_STATUS_WAIT_MOTOR_CALI_UP;
App.MotorPara.InitialFlag = true;
App.MotorPara.CaliNum = 0;
@@ -1161,8 +1030,6 @@ static void AppLoopHandler(void)
App.StopFlag = false;
App.UpFlag = true;
BDC_UP();
LED_RED_ON();
LED_GREEN_OFF();
App.CailUp1mSCnt = 0;
App.MotorPara.LoopOne = false;
}
@@ -1176,7 +1043,7 @@ static void AppLoopHandler(void)
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
Ddl_Delay1ms(1000);
Ddl_Delay1ms(1500);
App.DownFlag = true;
App.StopFlag = false;
App.Status = APP_STATUS_WAIT_MOTOR_CALI_DOWN;
@@ -1185,8 +1052,6 @@ static void AppLoopHandler(void)
else if(App.MotorPara.CaliNum >= 480 && App.MotorPara.LoopOne == true)
{
BDC_STOP();
LED_RED_OFF();
LED_GREEN_ON();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
@@ -1215,7 +1080,7 @@ static void AppLoopHandler(void)
App.DownFlag = false;
App.UpFlag = false;
App.MotorPara.LoopOne = true;
Ddl_Delay1ms(1000);
Ddl_Delay1ms(1500);
App.UpFlag = true;
App.StopFlag = false;
App.Status = APP_STATUS_WAIT_MOTOR_CALI_UP;
@@ -1225,8 +1090,6 @@ static void AppLoopHandler(void)
else if(App.MotorPara.CaliNum >= 480 && App.MotorPara.LoopOne == true)
{
BDC_STOP();
LED_RED_OFF();
LED_GREEN_ON();
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
@@ -1250,7 +1113,6 @@ static void AppLoopHandler(void)
case APP_STATUS_WAIT_STEADY:{
if(App.TD1mSDelayCnt <= 0)
{
Beeper_Start(300,0,1);
App.SideFlag = false;
App.CaliZeroFlag = false;
App.UpFlag = false;
@@ -1258,10 +1120,17 @@ static void AppLoopHandler(void)
App.EmeFlag = false;
App.StopFlag = true;
App.WaitFlag = true;
App.Status = APP_STATUS_HEIGHT_CONTROL;
App.Status = APP_STATUS_END_BEEP;
}
break;}
case APP_STATUS_END_BEEP:{
Beeper_Start(300,0,1);
LED_RED_OFF();
LED_GREEN_ON();
App.Status = APP_STATUS_HEIGHT_CONTROL;
break;}
case APP_STATUS_HEIGHT_CONTROL:{
if(App.MotorPara.Current_height <= 725
&& App.MotorPara.RestFlag == false
@@ -1332,11 +1201,6 @@ int main(void)
BSP_Init();
AppInit();
App.aaa = CENTRE_FRONT_CNT;
App.bbb = CENTRE_FRONT_SUM;
App.ccc = CENTRE_FRONT_VPT;
App.ddd = CENTRE_VPT;
while(1) {
#if(USE_WDT == 1)
FeedDogHandler();