diff --git a/Project/HC32F460/MDK/HC32F460.uvoptx b/Project/HC32F460/MDK/HC32F460.uvoptx index 470fd12..5b7904c 100644 --- a/Project/HC32F460/MDK/HC32F460.uvoptx +++ b/Project/HC32F460/MDK/HC32F460.uvoptx @@ -153,24 +153,7 @@ UL2CM3(-S0 -C0 -P0 -FD1FFF8000 -FC1000 -FN2 -FF0HC32F460_512K -FS00 -FL080000 -FF1HC32F460_otp -FS13000C00 -FL13FC -FP0($$Device:HC32F460JETA$FlashARM\HC32F460_512K.FLM) -FP1($$Device:HC32F460JETA$FlashARM\HC32F460_otp.FLM)) - - - 0 - 0 - 504 - 1 -
32426
- 0 - 0 - 0 - 0 - 0 - 1 - ..\source\User\Src\main.c - - \\BombSensor\../source/User/Src/main.c\504 - - + 0 diff --git a/Project/HC32F460/source/User/Inc/main.h b/Project/HC32F460/source/User/Inc/main.h index 7106d67..78b14f1 100644 --- a/Project/HC32F460/source/User/Inc/main.h +++ b/Project/HC32F460/source/User/Inc/main.h @@ -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; diff --git a/Project/HC32F460/source/User/Src/main.c b/Project/HC32F460/source/User/Src/main.c index 68e6320..f017eef 100644 --- a/Project/HC32F460/source/User/Src/main.c +++ b/Project/HC32F460/source/User/Src/main.c @@ -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();