YuanHongbin/Acc_Support_29V_HV2.x
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

双柱双电机S型传感器基本功能完成

Browse Source
This commit is contained in:
DESKTOP-KQHDCM4\97555
2025-10-14 17:44:33 +08:00
parent 1de51f1380
commit 92bb9bf934
4 changed files with 106 additions and 136 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Project/HC32F460/source/User/Inc/main.h
+8 -2
View File
@@ -10,12 +10,16 @@
#define OSC_CNT 40
#define SLOPE_CNT 30
#define SLOPE_VPT 1000
#define FOLLOW_VPT 20
#define SHIELD_VPT 5000
#define STOP_TIME_MS 500
#define RUN_TIME_MS 0
#define BDC_STOP() (SET_TWIN_BIT0(),SET_TWIN_BIT1())
#define BDC_UP() (CLR_TWIN_BIT0(),SET_TWIN_BIT1())
#define BDC_DOWN() (SET_TWIN_BIT0(),CLR_TWIN_BIT1())
#define BDC_UP() (SET_TWIN_BIT0(),CLR_TWIN_BIT1())
#define BDC_DOWN() (CLR_TWIN_BIT0(),SET_TWIN_BIT1())
/*控制等级*/
typedef enum {
@@ -66,6 +70,8 @@ typedef enum {
APP_STATUS_IDLE,
APP_STATUS_ON,
APP_STATUS_OFF,
APP_STATUS_UP_2S,
APP_STATUS_DOWN_2S,
APP_STATUS_WAIT,
}AppStatus_m;
Project/HC32F460/source/User/Src/main.c
+85 -65
View File
@@ -78,63 +78,63 @@ void GradeControl(ControlLevel_m level)
{
switch(level){
case LEVEL_1:{
App.ADTrack.Slope_VPT = 1000;
App.ADTrack.Slope_VPT = SLOPE_VPT;
App.ADTrack.LeftUpVPT = 10000;
App.ADTrack.LeftLowVPT = 10000;
App.ADTrack.RightUpVPT = 10000;
App.ADTrack.RightLowVPT = 10000;
App.ADTrack.LeftUpShieldVPT = 5000;
App.ADTrack.LeftLowShieldVPT = 5000;
App.ADTrack.RightUpShieldVPT = 5000;
App.ADTrack.RightLowShieldVPT = 5000;
App.ADTrack.LeftUpShieldVPT = SHIELD_VPT;
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
break;}
case LEVEL_2:{
App.ADTrack.Slope_VPT = 1000;
App.ADTrack.Slope_VPT = SLOPE_VPT;
App.ADTrack.LeftUpVPT = 20000;
App.ADTrack.LeftLowVPT = 20000;
App.ADTrack.RightUpVPT = 20000;
App.ADTrack.RightLowVPT = 20000;
App.ADTrack.LeftUpShieldVPT = 5000;
App.ADTrack.LeftLowShieldVPT = 5000;
App.ADTrack.RightUpShieldVPT = 5000;
App.ADTrack.RightLowShieldVPT = 5000;
App.ADTrack.LeftUpShieldVPT = SHIELD_VPT;
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
break;}
case LEVEL_3:{
App.ADTrack.Slope_VPT = 1000;
App.ADTrack.Slope_VPT = SLOPE_VPT;
App.ADTrack.LeftUpVPT = 30000;
App.ADTrack.LeftLowVPT = 30000;
App.ADTrack.RightUpVPT = 30000;
App.ADTrack.RightLowVPT = 30000;
App.ADTrack.LeftUpShieldVPT = 5000;
App.ADTrack.LeftLowShieldVPT = 5000;
App.ADTrack.RightUpShieldVPT = 5000;
App.ADTrack.RightLowShieldVPT = 5000;
App.ADTrack.LeftUpShieldVPT = SHIELD_VPT;
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
break;}
case LEVEL_4:{
App.ADTrack.Slope_VPT = 1000;
App.ADTrack.Slope_VPT = SLOPE_VPT;
App.ADTrack.LeftUpVPT = 40000;
App.ADTrack.LeftLowVPT = 40000;
App.ADTrack.RightUpVPT = 40000;
App.ADTrack.RightLowVPT = 40000;
App.ADTrack.LeftUpShieldVPT = 5000;
App.ADTrack.LeftLowShieldVPT = 5000;
App.ADTrack.RightUpShieldVPT = 5000;
App.ADTrack.RightLowShieldVPT = 5000;
App.ADTrack.LeftUpShieldVPT = SHIELD_VPT;
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
break;}
case LEVEL_5:{
App.ADTrack.Slope_VPT = 1000;
App.ADTrack.Slope_VPT = SLOPE_VPT;
App.ADTrack.LeftUpVPT = 50000;
App.ADTrack.LeftLowVPT = 50000;
App.ADTrack.RightUpVPT = 50000;
App.ADTrack.RightLowVPT = 50000;
App.ADTrack.LeftUpShieldVPT = 5000;
App.ADTrack.LeftLowShieldVPT = 5000;
App.ADTrack.RightUpShieldVPT = 5000;
App.ADTrack.RightLowShieldVPT = 5000;
App.ADTrack.LeftUpShieldVPT = SHIELD_VPT;
App.ADTrack.LeftLowShieldVPT = SHIELD_VPT;
App.ADTrack.RightUpShieldVPT = SHIELD_VPT;
App.ADTrack.RightLowShieldVPT = SHIELD_VPT;
break;}
}
}
@@ -228,10 +228,10 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
if((abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT)
&& (abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT))
{
BDC_DOWN();
App.UpFlag = false;
BDC_UP();
App.UpFlag = true;
App.StopFlag = false;
App.DownFlag = true;
App.DownFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
}
}
@@ -251,9 +251,9 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
if((abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT)
&& (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT))
{
BDC_UP();
App.UpFlag = true;
App.DownFlag = false;
BDC_DOWN();
App.UpFlag = false;
App.DownFlag = true;
App.StopFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
}
@@ -278,10 +278,10 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
if((abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT)
&& (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT))
{
BDC_DOWN();
App.UpFlag = false;
BDC_UP();
App.UpFlag = true;
App.StopFlag = false;
App.DownFlag = true;
App.DownFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
}
}
@@ -302,17 +302,17 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
if((abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT)
&& (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT))
{
BDC_UP();
App.DownFlag = false;
BDC_DOWN();
App.DownFlag = true;
App.StopFlag = false;
App.UpFlag = true;
App.UpFlag = false;
App.RunCollDelay1mSCnt = RUN_TIME_MS;
}
}
}
}
}
else if(App.UpFlag == true)
else if(App.DownFlag == true)
{
if(App.LeftRunFalg == true)
{
@@ -359,7 +359,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
}
}
}
else if(App.DownFlag == true)
else if(App.UpFlag == true)
{
if(App.LeftRunFalg == true)
{
@@ -410,36 +410,36 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
if (lu_K.peaks_positive
&& lu_K.valleys_negative
&& App.LeftUpCS1237_SlopeArray[0] < App.ADTrack.Slope_VPT / 2
&& App.LeftUpCS1237_SlopeArray[0] > -App.ADTrack.Slope_VPT / 2
&& App.LeftUpCS1237_SlopeArray[1] < App.ADTrack.Slope_VPT / 2
&& App.LeftUpCS1237_SlopeArray[1] > -App.ADTrack.Slope_VPT / 2
&& App.LeftUpCS1237_SlopeArray[2] < App.ADTrack.Slope_VPT / 2
&& App.LeftUpCS1237_SlopeArray[2] > -App.ADTrack.Slope_VPT / 2
&& 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] < App.ADTrack.Slope_VPT / 2
&& App.LeftLowCS1237_SlopeArray[0] > -App.ADTrack.Slope_VPT / 2
&& App.LeftLowCS1237_SlopeArray[1] < App.ADTrack.Slope_VPT / 2
&& App.LeftLowCS1237_SlopeArray[1] > -App.ADTrack.Slope_VPT / 2
&& App.LeftLowCS1237_SlopeArray[2] < App.ADTrack.Slope_VPT / 2
&& App.LeftLowCS1237_SlopeArray[2] > -App.ADTrack.Slope_VPT / 2
&& 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] < App.ADTrack.Slope_VPT / 2
&& App.RightUpCS1237_SlopeArray[0] > -App.ADTrack.Slope_VPT / 2
&& App.RightUpCS1237_SlopeArray[1] < App.ADTrack.Slope_VPT / 2
&& App.RightUpCS1237_SlopeArray[1] > -App.ADTrack.Slope_VPT / 2
&& App.RightUpCS1237_SlopeArray[2] < App.ADTrack.Slope_VPT / 2
&& App.RightUpCS1237_SlopeArray[2] > -App.ADTrack.Slope_VPT / 2
&& 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] < App.ADTrack.Slope_VPT / 2
&& App.RightLowCS1237_SlopeArray[0] > -App.ADTrack.Slope_VPT / 2
&& App.RightLowCS1237_SlopeArray[1] < App.ADTrack.Slope_VPT / 2
&& App.RightLowCS1237_SlopeArray[1] > -App.ADTrack.Slope_VPT / 2
&& App.RightLowCS1237_SlopeArray[2] < App.ADTrack.Slope_VPT / 2
&& App.RightLowCS1237_SlopeArray[2] > -App.ADTrack.Slope_VPT / 2)
&& 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.ADTrack.LeftUp_Org_AdZero = lu_ad[0];
@@ -500,8 +500,8 @@ static void AppInit(void)
RIGHTUP_CS1237_Init();
RIGHTLOW_CS1237_Init();
App.Status = APP_STATUS_WAIT;
App.TD1mSDelayCnt = 2000;
App.Status = APP_STATUS_DOWN_2S;
App.TD1mSDelayCnt = 10000;
App.RStatus = READ_STATUS_START;
GradeControl(App.Para.Layout.ControlLevel);//等级控制
@@ -620,6 +620,26 @@ static void AppLoopHandler(void)
App.Status = APP_STATUS_IDLE;
break;}
case APP_STATUS_UP_2S:{
BDC_UP();
if(App.TD1mSDelayCnt <= 0)
{
App.TD1mSDelayCnt = 2000;
App.Status = APP_STATUS_DOWN_2S;
BDC_STOP();
}
break;}
case APP_STATUS_DOWN_2S:{
BDC_DOWN();
if(App.TD1mSDelayCnt <= 0)
{
App.TD1mSDelayCnt = 2000;
App.Status = APP_STATUS_WAIT;
BDC_STOP();
}
break;}
case APP_STATUS_WAIT:{
if(App.TD1mSDelayCnt <= 0)
{