1663 lines
60 KiB
C
1663 lines
60 KiB
C
#include <stdlib.h>
|
|
#include <math.h>
|
|
|
|
#include "main.h"
|
|
#include "Encryption.h"
|
|
#include "Debug.h"
|
|
#include "Algorithm.h"
|
|
#include "dc_motor.h"
|
|
#include "CS1237_LEFTUP.h"
|
|
#include "CS1237_LEFTLOW.h"
|
|
#include "CS1237_RIGHTUP.h"
|
|
#include "CS1237_RIGHTLOW.h"
|
|
#include "BeepAndLed.h"
|
|
|
|
#define MAIN_DBG_LOG(...) DBG_LOG(__VA_ARGS__)
|
|
#define MAIN_DBG_ARRAY(ARRAY, SIZE) DBG_ARRAY(ARRAY,SIZE)
|
|
|
|
#define SOFTWARE_VERSION 11 //软件版本
|
|
#define HARDWARE_VERSION 20 //硬件版本
|
|
|
|
AppDetect_t App;
|
|
|
|
static void show(uint8_t ch,int32_t ad)
|
|
{
|
|
if(App.Para.Layout.DBGWaveOut == true)
|
|
{
|
|
uint8_t bao[7];
|
|
bao[0] = 0x5A;
|
|
bao[1] = ch;
|
|
bao[2] = ad & 0xFF;
|
|
bao[3] = ad >> 8 & 0xFF;
|
|
bao[4] = ad >> 16 & 0xFF;
|
|
bao[5] = ad >> 24 & 0xFF;
|
|
bao[6] = 0xA5;
|
|
DebugUartSend(bao,7);
|
|
}
|
|
}
|
|
void LEFTUP_CS1237AinADataProcessCallBack(int32_t AD)
|
|
{
|
|
// if(abs(AD - App.LeftUpCS1237_TrendArray[0]) > 2000000)
|
|
// {
|
|
// AD = App.LeftUpCS1237_TrendArray[0];
|
|
// }
|
|
if(App.MotorPara.CaliFlag == true)
|
|
{
|
|
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];
|
|
}
|
|
App.LeftUpCS1237_TrendArray[0] = AD;
|
|
}
|
|
else{
|
|
for(int i = OSC_CNT-1; i > 0; i--)
|
|
{
|
|
App.LeftUpCS1237_TrendArray[i] = App.LeftUpCS1237_TrendArray[i - 1];
|
|
}
|
|
App.LeftUpCS1237_TrendArray[0] = AD;
|
|
}
|
|
show(1,AD);
|
|
App.LeftUpCS1237ReadEndFlag = true;
|
|
}
|
|
void LEFTLOW_CS1237AinADataProcessCallBack(int32_t AD)
|
|
{
|
|
// if(abs(AD - App.LeftLowCS1237_TrendArray[0]) > 2000000)
|
|
// {
|
|
// AD = App.LeftLowCS1237_TrendArray[0];
|
|
// }
|
|
if(App.MotorPara.CaliFlag == true)
|
|
{
|
|
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];
|
|
}
|
|
App.LeftLowCS1237_TrendArray[0] = AD;
|
|
}
|
|
else
|
|
{
|
|
// for(int i = OSC_CNT-1; i > 0; i--)
|
|
// {
|
|
// App.LeftLowCS1237_TrendArray[i] = App.LeftLowCS1237_TrendArray[i - 1];
|
|
// }
|
|
App.LeftLowCS1237_TrendArray[0] = AD;
|
|
}
|
|
show(2,AD);
|
|
App.LeftLowCS1237ReadEndFlag = true;
|
|
}
|
|
void RIGHTUP_CS1237AinADataProcessCallBack(int32_t AD)
|
|
{
|
|
// if(abs(AD - App.RightUpCS1237_TrendArray[0]) > 2000000)
|
|
// {
|
|
// AD = App.RightUpCS1237_TrendArray[0];
|
|
// }
|
|
if(App.MotorPara.CaliFlag == true)
|
|
{
|
|
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];
|
|
}
|
|
App.RightUpCS1237_TrendArray[0] = AD;
|
|
}
|
|
else
|
|
{
|
|
// for(int i = OSC_CNT-1; i > 0; i--)
|
|
// {
|
|
// App.RightUpCS1237_TrendArray[i] = App.RightUpCS1237_TrendArray[i - 1];
|
|
// }
|
|
App.RightUpCS1237_TrendArray[0] = AD;
|
|
}
|
|
show(3,AD);
|
|
App.RightUpCS1237ReadEndFlag = true;
|
|
}
|
|
void RIGHTLOW_CS1237AinADataProcessCallBack(int32_t AD)
|
|
{
|
|
// if(abs(AD - App.RightLowCS1237_TrendArray[0]) > 2000000)
|
|
// {
|
|
// AD = App.RightLowCS1237_TrendArray[0];
|
|
// }
|
|
if(App.MotorPara.CaliFlag == true)
|
|
{
|
|
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];
|
|
}
|
|
App.RightLowCS1237_TrendArray[0] = AD;
|
|
}
|
|
else
|
|
{
|
|
// for(int i = OSC_CNT-1; i > 0; i--)
|
|
// {
|
|
// App.RightLowCS1237_TrendArray[i] = App.RightLowCS1237_TrendArray[i - 1];
|
|
// }
|
|
App.RightLowCS1237_TrendArray[0] = AD;
|
|
}
|
|
show(4,AD);
|
|
App.RightLowCS1237ReadEndFlag = true;
|
|
}
|
|
void GradeControl(ControlLevel_m level)
|
|
{
|
|
switch(level){
|
|
case LEVEL_1:{
|
|
App.ADTrack.Slope_VPT = SLOPE_VPT;
|
|
App.ADTrack.LeftUpVPT = 5000;
|
|
App.ADTrack.LeftLowVPT = 5000;
|
|
App.ADTrack.RightUpVPT = 5000;
|
|
App.ADTrack.RightLowVPT = 5000;
|
|
App.ADTrack.Stop_VPT = 4000;
|
|
break;}
|
|
|
|
case LEVEL_2:{
|
|
App.ADTrack.Slope_VPT = SLOPE_VPT;
|
|
App.ADTrack.LeftUpVPT = 10000;
|
|
App.ADTrack.LeftLowVPT = 10000;
|
|
App.ADTrack.RightUpVPT = 10000;
|
|
App.ADTrack.RightLowVPT = 10000;
|
|
App.ADTrack.Stop_VPT = 8000;
|
|
break;}
|
|
|
|
case LEVEL_3:{
|
|
App.ADTrack.Slope_VPT = SLOPE_VPT;
|
|
App.ADTrack.LeftUpVPT = 18000;
|
|
App.ADTrack.LeftLowVPT = 18000;
|
|
App.ADTrack.RightUpVPT = 18000;
|
|
App.ADTrack.RightLowVPT = 18000;
|
|
App.ADTrack.Stop_VPT = 15000;
|
|
break;}
|
|
|
|
case LEVEL_4:{
|
|
App.ADTrack.Slope_VPT = SLOPE_VPT;
|
|
App.ADTrack.LeftUpVPT = 25000;
|
|
App.ADTrack.LeftLowVPT = 25000;
|
|
App.ADTrack.RightUpVPT = 25000;
|
|
App.ADTrack.RightLowVPT = 25000;
|
|
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.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, 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++)
|
|
{
|
|
lu_diff[i] = lu_ad[i] - lu_ad[OSC_CNT - 1];
|
|
ll_diff[i] = ll_ad[i] - ll_ad[OSC_CNT - 1];
|
|
ru_diff[i] = ru_ad[i] - ru_ad[OSC_CNT - 1];
|
|
rl_diff[i] = rl_ad[i] - rl_ad[OSC_CNT - 1];
|
|
}
|
|
TrendResult result_lu = analyze_trend(lu_diff, OSC_CNT);
|
|
TrendResult result_ll = analyze_trend(ll_diff, OSC_CNT);
|
|
TrendResult result_ru = analyze_trend(ru_diff, OSC_CNT);
|
|
TrendResult result_rl = analyze_trend(rl_diff, OSC_CNT);
|
|
|
|
int32_t lu_slope = result_lu.slope;
|
|
int32_t ll_slope = result_ll.slope;
|
|
int32_t ru_slope = result_ru.slope;
|
|
int32_t rl_slope = result_rl.slope;
|
|
// show(1, lu_slope);
|
|
// show(2, ll_slope);
|
|
// show(3, ru_slope);
|
|
// show(4, rl_slope);
|
|
|
|
|
|
for(int i = SLOPE_CNT-1; i > 0; i--)
|
|
{
|
|
App.LeftUpCS1237_SlopeArray[i] = App.LeftUpCS1237_SlopeArray[i - 1];
|
|
App.LeftLowCS1237_SlopeArray[i] = App.LeftLowCS1237_SlopeArray[i - 1];
|
|
App.RightUpCS1237_SlopeArray[i] = App.RightUpCS1237_SlopeArray[i - 1];
|
|
App.RightLowCS1237_SlopeArray[i] = App.RightLowCS1237_SlopeArray[i - 1];
|
|
}
|
|
App.LeftUpCS1237_SlopeArray[0] = lu_slope;
|
|
App.LeftLowCS1237_SlopeArray[0] = ll_slope;
|
|
App.RightUpCS1237_SlopeArray[0] = ru_slope;
|
|
App.RightLowCS1237_SlopeArray[0] = rl_slope;
|
|
|
|
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 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)
|
|
{
|
|
if(App.DownFlag == true && App.EmergencyDelay1mSCnt > 0)
|
|
{
|
|
BDC_UP();
|
|
}
|
|
else if(App.UpFlag == true && App.EmergencyDelay1mSCnt > 0)
|
|
{
|
|
BDC_DOWN();
|
|
}
|
|
else if(App.EmergencyDelay1mSCnt <= 0)
|
|
{
|
|
BDC_STOP();
|
|
App.StopFlag = true;
|
|
App.DownFlag = false;
|
|
App.UpFlag = false;
|
|
App.EmeFlag = false;
|
|
App.StopCollDelay1mSCnt = STOP_TIME_MS;
|
|
App.SlopeCollDelay1mSCnt = SLOPE_START_MS;
|
|
}
|
|
}
|
|
else if(App.StopFlag == true && App.StopCollDelay1mSCnt <= 0)
|
|
{
|
|
App.LeftRunFalg = false;
|
|
App.RightRunFalg = false;
|
|
App.CentreRunFalg = false;
|
|
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(LeftDiff > RightDiff)//判断偏左
|
|
{
|
|
App.LeftRunFalg = true;
|
|
if(((Olu_diff1+Oru_diff1) > (App.ADTrack.Stop_VPT))//判断上拉
|
|
&& ((Olu_diff2+Oru_diff2) > (App.ADTrack.Stop_VPT))
|
|
&& ((Olu_diff3+Oru_diff3) > (App.ADTrack.Stop_VPT))
|
|
&& ((Oll_diff1+Orl_diff1) < -(App.ADTrack.Stop_VPT))
|
|
&& ((Oll_diff2+Orl_diff2) < -(App.ADTrack.Stop_VPT))
|
|
&& ((Oll_diff3+Orl_diff3) < -(App.ADTrack.Stop_VPT))
|
|
)
|
|
{
|
|
// if(App.CaliZeroFlag == true)//判断是否校零
|
|
{
|
|
if(App.MotorPara.Current_height < 1425)
|
|
{
|
|
BDC_UP();
|
|
App.CaliZeroFlag = false;
|
|
App.UpFlag = true;
|
|
App.StopFlag = false;
|
|
App.DownFlag = false;
|
|
App.EmeFlag = false;
|
|
App.RunCollDelay1mSCnt = RUN_TIME_MS;
|
|
}
|
|
}
|
|
}
|
|
else if(((Olu_diff1+Oru_diff1) < -(App.ADTrack.Stop_VPT))//判断下拉
|
|
&& ((Olu_diff2+Oru_diff2) < -(App.ADTrack.Stop_VPT))
|
|
&& ((Olu_diff3+Oru_diff3) < -(App.ADTrack.Stop_VPT))
|
|
&& ((Oll_diff1+Orl_diff1) > (App.ADTrack.Stop_VPT))
|
|
&& ((Oll_diff2+Orl_diff2) > (App.ADTrack.Stop_VPT))
|
|
&& ((Oll_diff3+Orl_diff3) > (App.ADTrack.Stop_VPT))
|
|
)
|
|
{
|
|
// if(App.CaliZeroFlag == true)//判断是否校零
|
|
{
|
|
if(App.MotorPara.Current_height > 1005)
|
|
{
|
|
BDC_DOWN();
|
|
App.CaliZeroFlag = false;
|
|
App.UpFlag = false;
|
|
App.StopFlag = false;
|
|
App.DownFlag = true;
|
|
App.EmeFlag = false;
|
|
App.RunCollDelay1mSCnt = RUN_TIME_MS;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else if(RightDiff > LeftDiff)//判断偏右
|
|
{
|
|
App.RightRunFalg = true;
|
|
if(((Olu_diff1+Oru_diff1) > (App.ADTrack.Stop_VPT))//判断上拉
|
|
&& ((Olu_diff2+Oru_diff2) > (App.ADTrack.Stop_VPT))
|
|
&& ((Olu_diff3+Oru_diff3) > (App.ADTrack.Stop_VPT))
|
|
&& ((Oll_diff1+Orl_diff1) < -(App.ADTrack.Stop_VPT))
|
|
&& ((Oll_diff2+Orl_diff2) < -(App.ADTrack.Stop_VPT))
|
|
&& ((Oll_diff3+Orl_diff3) < -(App.ADTrack.Stop_VPT))
|
|
)
|
|
{
|
|
// if(App.CaliZeroFlag == true)//判断是否校零
|
|
{
|
|
if(App.MotorPara.Current_height < 1425)
|
|
{
|
|
BDC_UP();
|
|
App.CaliZeroFlag = false;
|
|
App.UpFlag = true;
|
|
App.StopFlag = false;
|
|
App.DownFlag = false;
|
|
App.EmeFlag = false;
|
|
App.RunCollDelay1mSCnt = RUN_TIME_MS;
|
|
}
|
|
}
|
|
}
|
|
else if(((Olu_diff1+Oru_diff1) < -(App.ADTrack.Stop_VPT))//判断下拉
|
|
&& ((Olu_diff2+Oru_diff2) < -(App.ADTrack.Stop_VPT))
|
|
&& ((Olu_diff3+Oru_diff3) < -(App.ADTrack.Stop_VPT))
|
|
&& ((Oll_diff1+Orl_diff1) > (App.ADTrack.Stop_VPT))
|
|
&& ((Oll_diff2+Orl_diff2) > (App.ADTrack.Stop_VPT))
|
|
&& ((Oll_diff3+Orl_diff3) > (App.ADTrack.Stop_VPT))
|
|
)
|
|
{
|
|
// if(App.CaliZeroFlag == true)//判断是否校零
|
|
{
|
|
if(App.MotorPara.Current_height > 1005)
|
|
{
|
|
BDC_DOWN();
|
|
App.CaliZeroFlag = false;
|
|
App.UpFlag = false;
|
|
App.StopFlag = false;
|
|
App.DownFlag = true;
|
|
App.EmeFlag = false;
|
|
App.RunCollDelay1mSCnt = RUN_TIME_MS;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else if((LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum && App.CentCollDelay1mSCnt <= 0))//判断左边操作
|
|
{
|
|
App.LeftRunFalg = true;
|
|
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))
|
|
&& (Oll_diff2 <= -(App.ADTrack.LeftLowVPT*5/6))
|
|
&& (Oll_diff3 <= -(App.ADTrack.LeftLowVPT*5/6)))
|
|
{
|
|
if(((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((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 < 1425)
|
|
{
|
|
BDC_UP();
|
|
App.CaliZeroFlag = false;
|
|
App.UpFlag = true;
|
|
App.DownFlag = false;
|
|
App.StopFlag = false;
|
|
App.EmeFlag = false;
|
|
App.RunCollDelay1mSCnt = RUN_TIME_MS;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
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))
|
|
&& (Oll_diff2 >= (App.ADTrack.LeftLowVPT*5/6))
|
|
&& (Oll_diff3 >= (App.ADTrack.LeftLowVPT*5/6)))
|
|
)
|
|
{
|
|
if(((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((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 > 1005)
|
|
{
|
|
BDC_DOWN();
|
|
App.CaliZeroFlag = false;
|
|
App.UpFlag = false;
|
|
App.DownFlag = true;
|
|
App.StopFlag = false;
|
|
App.EmeFlag = false;
|
|
App.RunCollDelay1mSCnt = RUN_TIME_MS;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else if(LeftSlopeSum < RightSlopeSum && LeftDiffSum < RightDiffSum && App.CentCollDelay1mSCnt <= 0)//判断右边操作
|
|
{
|
|
App.RightRunFalg = true;
|
|
if((Oru_diff1 >= (App.ADTrack.RightUpVPT*5/6)) //通过差值判断上下拉方向
|
|
&& (Oru_diff2 >= (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)))
|
|
{
|
|
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))
|
|
)
|
|
{
|
|
// 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(App.MotorPara.Current_height < 1425)
|
|
{
|
|
BDC_UP();
|
|
App.CaliZeroFlag = false;
|
|
App.UpFlag = true;
|
|
App.StopFlag = false;
|
|
App.DownFlag = false;
|
|
App.EmeFlag = false;
|
|
App.RunCollDelay1mSCnt = RUN_TIME_MS;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else if((Oru_diff1 <= -(App.ADTrack.RightUpVPT*5/6)) //通过差值判断上下拉方向
|
|
&& (Oru_diff2 <= -(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)))
|
|
{
|
|
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))
|
|
)
|
|
{
|
|
// 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(App.MotorPara.Current_height > 1005)
|
|
{
|
|
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.DownFlag == true && App.RunCollDelay1mSCnt <= 0)
|
|
{
|
|
if(App.LeftRunFalg == true)
|
|
{
|
|
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 >= 1425
|
|
|| App.MotorPara.Current_height <= 1005
|
|
)
|
|
{
|
|
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((abs(Oll_diff1) + EME_STOP_VPT) < abs(Orl_diff1)
|
|
// && (abs(Oll_diff2) + EME_STOP_VPT) < abs(Orl_diff2)
|
|
// && (abs(Oll_diff3) + EME_STOP_VPT) < abs(Orl_diff3)
|
|
// && (abs(Olu_diff1) + EME_STOP_VPT) < abs(Oru_diff1)
|
|
// && (abs(Olu_diff2) + EME_STOP_VPT) < abs(Oru_diff2)
|
|
// && (abs(Olu_diff3) + EME_STOP_VPT) < abs(Oru_diff3)
|
|
// && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
|
|
// {
|
|
// BDC_STOP();
|
|
// App.EmeFlag = true;
|
|
// App.EmergencyDelay1mSCnt = EME_STOP_MS;
|
|
// }
|
|
}
|
|
else if(App.RightRunFalg == true)
|
|
{
|
|
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 >= 1425
|
|
|| App.MotorPara.Current_height <= 1005
|
|
)
|
|
{
|
|
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(abs(Oll_diff1) > (abs(Orl_diff1) + EME_STOP_VPT)
|
|
// && abs(Oll_diff2) > (abs(Orl_diff2) + EME_STOP_VPT)
|
|
// && abs(Oll_diff3) > (abs(Orl_diff3) + EME_STOP_VPT)
|
|
// && abs(Olu_diff1) > (abs(Oru_diff1) + EME_STOP_VPT)
|
|
// && abs(Olu_diff2) > (abs(Oru_diff2) + EME_STOP_VPT)
|
|
// && abs(Olu_diff3) > (abs(Oru_diff3) + EME_STOP_VPT)
|
|
// && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
|
|
// {
|
|
// BDC_STOP();
|
|
// App.EmeFlag = true;
|
|
// App.EmergencyDelay1mSCnt = EME_STOP_MS;
|
|
// }
|
|
}
|
|
}
|
|
else if(App.UpFlag == true && App.RunCollDelay1mSCnt <= 0)
|
|
{
|
|
if(App.LeftRunFalg == true)
|
|
{
|
|
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 >= 1425
|
|
|| App.MotorPara.Current_height <= 1005
|
|
)
|
|
{
|
|
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((abs(Oll_diff1) + EME_STOP_VPT) < abs(Orl_diff1)
|
|
// && (abs(Oll_diff2) + EME_STOP_VPT) < abs(Orl_diff2)
|
|
// && (abs(Oll_diff3) + EME_STOP_VPT) < abs(Orl_diff3)
|
|
// && (abs(Olu_diff1) + EME_STOP_VPT) < abs(Oru_diff1)
|
|
// && (abs(Olu_diff2) + EME_STOP_VPT) < abs(Oru_diff2)
|
|
// && (abs(Olu_diff3) + EME_STOP_VPT) < abs(Oru_diff3)
|
|
// && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
|
|
// {
|
|
// BDC_STOP();
|
|
// App.EmeFlag = true;
|
|
// App.EmergencyDelay1mSCnt = EME_STOP_MS;
|
|
// }
|
|
}
|
|
else if(App.RightRunFalg == true)
|
|
{
|
|
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 >= 1425
|
|
|| App.MotorPara.Current_height <= 1005
|
|
)
|
|
{
|
|
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(abs(Oll_diff1) > (abs(Orl_diff1) + EME_STOP_VPT)
|
|
// && abs(Oll_diff2) > (abs(Orl_diff2) + EME_STOP_VPT)
|
|
// && abs(Oll_diff3) > (abs(Orl_diff3) + EME_STOP_VPT)
|
|
// && abs(Olu_diff1) > (abs(Oru_diff1) + EME_STOP_VPT)
|
|
// && abs(Olu_diff2) > (abs(Oru_diff2) + EME_STOP_VPT)
|
|
// && abs(Olu_diff3) > (abs(Oru_diff3) + EME_STOP_VPT)
|
|
// && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
|
|
// {
|
|
// BDC_STOP();
|
|
// App.EmeFlag = true;
|
|
// App.EmergencyDelay1mSCnt = EME_STOP_MS;
|
|
// }
|
|
}
|
|
}
|
|
|
|
if(
|
|
// lu_K.peaks_positive
|
|
// && lu_K.valleys_negative
|
|
App.LeftUpCS1237_SlopeArray[0] < FOLLOW_VPT
|
|
&& App.LeftUpCS1237_SlopeArray[0] > -FOLLOW_VPT
|
|
&& App.LeftUpCS1237_SlopeArray[1] < FOLLOW_VPT
|
|
&& App.LeftUpCS1237_SlopeArray[1] > -FOLLOW_VPT
|
|
&& App.LeftUpCS1237_SlopeArray[2] < FOLLOW_VPT
|
|
&& App.LeftUpCS1237_SlopeArray[2] > -FOLLOW_VPT
|
|
// && ll_K.peaks_positive
|
|
// && ll_K.valleys_negative
|
|
&& App.LeftLowCS1237_SlopeArray[0] < FOLLOW_VPT
|
|
&& App.LeftLowCS1237_SlopeArray[0] > -FOLLOW_VPT
|
|
&& App.LeftLowCS1237_SlopeArray[1] < FOLLOW_VPT
|
|
&& App.LeftLowCS1237_SlopeArray[1] > -FOLLOW_VPT
|
|
&& App.LeftLowCS1237_SlopeArray[2] < FOLLOW_VPT
|
|
&& App.LeftLowCS1237_SlopeArray[2] > -FOLLOW_VPT
|
|
// && ru_K.peaks_positive
|
|
// && ru_K.valleys_negative
|
|
&& App.RightUpCS1237_SlopeArray[0] < FOLLOW_VPT
|
|
&& App.RightUpCS1237_SlopeArray[0] > -FOLLOW_VPT
|
|
&& App.RightUpCS1237_SlopeArray[1] < FOLLOW_VPT
|
|
&& App.RightUpCS1237_SlopeArray[1] > -FOLLOW_VPT
|
|
&& App.RightUpCS1237_SlopeArray[2] < FOLLOW_VPT
|
|
&& App.RightUpCS1237_SlopeArray[2] > -FOLLOW_VPT
|
|
// && rl_K.peaks_positive
|
|
// && rl_K.valleys_negative
|
|
&& App.RightLowCS1237_SlopeArray[0] < FOLLOW_VPT
|
|
&& App.RightLowCS1237_SlopeArray[0] > -FOLLOW_VPT
|
|
&& App.RightLowCS1237_SlopeArray[1] < FOLLOW_VPT
|
|
&& App.RightLowCS1237_SlopeArray[1] > -FOLLOW_VPT
|
|
&& App.RightLowCS1237_SlopeArray[2] < FOLLOW_VPT
|
|
&& App.RightLowCS1237_SlopeArray[2] > -FOLLOW_VPT)
|
|
{
|
|
if(App.StopFlag == true
|
|
&& App.StopCollDelay1mSCnt <= 0
|
|
&& 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];
|
|
App.ADTrack.RightLow_Org_AdZero = rl_ad[0];
|
|
App.CaliZeroFlag = true;
|
|
}
|
|
}
|
|
}
|
|
#endif
|
|
static void AppInit(void)
|
|
{
|
|
memset(&App, 0x00, sizeof(App));
|
|
READ_APP_PARA((uint32_t *)&App.Para);
|
|
if(App.Para.FactoryFlag != 0x00){
|
|
App.Para.FactoryFlag = 0x00;
|
|
/*参数初始化*/
|
|
App.Para.Layout.ControlLevel = LEVEL_3;//出厂默认等级3
|
|
App.Para.Layout.DBGWaveOut = false;
|
|
SAVE_APP_PARA((uint32_t *)&App.Para);
|
|
}
|
|
if(App.Para.Layout.ControlLevel > 5 || App.Para.Layout.ControlLevel < 1){
|
|
App.Para.Layout.ControlLevel = LEVEL_3;//出厂默认等级3
|
|
SAVE_APP_PARA((uint32_t *)&App.Para);
|
|
}
|
|
DBG_LOG("\r\n\r\n");
|
|
DBG_LOG("****************************************************\r\n");
|
|
DBG_LOG("** **\r\n");
|
|
DBG_LOG("** Acc_Support_29V **\r\n");
|
|
DBG_LOG("** SoftWare V:%d.%d **\r\n", SOFTWARE_VERSION / 10, SOFTWARE_VERSION % 10);
|
|
DBG_LOG("** HardWare V:%d.%d **\r\n", HARDWARE_VERSION / 10, HARDWARE_VERSION % 10);
|
|
DBG_LOG("** Compile: %s %s **\r\n", __DATE__, __TIME__);
|
|
DBG_LOG("** **\r\n");
|
|
DBG_LOG("****************************************************\r\n\r\n");
|
|
|
|
//FeedDog();
|
|
LEFTUP_CS1237_Init();
|
|
LEFTLOW_CS1237_Init();
|
|
RIGHTUP_CS1237_Init();
|
|
RIGHTLOW_CS1237_Init();
|
|
|
|
BDC_STOP();
|
|
LED_RED_OFF();
|
|
LED_GREEN_ON();
|
|
App.Status = APP_STATUS_START_BEEP;
|
|
App.MotorPara.InitialFlag = false;
|
|
App.RStatus = READ_STATUS_START;
|
|
GradeControl(App.Para.Layout.ControlLevel);//等级控制
|
|
|
|
App.WaitFlag = false;
|
|
}
|
|
static void ReadLoopHandler(void)
|
|
{
|
|
switch(App.RStatus){
|
|
case READ_STATUS_IDLE:{
|
|
break;}
|
|
|
|
case READ_STATUS_START:{
|
|
App.LeftUpCS1237ReadEndFlag = false;
|
|
App.LeftLowCS1237ReadEndFlag = false;
|
|
App.RightUpCS1237ReadEndFlag = false;
|
|
App.RightLowCS1237ReadEndFlag = false;
|
|
App.RStatus = READ_STATUS_LEFTUP;
|
|
break;}
|
|
|
|
case READ_STATUS_LEFTUP:{
|
|
LEFTUP_CS1237Start();
|
|
App.Read1mSDelayCnt = 50;
|
|
App.RStatus = READ_STATUS_WAIT_LEFTUP;
|
|
break;}
|
|
|
|
case READ_STATUS_WAIT_LEFTUP:{
|
|
if(App.Read1mSDelayCnt <= 0)
|
|
{
|
|
App.RStatus = READ_STATUS_LEFTLOW;
|
|
}
|
|
else if(App.LeftUpCS1237ReadEndFlag == true)
|
|
{
|
|
App.RStatus = READ_STATUS_LEFTLOW;
|
|
}
|
|
break;}
|
|
|
|
case READ_STATUS_LEFTLOW:{
|
|
LEFTLOW_CS1237Start();
|
|
App.Read1mSDelayCnt = 50;
|
|
App.RStatus = READ_STATUS_WAIT_LEFTLOW;
|
|
break;}
|
|
|
|
case READ_STATUS_WAIT_LEFTLOW:{
|
|
if(App.Read1mSDelayCnt <= 0)
|
|
{
|
|
App.RStatus = READ_STATUS_RIGHTUP;
|
|
}
|
|
else if(App.LeftLowCS1237ReadEndFlag == true)
|
|
{
|
|
App.RStatus = READ_STATUS_RIGHTUP;
|
|
}
|
|
break;}
|
|
|
|
case READ_STATUS_RIGHTUP:{
|
|
RIGHTUP_CS1237Start();
|
|
App.Read1mSDelayCnt = 50;
|
|
App.RStatus = READ_STATUS_WAIT_RIGHTUP;
|
|
break;}
|
|
|
|
case READ_STATUS_WAIT_RIGHTUP:{
|
|
if(App.Read1mSDelayCnt <= 0)
|
|
{
|
|
App.RStatus = READ_STATUS_RIGHTLOW;
|
|
}
|
|
else if(App.RightUpCS1237ReadEndFlag == true)
|
|
{
|
|
App.RStatus = READ_STATUS_RIGHTLOW;
|
|
}
|
|
break;}
|
|
|
|
case READ_STATUS_RIGHTLOW:{
|
|
RIGHTLOW_CS1237Start();
|
|
App.Read1mSDelayCnt = 50;
|
|
App.RStatus = READ_STATUS_WAIT_RIGHTLOW;
|
|
break;}
|
|
|
|
case READ_STATUS_WAIT_RIGHTLOW:{
|
|
if(App.Read1mSDelayCnt <= 0)
|
|
{
|
|
App.RStatus = READ_STATUS_SUPPORT_CONT;
|
|
}
|
|
else if(App.RightLowCS1237ReadEndFlag == true)
|
|
{
|
|
App.RStatus = READ_STATUS_SUPPORT_CONT;
|
|
}
|
|
break;}
|
|
|
|
case READ_STATUS_SUPPORT_CONT:{
|
|
if(App.LeftUpCS1237ReadEndFlag == true &&
|
|
App.LeftLowCS1237ReadEndFlag == true &&
|
|
App.RightUpCS1237ReadEndFlag == true &&
|
|
App.RightLowCS1237ReadEndFlag == true &&
|
|
App.WaitFlag == true)
|
|
{
|
|
#if(USE_AGC == 1)
|
|
SupportControlHandle(App.LeftUpCS1237_TrendArray, App.LeftLowCS1237_TrendArray, App.RightUpCS1237_TrendArray, App.RightLowCS1237_TrendArray);
|
|
#endif
|
|
}
|
|
App.RStatus = READ_STATUS_START;
|
|
break;}
|
|
}
|
|
}
|
|
|
|
static void AppLoopHandler(void)
|
|
{
|
|
switch(App.Status) {
|
|
case APP_STATUS_IDLE:{
|
|
break;}
|
|
|
|
case APP_STATUS_ON:{
|
|
App.Status = APP_STATUS_IDLE;
|
|
break;}
|
|
|
|
case APP_STATUS_OFF:{
|
|
|
|
App.Status = APP_STATUS_IDLE;
|
|
break;}
|
|
|
|
case APP_STATUS_START_BEEP:{
|
|
LED_RED_ON();
|
|
LED_GREEN_OFF();
|
|
Beeper_Start(200,200,2);
|
|
App.TD1mSDelayCnt = 60000;//上电复位触发超时: 1分钟无复位帧返回则转异常状态
|
|
App.PwRstNoChg1mSCnt = 0;//清到底检测计数
|
|
App.PwRstLastHeight = App.MotorPara.Current_height;//记录初始高度用于到底检测
|
|
App.PwRstRiseRef = App.MotorPara.Current_height;//反弹判定参考高度初始化(只跟跌不跟涨)
|
|
App.PwRstHtSeen = false;//高度基准未就绪, 收到首帧有效高度后再初始化参考值并启动到底检测
|
|
App.PwRstReboundCnt = 0;//清连续反弹计数
|
|
App.PwRstMinHeight = 1430;//历史最低高度初始化为量程上限, 收到首帧有效高度时精确初始化
|
|
App.PwRstBottomFlag = false;
|
|
App.PwRstInchMode = false;
|
|
App.PwRstInch1mSCnt = 0;
|
|
App.PwRstInchNoMvCnt = 0;
|
|
App.PwRstErrValid = false;//清报错码处理标志, 上电后首个报错码触发一次停顿
|
|
App.PwRstErrCode = 0;
|
|
App.PwRstSamp1mSCnt = 0;//清高度采样计时
|
|
App.PwRstRiseCandCnt = 0;//清反弹候选计时
|
|
App.PwRstRiseCandMax = 0;//清反弹候选最高点
|
|
App.PwRstStartDlyCnt = 2000;//上电先延时2秒: 整体断电再上电时控制盒自身初始化未完成, 立即下降会因状态切换抖动误触发碰撞保护
|
|
App.PwRstMovedFlag = false;//未发生位移: 延时/控制盒软启动期间高度停滞不判堵转到底
|
|
App.Status = APP_STATUS_POWER_RESET;//上电先走复位流程, 保证换控制盒后高度基准一致, 再进入校准
|
|
break;}
|
|
|
|
case APP_STATUS_POWER_RESET:{
|
|
/* 上电复位流程: 不判断高度一直下降(换控制盒后编码器可能错位, 高度信息不可信)。
|
|
到底检测(1ms例程): 反弹(高度较参考点反升>1cm, 控制盒碰撞保护主动反向)首次只
|
|
停1秒恢复下降观察(常规到底接触的小反弹会自行稳定), 再次反弹才判定碰撞转点动
|
|
下降(模拟手按, 每1000ms降200ms停800ms)一点点挪, 连续2个点动周期无位移才最终
|
|
确认到底且需接近历史最低点(停在高处=被障碍物/碰撞保护托住, 转点动试探; 全程
|
|
未反弹则高度2秒无变化确认)→停1秒→持续下降触发控制盒发复位
|
|
帧(01 04)→停2s→再下降→控制盒完成复位发高度帧(编码器基准重置, 高度一致)→直接
|
|
记录零点开始校准(去掉起步动作)。到底确认时清反弹计数, 确认后的下压反弹只停
|
|
1秒继续下压不转点动。
|
|
1分钟无复位帧返回则转异常状态(下降/点动有位移时自动刷新超时, 恢复流程不被打断) */
|
|
if(App.PwRstStartDlyCnt > 0)
|
|
{ //上电延时2秒: 等电机控制盒自身初始化完成再开始下降(延时期间控制盒发出的
|
|
//首帧高度帧同步把高度基准就绪), 避免控制盒未就绪时收到下降指令产生状态切换抖动
|
|
BDC_STOP();
|
|
break;}
|
|
if(App.MotorPara.RestFlag == true)
|
|
{ //收到复位帧: 复位流程进行中
|
|
App.PowerResetFlag = true;
|
|
if(App.RestStop1mSCnt > 0)
|
|
{
|
|
BDC_STOP();//复位停顿: 先停止电机, 让控制盒进入复位流程
|
|
}
|
|
else
|
|
{
|
|
BDC_DOWN();//停顿结束: 下降完成复位, 收到高度帧后RestFlag清除
|
|
}
|
|
App.TD1mSDelayCnt = 60000;//复位进行中, 刷新1分钟超时
|
|
}
|
|
else if(App.PowerResetFlag == true)
|
|
{ //复位完成(收到高度帧, 复位标志清除): 车已在底部, 直接记录零点开始校准
|
|
App.PowerResetFlag = false;
|
|
BDC_STOP();
|
|
App.StopFlag = true;
|
|
App.DownFlag = false;
|
|
App.UpFlag = false;
|
|
App.MotorPara.CaliFlag = false;
|
|
App.TD1mSDelayCnt = 1500;
|
|
App.Status = APP_STATUS_ZERO_MARKING;
|
|
}
|
|
else if(App.TD1mSDelayCnt <= 0)
|
|
{ //1分钟无复位帧返回: 转异常状态(停机待机)
|
|
BDC_STOP();
|
|
App.StopFlag = true;
|
|
App.DownFlag = false;
|
|
App.UpFlag = false;
|
|
App.Status = APP_STATUS_IDLE;
|
|
}
|
|
else if(App.PwRstStop1mSCnt > 0)
|
|
{ //停顿: 反弹后或最终确认到底后先停止电机片刻(让控制盒碰撞保护状态稳定)
|
|
BDC_STOP();
|
|
}
|
|
else if(App.PwRstBottomFlag == true)
|
|
{ //最终确认到底: 和正常到底复位流程一样, 持续下降触发控制盒复位帧
|
|
//若再次触发碰撞保护(高度反升), 1ms例程会重新停1秒转点动
|
|
BDC_DOWN();
|
|
}
|
|
else if(App.PwRstInchMode == true)
|
|
{ //点动下降(发生反弹后, 确认到底前): 模拟手按节奏(每1000ms降200ms停800ms),
|
|
//一点点往下挪, 冲击小不触发碰撞保护
|
|
if(App.PwRstInch1mSCnt < 200)
|
|
{
|
|
BDC_DOWN();
|
|
}
|
|
else
|
|
{
|
|
BDC_STOP();
|
|
if(App.PwRstInch1mSCnt >= 1000)
|
|
{
|
|
App.PwRstInch1mSCnt = 0;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{ //下降中(未触发过反弹): 到底检测在App1mSRoutine(真实1ms)
|
|
BDC_DOWN();//一直下降, 直到确认到底
|
|
}
|
|
break;}
|
|
|
|
case APP_STATUS_ZERO_MARKING:{//上电复位完成后直接进入此状态记录零点(起步动作已去掉)
|
|
if(App.TD1mSDelayCnt <= 0)
|
|
{
|
|
App.ADTrack.LeftUp_Org_AdZero = App.LeftUpCS1237_TrendArray[0];
|
|
App.ADTrack.LeftLow_Org_AdZero = App.LeftLowCS1237_TrendArray[0];
|
|
App.ADTrack.RightUp_Org_AdZero = App.RightUpCS1237_TrendArray[0];
|
|
App.ADTrack.RightLow_Org_AdZero = App.RightLowCS1237_TrendArray[0];
|
|
// DBG_LOG("Frist Pack Data LeftUp Zero AD: %08d\tLeftUp Zero AD: %08d\tRightUp Zero AD: %08d\tRightLow Zero AD: %08d\r\n",
|
|
// App.ADTrack.LeftUp_Org_AdZero,
|
|
// 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;
|
|
App.TD1mSDelayCnt = TRAVEL_TIME_MS;
|
|
App.StopFlag = false;
|
|
App.UpFlag = true;
|
|
BDC_UP();
|
|
App.CailUp1mSCnt = 0;
|
|
App.MotorPara.LoopOne = false;
|
|
}
|
|
break;}
|
|
|
|
case APP_STATUS_WAIT_MOTOR_CALI_UP:{
|
|
if(App.MotorPara.Current_height >= 1430 && App.CailUp1mSCnt >= 17000)
|
|
{
|
|
App.CailUp1mSCnt = 0;
|
|
BDC_STOP();
|
|
App.StopFlag = true;
|
|
App.DownFlag = false;
|
|
App.UpFlag = false;
|
|
Ddl_Delay1ms(1500);
|
|
App.DownFlag = true;
|
|
App.StopFlag = false;
|
|
App.Status = APP_STATUS_WAIT_MOTOR_CALI_DOWN;
|
|
BDC_DOWN();
|
|
}
|
|
else if(App.MotorPara.CaliNum >= 410 && 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");
|
|
weightedMovingAverageWithEnhance(App.MotorPara.LU_HeightStress,431,7,341,430,4,0);
|
|
// DBG_LOG("===================LL Data monitoring====================\r\n");
|
|
weightedMovingAverageWithEnhance(App.MotorPara.LL_HeightStress,431,7,341,430,4,0);
|
|
// DBG_LOG("===================RU Data monitoring====================\r\n");
|
|
weightedMovingAverageWithEnhance(App.MotorPara.RU_HeightStress,431,7,341,430,4,0);
|
|
// DBG_LOG("===================RL Data monitoring====================\r\n");
|
|
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;
|
|
}
|
|
break;}
|
|
|
|
case APP_STATUS_WAIT_MOTOR_CALI_DOWN:{
|
|
if(App.MotorPara.Current_height <= 1005)
|
|
{
|
|
BDC_STOP();
|
|
App.StopFlag = true;
|
|
App.DownFlag = false;
|
|
App.UpFlag = false;
|
|
App.MotorPara.LoopOne = true;
|
|
Ddl_Delay1ms(1500);
|
|
App.UpFlag = true;
|
|
App.StopFlag = false;
|
|
App.Status = APP_STATUS_WAIT_MOTOR_CALI_UP;
|
|
BDC_UP();
|
|
App.CailUp1mSCnt = 0;
|
|
}
|
|
else if(App.MotorPara.CaliNum >= 410 && 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");
|
|
weightedMovingAverageWithEnhance(App.MotorPara.LU_HeightStress,431,7,341,430,4,0);
|
|
// DBG_LOG("===================LL Data monitoring====================\r\n");
|
|
weightedMovingAverageWithEnhance(App.MotorPara.LL_HeightStress,431,7,341,430,4,0);
|
|
// DBG_LOG("===================RU Data monitoring====================\r\n");
|
|
weightedMovingAverageWithEnhance(App.MotorPara.RU_HeightStress,431,7,341,430,4,0);
|
|
// DBG_LOG("===================RL Data monitoring====================\r\n");
|
|
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;
|
|
}
|
|
break;}
|
|
|
|
case APP_STATUS_WAIT_STEADY:{
|
|
if(App.TD1mSDelayCnt <= 0)
|
|
{
|
|
App.SideFlag = false;
|
|
App.CaliZeroFlag = false;
|
|
App.UpFlag = false;
|
|
App.DownFlag = false;
|
|
App.EmeFlag = false;
|
|
App.StopFlag = true;
|
|
App.WaitFlag = true;
|
|
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.OverheatFlag == true)
|
|
{
|
|
if(App.MotorPara.Current_height <= 1005)
|
|
{
|
|
BDC_DOWN();//过热+低位: 继续下降触发控制盒复位(SupportControlHandle在<=1005时不执行BDC_DOWN)
|
|
}
|
|
//过热+高位(>1005): 由SupportControlHandle根据传感器正常判断下降
|
|
}
|
|
else if(App.MotorPara.RestFlag == true)
|
|
{
|
|
if(App.RestStop1mSCnt > 0)
|
|
{
|
|
BDC_STOP();//复位停顿: 先停止电机, 让控制盒进入复位流程
|
|
}
|
|
else
|
|
{
|
|
BDC_DOWN();//停顿结束: 再次下降, 触发控制盒完成复位(收到高度帧后RestFlag清除)
|
|
}
|
|
}
|
|
else if(App.MotorPara.Current_height <= 1005
|
|
&& App.MotorPara.RestFlag == false
|
|
&& App.HeightContRolCnt >= 500
|
|
&& GET_BIT0_DOUT() == 1
|
|
&& GET_BIT1_DOUT() == 1)
|
|
{
|
|
App.HeightContRolCnt = 0;
|
|
BDC_UP();
|
|
Ddl_Delay1ms(500);
|
|
BDC_STOP();
|
|
}
|
|
else if(App.MotorPara.Current_height >= 1425
|
|
&& App.MotorPara.RestFlag == false
|
|
&& App.HeightContRolCnt >= 500
|
|
&& GET_BIT0_DOUT() == 1
|
|
&& GET_BIT1_DOUT() == 1)
|
|
{
|
|
App.HeightContRolCnt = 0;
|
|
BDC_DOWN();
|
|
Ddl_Delay1ms(500);
|
|
BDC_STOP();
|
|
}
|
|
break;}
|
|
}
|
|
}
|
|
static void App1mSRoutine(void)
|
|
{
|
|
if(App.TD1mSDelayCnt > 0)
|
|
App.TD1mSDelayCnt--;
|
|
if(App.Read1mSDelayCnt > 0)
|
|
App.Read1mSDelayCnt--;
|
|
if(App.Uart1Delay1mSCnt > 0)
|
|
App.Uart1Delay1mSCnt--;
|
|
if(App.Uart3RxTimeOut1mSCnt > 0)
|
|
App.Uart3RxTimeOut1mSCnt--;
|
|
if(App.RestStop1mSCnt > 0)
|
|
App.RestStop1mSCnt--;
|
|
if(App.PwRstStartDlyCnt > 0)
|
|
App.PwRstStartDlyCnt--;//上电复位开始前延时(等控制盒初始化)
|
|
if(App.PwRstStop1mSCnt > 0)
|
|
{
|
|
App.PwRstStop1mSCnt--;
|
|
if(App.PwRstStop1mSCnt == 0)
|
|
{ //停顿结束: 反弹参考点重置为当前高度, 避免停顿中控制盒抬升高度导致恢复下降后立即误判反弹
|
|
App.PwRstRiseRef = App.MotorPara.Current_height;
|
|
App.PwRstRiseCandCnt = 0;//清反弹候选(停顿期间残留计时, 防止恢复后快速误确认)
|
|
App.PwRstRiseCandMax = App.MotorPara.Current_height;
|
|
}
|
|
}
|
|
if(App.StopCollDelay1mSCnt > 0)
|
|
App.StopCollDelay1mSCnt--;
|
|
if(App.RunCollDelay1mSCnt > 0)
|
|
App.RunCollDelay1mSCnt--;
|
|
if(App.EmergencyDelay1mSCnt > 0)
|
|
App.EmergencyDelay1mSCnt--;
|
|
if(App.SlopeCollDelay1mSCnt > 0)
|
|
App.SlopeCollDelay1mSCnt--;
|
|
if(App.CentCollDelay1mSCnt > 0)
|
|
App.CentCollDelay1mSCnt--;
|
|
if(App.MotorPara.InitialFlag == true)
|
|
App.CailUp1mSCnt++;
|
|
if((App.MotorPara.Current_height < 1005 || App.MotorPara.Current_height > 1425))
|
|
{
|
|
App.HeightContRolCnt++;
|
|
}
|
|
else
|
|
{
|
|
App.HeightContRolCnt = 0;
|
|
}
|
|
if(App.Status == APP_STATUS_POWER_RESET && App.PwRstHtSeen == true)
|
|
{ //上电复位高度采样: 每秒记录一次当前高度(覆盖停顿/点动/复位全程), 打印完整下降曲线,
|
|
//直观展示控制盒上报高度的变化趋势, 用于排查反弹是物理现象还是逻辑误判
|
|
if(++App.PwRstSamp1mSCnt >= 1000)
|
|
{
|
|
App.PwRstSamp1mSCnt = 0;
|
|
Uart3ErrLog_PushBottom(8, (uint16_t)App.MotorPara.Current_height);
|
|
}
|
|
}
|
|
if(App.Status == APP_STATUS_POWER_RESET
|
|
&& App.MotorPara.RestFlag == false //复位进行中不检测
|
|
&& App.PowerResetFlag == false //复位完成不检测
|
|
&& App.PwRstStop1mSCnt == 0 //停顿中不检测
|
|
&& App.PwRstStartDlyCnt == 0 //上电延时中不检测(车静止高度不变, 会误判堵转到底)
|
|
&& App.PwRstHtSeen == true) //未收到首帧有效高度不检测(高度基准未就绪)
|
|
{ //上电复位下降中: 到底检测(真实1ms计时, 主循环频率不定不能在此计数)
|
|
//控制盒换新后缓存高度偏大(1000+), 到995本该减速仍全速下降, 硬触底产生震荡,
|
|
//控制盒碰撞保护主动控制电机反向。策略: 首次反弹(高度较参考点反升>1cm)只停1秒
|
|
//恢复下降观察(常规到底接触的小反弹会自行稳定, 接着2秒堵转确认走正常流程),
|
|
//再次反弹才判定硬触底震荡碰撞转点动下降(模拟手按, 约1秒一次短脉冲), 连续2个
|
|
//点动周期无位移判定到底, 之后和正常到底复位流程一样: 停1秒→下降触发复位帧
|
|
//→停2s→下降→收到高度帧复位完成。到底确认时清反弹计数, 确认后下压的反弹同样
|
|
//只停1秒继续下压, 不会转点动
|
|
//未反弹时堵转判定为高度2秒无变化(碰撞悬停时高度会冻结1~2秒, 尽量避开误判)
|
|
//下降/点动有位移时刷新上电复位1分钟超时, 恢复流程不因总时长过长被停机
|
|
//历史最低高度参考: 确认到底必须接近最低点, 停在高处=被障碍物/碰撞保护托住, 转点动试探
|
|
if(App.MotorPara.Current_height < App.PwRstMinHeight)
|
|
{ //记录本次复位下降过程的历史最低高度(到底只可能是最低点)
|
|
App.PwRstMinHeight = App.MotorPara.Current_height;
|
|
}
|
|
if(App.MotorPara.Current_height > App.PwRstRiseRef + 10)
|
|
{ //高度较参考点反升>1cm: 碰撞反弹候选(300ms持续确认, 滤除瞬时抖动毛刺)
|
|
//启动/急停时机械抖动造成的高度毛刺只持续1~2帧, 直接判定会把正常高度误判为反弹
|
|
//参考点只跟跌不跟涨, 高度帧逐帧上升可跨帧累计检出, 不受帧率影响
|
|
App.PwRstNoChg1mSCnt = 0;
|
|
if(App.PwRstRiseCandCnt == 0)
|
|
{
|
|
App.PwRstRiseCandMax = App.MotorPara.Current_height;//新候选起点: 重置最高点
|
|
}
|
|
if(App.MotorPara.Current_height > App.PwRstRiseCandMax)
|
|
{
|
|
App.PwRstRiseCandMax = App.MotorPara.Current_height;//跟踪候选期间最高点
|
|
}
|
|
if(App.PwRstRiseCandCnt < 300)
|
|
{
|
|
App.PwRstRiseCandCnt++;//候选持续计时(封顶300ms)
|
|
}
|
|
if(App.PwRstRiseCandCnt >= 300
|
|
&& App.MotorPara.Current_height >= App.PwRstRiseCandMax - 2)
|
|
{ //反升持续300ms且高度未回落(仍接近候选期最高点): 确认真实反弹(控制盒碰撞保护反向)
|
|
App.PwRstRiseCandCnt = 0;
|
|
App.PwRstLastHeight = App.MotorPara.Current_height;
|
|
App.PwRstRiseRef = App.MotorPara.Current_height;//参考点重置, 停顿结束恢复下降后重新判定
|
|
App.PwRstStop1mSCnt = 1000;//先停1秒, 等控制盒碰撞保护状态稳定
|
|
if(App.PwRstInchMode == true)
|
|
{ //点动中再反弹: 重置点动周期, 停1秒后继续点动
|
|
App.PwRstInchLastHeight = App.MotorPara.Current_height;//点动周期起点高度
|
|
App.PwRstInchNoMvCnt = 0;
|
|
Uart3ErrLog_PushBottom(5, (uint16_t)App.MotorPara.Current_height);//打印反弹判定事件
|
|
}
|
|
else if(App.PwRstReboundCnt == 0)
|
|
{ //首次反弹: 常规到底接触的小反弹, 停1秒恢复下降观察, 不转点动
|
|
App.PwRstReboundCnt = 1;
|
|
Uart3ErrLog_PushBottom(7, (uint16_t)App.MotorPara.Current_height);//打印首次反弹观察事件
|
|
}
|
|
else
|
|
{ //再次反弹: 确认硬触底震荡碰撞, 转点动模式(作废之前的到底确认重新确认)
|
|
App.PwRstReboundCnt = 0;
|
|
App.PwRstInchMode = true;//之后一律点动下降
|
|
App.PwRstBottomFlag = false;//之前确认的到底作废, 重新点动确认
|
|
App.PwRstInchLastHeight = App.MotorPara.Current_height;//点动周期起点高度
|
|
App.PwRstInchNoMvCnt = 0;
|
|
Uart3ErrLog_PushBottom(5, (uint16_t)App.MotorPara.Current_height);//打印反弹判定事件
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if(App.PwRstRiseCandCnt > 0)
|
|
{ //反升未持续300ms(瞬时毛刺/急停抖动回归): 取消候选, 参考点重置为当前高度
|
|
//(低毛刺帧已把参考点拉低, 不重置的话回归后的正常高度会反复进入候选)
|
|
App.PwRstRiseCandCnt = 0;
|
|
App.PwRstRiseCandMax = App.MotorPara.Current_height;
|
|
App.PwRstRiseRef = App.MotorPara.Current_height;
|
|
}
|
|
if(App.PwRstBottomFlag == true)
|
|
{ //到底已确认: 不再做到底检测, 持续下降直到收到复位帧(否则会反复触发堵转停顿)
|
|
App.PwRstNoChg1mSCnt = 0;
|
|
App.PwRstLastHeight = App.MotorPara.Current_height;
|
|
}
|
|
else if(App.PwRstInchMode == true)
|
|
{ //点动模式: 每个点动周期(1s)末尾比较该周期是否移动, 连续2周期无位移判定到底
|
|
if(App.PwRstInch1mSCnt == 999)
|
|
{
|
|
if(App.MotorPara.Current_height == App.PwRstInchLastHeight)
|
|
{
|
|
if(++App.PwRstInchNoMvCnt >= 2)
|
|
{
|
|
if(App.MotorPara.Current_height > App.PwRstMinHeight + 5)
|
|
{ //点动无位移但停在高于历史最低点: 被障碍物/碰撞保护托住, 不是到底, 继续点动试探
|
|
App.PwRstInchNoMvCnt = 0;
|
|
}
|
|
else
|
|
{ //连续2个点动周期无位移且接近历史最低点: 确认到底, 之后走正常到底复位流程
|
|
App.PwRstBottomFlag = true;
|
|
App.PwRstReboundCnt = 0;//到底已确认, 清反弹计数(后续下压反弹只停1秒继续压)
|
|
App.PwRstInchNoMvCnt = 0;
|
|
App.PwRstStop1mSCnt = 1000;//停1秒后下降触发复位
|
|
Uart3ErrLog_PushBottom(4, (uint16_t)App.MotorPara.Current_height);//打印到底确认事件
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
App.PwRstInchNoMvCnt = 0;
|
|
App.PwRstInchLastHeight = App.MotorPara.Current_height;
|
|
App.TD1mSDelayCnt = 60000;//点动脉冲有位移: 刷新上电复位超时
|
|
}
|
|
}
|
|
if(App.MotorPara.Current_height <= App.PwRstRiseRef)
|
|
{
|
|
App.PwRstRiseRef = App.MotorPara.Current_height;//点动脉冲下降时参考点跟随(只跟跌不跟涨)
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if(App.MotorPara.Current_height <= App.PwRstRiseRef)
|
|
{
|
|
App.PwRstRiseRef = App.MotorPara.Current_height;//下降/持平: 参考点跟随(只跟跌不跟涨)
|
|
}
|
|
if(App.MotorPara.Current_height != App.PwRstLastHeight)
|
|
{ //高度变化: 电机已响应下降指令, 标记已位移(此后高度稳定才可能是堵转到底)
|
|
App.PwRstMovedFlag = true;
|
|
App.PwRstNoChg1mSCnt = 0;
|
|
App.PwRstLastHeight = App.MotorPara.Current_height;
|
|
App.TD1mSDelayCnt = 60000;//高度在变(下降/反弹中): 刷新上电复位超时
|
|
}
|
|
else if(App.PwRstMovedFlag == true)
|
|
{ //高度稳定且已发生过位移: 才允许堵转到底判定
|
|
if(++App.PwRstNoChg1mSCnt >= 2000)
|
|
{
|
|
if(App.MotorPara.Current_height > App.PwRstMinHeight + 5)
|
|
{ //高度2秒无变化但停在高于历史最低点: 碰撞后控制盒把车托在障碍物上, 不是到底,
|
|
//转点动模式一点点试探下行
|
|
App.PwRstReboundCnt = 0;
|
|
App.PwRstInchMode = true;
|
|
App.PwRstInchLastHeight = App.MotorPara.Current_height;//点动周期起点高度
|
|
App.PwRstInchNoMvCnt = 0;
|
|
App.PwRstNoChg1mSCnt = 0;
|
|
App.PwRstStop1mSCnt = 1000;//先停1秒再开始点动
|
|
Uart3ErrLog_PushBottom(5, (uint16_t)App.MotorPara.Current_height);//打印碰撞托住转点动事件
|
|
}
|
|
else
|
|
{ //2秒无高度变化且接近历史最低点: 确认到底(未触发过反弹时的堵转判定)
|
|
App.PwRstBottomFlag = true;
|
|
App.PwRstReboundCnt = 0;//到底已确认, 清反弹计数(后续下压反弹只停1秒继续压)
|
|
App.PwRstNoChg1mSCnt = 0;
|
|
App.PwRstStop1mSCnt = 1000;//停1秒后下降触发复位
|
|
Uart3ErrLog_PushBottom(4, (uint16_t)App.MotorPara.Current_height);//打印到底确认事件
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{ //高度稳定但电机从未响应(上电延时后控制盒软启动中): 不累积堵转计时
|
|
App.PwRstNoChg1mSCnt = 0;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
if(App.PwRstInch1mSCnt < 1000)
|
|
App.PwRstInch1mSCnt++;
|
|
App.FeedDogDlyCnt++;
|
|
}
|
|
void FeedDogHandler(void)
|
|
{
|
|
if(App.FeedDogDlyCnt >= 200) {
|
|
FeedDog();
|
|
App.FeedDogDlyCnt = 0;
|
|
}
|
|
}
|
|
int main(void)
|
|
{
|
|
BSP_Init();
|
|
AppInit();
|
|
|
|
while(1) {
|
|
#if(USE_WDT == 1)
|
|
FeedDogHandler();
|
|
#endif
|
|
DebugLoopHandler();
|
|
Uart3ErrLogLoopHandler(); //打印控制盒报错→复位→恢复事件时间线
|
|
#if(USE_CS1237_LEFTUP == 1)
|
|
LEFTUP_CS1237DataLoopCollect();
|
|
#endif
|
|
#if(USE_CS1237_LEFTLOW == 1)
|
|
LEFTLOW_CS1237DataLoopCollect();
|
|
#endif
|
|
#if(USE_CS1237_RIGHTUP == 1)
|
|
RIGHTUP_CS1237DataLoopCollect();
|
|
#endif
|
|
#if(USE_CS1237_RIGHTLOW == 1)
|
|
RIGHTLOW_CS1237DataLoopCollect();
|
|
#endif
|
|
ReadLoopHandler();
|
|
DcmotorLoopHandler();
|
|
BeeperLoopHandler();
|
|
AdcLoopHander();
|
|
AppLoopHandler();
|
|
}
|
|
}
|
|
/*****************************************************************************************
|
|
* 函数名称: SysTick_IrqHandler
|
|
* 功能描述: 系统滴答中断
|
|
* 参 数: 无
|
|
* 返 回 值: 无
|
|
*****************************************************************************************/
|
|
void SysTick_IrqHandler(void)
|
|
{
|
|
SysTick_IncTick();
|
|
App1mSRoutine();
|
|
Dbg1msRoutine();
|
|
LEFTUP_CS12371mSRoutine();
|
|
LEFTLOW_CS12371mSRoutine();
|
|
RIGHTUP_CS12371mSRoutine();
|
|
RIGHTLOW_CS12371mSRoutine();
|
|
Beeper_ClearDelay();
|
|
Dcmotor1msRoutine();
|
|
Adc1mSRoutine();
|
|
}
|
|
|
|
/*****************************************************************************************
|
|
* 函数名称: DBG_RxPinIntCallBack
|
|
* 功能描述: DBG接收中断回调函数
|
|
* 参 数: 无
|
|
* 返 回 值: 无
|
|
*****************************************************************************************/
|
|
void DBG_RxPinIntCallBack(void)
|
|
{
|
|
//PORT_Toggle(LED_Blue_PORT, LED_Blue_PIN);
|
|
}
|
|
|
|
|
|
|
|
#if(USE_USART3 == 1)
|
|
// 定义函数Usart3RxIrqCallback，用于处理串口接收中断
|
|
void Usart3RxIrqCallback(void)
|
|
{
|
|
// 如果1分钟超时计数器为0，则清空RxLen
|
|
if(App.Uart3RxTimeOut1mSCnt == 0)
|
|
{
|
|
App.RxUart3Data.HeaderFrame = false;
|
|
App.RxUart3Data.RxLen = 0;
|
|
}
|
|
App.Uart3RxTimeOut1mSCnt = 5;
|
|
// 读取串口接收到的数据
|
|
uint8_t Data = Uart3Rec();
|
|
if(App.RxUart3Data.HeaderFrame == false)
|
|
{
|
|
if(App.RxUart3Data.RxLen == 0 && Data == 0x01)
|
|
{
|
|
App.RxUart3Data.HeaderFrame = true;
|
|
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;
|
|
// }
|
|
// 如果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] == 0x04)
|
|
{
|
|
uint8_t firstErr = (App.MotorPara.RestFlag == false) ? 0 : 1; //0:首次信息帧 1:复位中回应
|
|
App.MotorPara.RestFlag = true;
|
|
App.MotorPara.OverheatFlag = false;//收到复位帧, 过热状态解除
|
|
if(firstErr == 0)
|
|
{
|
|
App.RestStop1mSCnt = 2000;//首次收到复位帧: 启动停顿计时, 先停止电机让控制盒进入复位流程, 计时结束再下降触发复位
|
|
}
|
|
Uart3ErrLog_Push(firstErr, App.RxUart3Data.RxBuff, 0);//打印复位帧事件(0:首次触发 1:复位中回应)
|
|
}
|
|
else if(App.RxUart3Data.RxBuff[0] == 0x01 && App.RxUart3Data.RxBuff[1] == 0x01)
|
|
{
|
|
int16_t h = ((App.RxUart3Data.RxBuff[2]<<8) & 0xff00) + (App.RxUart3Data.RxBuff[3] & 0x00ff);
|
|
if(App.MotorPara.RestFlag == true)
|
|
{
|
|
Uart3ErrLog_Push(2, App.RxUart3Data.RxBuff, (uint16_t)h);//打印复位完成事件(收到高度帧)
|
|
}
|
|
App.MotorPara.RestFlag = false;
|
|
App.MotorPara.OverheatFlag = false;//收到高度帧, 确认复位完成
|
|
if(h >= 990 && h <= 1430)
|
|
{
|
|
App.MotorPara.Current_height = h;
|
|
if(App.Status == APP_STATUS_POWER_RESET && App.PwRstHtSeen == false)
|
|
{ //上电复位中首次收到有效高度帧: 高度基准就绪, 以首帧高度初始化到底检测参考值
|
|
//(此前Current_height为0, 若用入口时的0做参考, 首帧会被误判为碰撞反弹)
|
|
App.PwRstHtSeen = true;
|
|
App.PwRstLastHeight = h;
|
|
App.PwRstRiseRef = h;
|
|
App.PwRstMinHeight = h;
|
|
}
|
|
}
|
|
if(App.MotorPara.InitialFlag == true && h >= 1000 && h <= 1430)
|
|
{//缓存每个高度点对于零点应力的差值，对不同高度点的应力进行补偿
|
|
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[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];
|
|
}
|
|
}
|
|
else
|
|
{ //未知帧(帧头非01 04/01 01): 过热等其它报错可能是任意组合, 全部记录供分析
|
|
/* 报错帧(01 02 xx xx): 置OverheatFlag, HEIGHT_CONTROL跳过限位纠偏, 由SupportControlHandle判断下降触发控制盒复位
|
|
已知报错帧: 01 02 00 10(过热) 01 02 40 00(E07) 01 02 00 08(E12) 01 02 02 00(E02) */
|
|
if(App.RxUart3Data.RxBuff[0] == 0x01 && App.RxUart3Data.RxBuff[1] == 0x02)
|
|
{
|
|
uint16_t errCode = ((uint16_t)App.RxUart3Data.RxBuff[2] << 8) | App.RxUart3Data.RxBuff[3];
|
|
App.MotorPara.OverheatFlag = true;
|
|
if(App.Status == APP_STATUS_POWER_RESET && App.MotorPara.RestFlag == false
|
|
&& (App.PwRstErrValid == false || App.PwRstErrCode != errCode))
|
|
{ //上电复位中收到报错帧: 退出点动流程, 置到底标志关掉到底检测, 先停1秒再
|
|
//长按下降(持续下降)等复位帧。同一报错码连发只处理一次(沿触发): 控制盒报错
|
|
//状态会持续广播报错帧, 若每帧都重启1秒停顿则停顿被无限续期, 长按下降永远
|
|
//无法执行, 复位帧永远等不到; 不同报错码视为新报错重新停顿一次
|
|
App.PwRstErrValid = true;
|
|
App.PwRstErrCode = errCode;
|
|
App.PwRstInchMode = false;
|
|
App.PwRstBottomFlag = true;
|
|
App.PwRstInch1mSCnt = 0;
|
|
App.PwRstInchNoMvCnt = 0;
|
|
App.PwRstNoChg1mSCnt = 0;
|
|
App.PwRstStop1mSCnt = 1000;//先停1秒再继续下降触发复位
|
|
App.PwRstLastHeight = App.MotorPara.Current_height;
|
|
App.PwRstRiseRef = App.MotorPara.Current_height;//参考点重置, 长按下降中再反弹仍可检出
|
|
App.PwRstReboundCnt = 0;//清反弹计数, 长按下降中反弹只停1秒继续压不转点动
|
|
}
|
|
}
|
|
Uart3ErrLog_Push(3, App.RxUart3Data.RxBuff, 0);//打印未知帧事件(过热帧01 02 00 10, E07帧01 02 40 00等)
|
|
}
|
|
memset(App.RxUart3Data.RxBuff, 0, sizeof(App.RxUart3Data.RxBuff));
|
|
App.RxUart3Data.RxLen = 0;
|
|
App.Uart3RxTimeOut1mSCnt = 0;
|
|
App.RxUart3Data.HeaderFrame = false;
|
|
}
|
|
}
|
|
}
|
|
#endif
|
|
void RtcPeriod_IrqCallback(void)
|
|
{
|
|
//PORT_Toggle(LED_Green_PORT, LED_Green_PIN);
|
|
}
|
|
|
|
void ADC_DMA_CallBack(void)
|
|
{
|
|
|
|
}
|
|
|