diff --git a/Project/HC32F460/MDK/HC32F460.uvoptx b/Project/HC32F460/MDK/HC32F460.uvoptx
index 69ea6a3..4212ed3 100644
--- a/Project/HC32F460/MDK/HC32F460.uvoptx
+++ b/Project/HC32F460/MDK/HC32F460.uvoptx
@@ -12,7 +12,7 @@
*.lib
*.txt; *.h; *.inc; *.md
*.plm
- *.cpp
+ *.cpp; *.cc; *.cxx
0
@@ -117,21 +117,6 @@
BIN\CMSIS_AGDI.dll
-
- 0
- JL2CM3
- -U4294967295 -O78 -S2 -ZTIFSpeedSel5000 -A0 -C0 -JU1 -JI127.0.0.1 -JP0 -RST0 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -TB1 -TFE0 -FO15 -FD1FFF8000 -FC1000 -FN2 -FF0HC32F460_512K.FLM -FS00 -FL080000 -FP0($$Device:HC32F460JETA$FlashARM\HC32F460_512K.FLM) -FF1HC32F460_otp.FLM -FS13000C00 -FL13FC -FP1($$Device:HC32F460JETA$FlashARM\HC32F460_otp.FLM)
-
-
- 0
- UL2CM3
- UL2CM3(-S0 -C0 -P0 ) -FN2 -FC1000 -FD1FFF8000 -FF0HC32F460_512K -FF1HC32F460_otp -FL080000 -FL13FC -FS00 -FS13000C00 -FP0($$Device:HC32F460JETA$FlashARM\HC32F460_512K.FLM) -FP1($$Device:HC32F460JETA$FlashARM\HC32F460_otp.FLM)
-
-
- 0
- CMSIS_AGDI
- -X"Any" -UAny -O206 -S9 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD1FFF8000 -FC1000 -FN2 -FF0HC32F460_512K.FLM -FS00 -FL080000 -FP0($$Device:HC32F460JETA$FlashARM\HC32F460_512K.FLM) -FF1HC32F460_otp.FLM -FS13000C00 -FL13FC -FP1($$Device:HC32F460JETA$FlashARM\HC32F460_otp.FLM)
-
0
ARMRTXEVENTFLAGS
@@ -152,45 +137,18 @@
DLGUARM
-
-<<<<<<< HEAD
-
-=======
-
-
+
0
- 0
- 291
- 1
- 4146
- 0
- 0
- 0
- 0
- 0
- 1
- ..\source\Module\dc_motor\dc_motor.c
-
- \\BombSensor\../source/Module/dc_motor/dc_motor.c\291
-
-
- 1
- 0
- 343
- 1
- 0
- 0
- 0
- 0
- 0
- 0
- 0
- ..\source\Module\dc_motor\dc_motor.c
-
-
-
-
->>>>>>> Motor_Test
+ CMSIS_AGDI
+ -X"Any" -UAny -O206 -S0 -C0 -P00000000 -N00("") -D00(00000000) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD1FFF8000 -FC1000 -FN2 -FF0HC32F460_512K.FLM -FS00 -FL080000 -FP0($$Device:HC32F460JETA$FlashARM\HC32F460_512K.FLM) -FF1HC32F460_otp.FLM -FS13000C00 -FL13FC -FP1($$Device:HC32F460JETA$FlashARM\HC32F460_otp.FLM)
+
+
+ 0
+ UL2CM3
+ 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
@@ -210,83 +168,61 @@
3
1
- App.LeftUpCS1237_SlopeArray[0],0x0A
+ App.EmeFlag
4
1
- App.LeftLowCS1237_SlopeArray[0],0x0A
+ App.ADTrack.LeftUp_Org_AdZero,0x0A
5
1
- App.RightUpCS1237_SlopeArray[0],0x0A
+ App.ADTrack.LeftLow_Org_AdZero,0x0A
6
1
- App.RightLowCS1237_SlopeArray[0],0x0A
+ App.ADTrack.RightUp_Org_AdZero,0x0A
7
1
- App.ADTrack.LeftUp_Org_AdZero,0x0A
+ App.ADTrack.RightLow_Org_AdZero,0x0A
8
1
- App.ADTrack.LeftLow_Org_AdZero,0x0A
+ App.LeftRunFalg
9
1
- App.ADTrack.RightUp_Org_AdZero,0x0A
+ App.RightRunFalg
10
1
- App.ADTrack.RightLow_Org_AdZero,0x0A
+ App.CentreRunFalg
11
1
- App.LeftRunFalg,0x0A
+ LeftLowDiff,0x0A
12
1
- App.RightRunFalg,0x0A
-
-
- 13
- 1
- App.CentreRunFalg,0x0A
-
-
- 18
- 1
- g_motor_data
+ RightLowDiff,0x0A
-
-
- 1
- 0
- 0x1FFFA910
- 0
-
-
0
0
1
-<<<<<<< HEAD
1
-=======
- 0
->>>>>>> Motor_Test
0
0
0
@@ -324,10 +260,10 @@
1
- 0
+ 1
0
2
- 5000000
+ 1000000
@@ -427,72 +363,7 @@
-
-
- 0
- 0
- 163
- 1
- 0
- 0
- 0
- 0
- 0
- 0
- 0
- ..\source\User\Src\main.c
-
-
-
-
- 1
- 0
- 185
- 1
- 0
- 0
- 0
- 0
- 0
- 0
- 0
- ..\source\User\Src\main.c
-
-
-
-
- 2
- 0
- 211
- 1
- 0
- 0
- 0
- 0
- 0
- 0
- 0
- ..\source\User\Src\main.c
-
-
-
-
- 3
- 0
- 235
- 1
- 0
- 0
- 0
- 0
- 0
- 0
- 0
- ..\source\User\Src\main.c
-
-
-
-
+
0
@@ -1152,7 +1023,7 @@
CS1237
- 1
+ 0
0
0
0
@@ -1208,7 +1079,7 @@
dc_motor
- 1
+ 0
0
0
0
diff --git a/Project/HC32F460/source/Module/dc_motor/dc_motor.h b/Project/HC32F460/source/Module/dc_motor/dc_motor.h
index 8125ac2..172d40b 100644
--- a/Project/HC32F460/source/Module/dc_motor/dc_motor.h
+++ b/Project/HC32F460/source/Module/dc_motor/dc_motor.h
@@ -28,11 +28,7 @@
#define MAX_SPEED 480 // 最大速度(PWM占空比)
#define ACCEL_STEP 10 // 加速步进值
#define DECEL_STEP 10 // 减速步进值
-<<<<<<< HEAD
#define ACCEL_INTERVAL 10 // 加减速间隔(ms)
-=======
-#define ACCEL_INTERVAL 2 // 加减速间隔(ms)
->>>>>>> Motor_Test
diff --git a/Project/HC32F460/source/User/Inc/main.h b/Project/HC32F460/source/User/Inc/main.h
index 82d5c43..cb6d967 100644
--- a/Project/HC32F460/source/User/Inc/main.h
+++ b/Project/HC32F460/source/User/Inc/main.h
@@ -5,7 +5,7 @@
#define USE_FILE_SYSTEM 1
-#define SDCARD_BLOCK_MAX (10000)
+#define SDCARD_BLOCK_MAX (10000)
#define OSC_CNT 40
#define SLOPE_CNT 30
@@ -13,15 +13,18 @@
#define SLOPE_VPT 1000
#define FOLLOW_VPT 20
#define SHIELD_VPT 2500
+#define EME_STOP_VPT 10000
-#define STOP_TIME_MS 0
+#define STOP_TIME_MS 500
#define RUN_TIME_MS 0
+#define EME_START_MS 500
+#define EME_STOP_MS 3000
#define TRAVEL_TIME_MS 30000
#define BDC_STOP() (SET_TWIN_BIT0(),SET_TWIN_BIT1())
-#define BDC_UP() (SET_TWIN_BIT0(),CLR_TWIN_BIT1())
+#define BDC_UP() (SET_TWIN_BIT0(),CLR_TWIN_BIT1())
#define BDC_DOWN() (SET_TWIN_BIT1(),CLR_TWIN_BIT0())
/*控制等级*/
@@ -99,13 +102,13 @@ typedef struct {
uint32_t Uart1Delay1mSCnt;
uint32_t StopCollDelay1mSCnt;
uint32_t RunCollDelay1mSCnt;
- uint32_t EvenRunDelay1mSCnt;
+ uint32_t EmergencyDelay1mSCnt;
uint8_t FeedDogDlyCnt;
bool UpFlag;//上升标志位
bool DownFlag;//下降标志位
bool StopFlag;//停止标志位
- bool EvenRunning;//平稳运行标志位
+ bool EmeFlag;//应急标志位
bool LeftRunFalg;
bool RightRunFalg;
bool CentreRunFalg;
diff --git a/Project/HC32F460/source/User/Src/main.c b/Project/HC32F460/source/User/Src/main.c
index 1919926..9d55dd2 100644
--- a/Project/HC32F460/source/User/Src/main.c
+++ b/Project/HC32F460/source/User/Src/main.c
@@ -203,184 +203,221 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
PeakValleyCheck ru_K = check_peaks_valleys(App.RightUpCS1237_SlopeArray, SLOPE_CNT);
PeakValleyCheck rl_K = check_peaks_valleys(App.RightLowCS1237_SlopeArray, SLOPE_CNT);
- if(App.StopFlag == true)
+ int32_t LeftSlopeSum = abs(App.LeftUpCS1237_SlopeArray[0]) + abs(App.LeftLowCS1237_SlopeArray[0]);
+ int32_t RightSlopeSum = abs(App.RightUpCS1237_SlopeArray[0]) + abs(App.RightLowCS1237_SlopeArray[0]);
+ int32_t LeftLowDiff = abs(Oll_diff1);
+ int32_t RightLowDiff = abs(Orl_diff1);
+ int32_t LeftDiffSum = abs(lu_diff[0]);
+ int32_t RightDiffSum = abs(ru_diff[0]);
+
+ if(App.EmeFlag == true)
{
- if(App.StopCollDelay1mSCnt <= 0)
+ if(App.DownFlag == true && App.EmergencyDelay1mSCnt > 0)
{
- uint32_t LeftSlopeSum = abs(App.LeftUpCS1237_SlopeArray[0]) + abs(App.LeftLowCS1237_SlopeArray[0]);
- uint32_t RightSlopeSum = abs(App.RightUpCS1237_SlopeArray[0]) + abs(App.RightLowCS1237_SlopeArray[0]);
-// uint32_t LeftDiffSum = abs(lu_diff[0]) + abs(ll_diff[0]);
-// uint32_t RightDiffSum = abs(ru_diff[0]) + abs(rl_diff[0]);
- uint32_t LeftDiffSum = abs(ll_diff[0]);
- uint32_t RightDiffSum = abs(rl_diff[0]);
-
- App.LeftRunFalg = false;
- App.RightRunFalg = false;
- App.CentreRunFalg = false;
- if(((lu_ad[0] < App.ADTrack.LeftUp_Org_AdZero-1000)
- && (ll_ad[0] > App.ADTrack.LeftLow_Org_AdZero+1000)
- && (ru_ad[0] < App.ADTrack.RightUp_Org_AdZero-1000)
- && (rl_ad[0] > App.ADTrack.RightLow_Org_AdZero+1000))
- || ((lu_ad[0] > App.ADTrack.LeftUp_Org_AdZero+1000)
- && (ll_ad[0] < App.ADTrack.LeftLow_Org_AdZero-1000)
- && (ru_ad[0] > App.ADTrack.RightUp_Org_AdZero+1000)
- && (rl_ad[0] < App.ADTrack.RightLow_Org_AdZero-1000)))
- {
- App.CentreRunFalg = true;
- if(abs(Oll_diff1) > abs(Orl_diff1))
- {
- App.LeftRunFalg = true;
- if((App.LeftLowCS1237_SlopeArray[0] > App.ADTrack.Slope_VPT/3))
- {
- if(Oll_diff1 < -App.ADTrack.LeftLowVPT/2)
- {
- BDC_UP();
- App.UpFlag = true;
- App.StopFlag = false;
- App.DownFlag = false;
- App.RunCollDelay1mSCnt = RUN_TIME_MS;
- }
- }
- else if((App.LeftLowCS1237_SlopeArray[0] < -App.ADTrack.Slope_VPT/3))
- {
- if(Oll_diff1 > App.ADTrack.LeftLowVPT/2)
- {
- BDC_DOWN();
- App.UpFlag = false;
- App.StopFlag = false;
- App.DownFlag = true;
- App.RunCollDelay1mSCnt = RUN_TIME_MS;
- }
- }
- }
- else if(abs(Oll_diff1) < abs(Orl_diff1))
- {
- App.RightRunFalg = true;
- if((App.RightLowCS1237_SlopeArray[0] > App.ADTrack.Slope_VPT/3))
- {
- if(Orl_diff1 < -App.ADTrack.RightLowVPT/2)
- {
- BDC_UP();
- App.UpFlag = true;
- App.StopFlag = false;
- App.DownFlag = false;
- App.RunCollDelay1mSCnt = RUN_TIME_MS;
- }
- }
- else if((App.RightLowCS1237_SlopeArray[0] < -App.ADTrack.Slope_VPT/3))
- {
- if(Orl_diff1 > App.ADTrack.RightLowVPT/2)
- {
- BDC_DOWN();
- App.UpFlag = false;
- App.StopFlag = false;
- App.DownFlag = true;
- App.RunCollDelay1mSCnt = RUN_TIME_MS;
- }
- }
- }
- }
- else if((LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum))
+ 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;
+ }
+ }
+ 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-1000)
+ && (ll_ad[0] > App.ADTrack.LeftLow_Org_AdZero+1000)
+ && (ru_ad[0] < App.ADTrack.RightUp_Org_AdZero-1000)
+ && (rl_ad[0] > App.ADTrack.RightLow_Org_AdZero+1000))
+ || ((lu_ad[0] > App.ADTrack.LeftUp_Org_AdZero+1000)
+ && (ll_ad[0] < App.ADTrack.LeftLow_Org_AdZero-1000)
+ && (ru_ad[0] > App.ADTrack.RightUp_Org_AdZero+1000)
+ && (rl_ad[0] < App.ADTrack.RightLow_Org_AdZero-1000)))
+ {
+ App.CentreRunFalg = true;
+ if(abs(Oll_diff1) > abs(Orl_diff1))
{
App.LeftRunFalg = true;
- if(((App.LeftUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT)
- && (App.LeftUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT)
- && (App.LeftUpCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)
- && (App.LeftLowCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
- && (App.LeftLowCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT)
- && (App.LeftLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT))
- || (
- (Olu_diff1 >= App.ADTrack.LeftUpVPT)
- && (Olu_diff2 >= App.ADTrack.LeftUpVPT)
- && (Olu_diff3 >= App.ADTrack.LeftUpVPT)
- && (Oll_diff1 <= -App.ADTrack.LeftLowVPT)
- && (Oll_diff2 <= -App.ADTrack.LeftLowVPT)
- && (Oll_diff3 <= -App.ADTrack.LeftLowVPT)))
- {
- if((abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT)
- && (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT)
- )
+// if((App.LeftLowCS1237_SlopeArray[0] > App.ADTrack.Slope_VPT/3))
+// {
+ if(Oll_diff1 < -App.ADTrack.LeftLowVPT/3)
{
BDC_UP();
App.UpFlag = true;
App.StopFlag = false;
App.DownFlag = false;
+ App.EmeFlag = false;
+ App.RunCollDelay1mSCnt = RUN_TIME_MS;
+ App.EmergencyDelay1mSCnt = EME_START_MS;
}
- }
- else if(((App.LeftUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
- && (App.LeftUpCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT)
- && (App.LeftUpCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)
- && (App.LeftLowCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT)
- && (App.LeftLowCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT)
- && (App.LeftLowCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT))
- || (
- (Olu_diff1 <= -App.ADTrack.LeftUpVPT)
- && (Olu_diff1 <= -App.ADTrack.LeftUpVPT)
- && (Olu_diff1 <= -App.ADTrack.LeftUpVPT)
- && (Oll_diff1 >= App.ADTrack.LeftLowVPT)
- && (Oll_diff1 >= App.ADTrack.LeftLowVPT)
- && (Oll_diff1 >= App.ADTrack.LeftLowVPT)))
- {
- if((abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT)
- && (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT)
- )
+// }
+// else if((App.LeftLowCS1237_SlopeArray[0] < -App.ADTrack.Slope_VPT/3))
+// {
+ if(Oll_diff1 > App.ADTrack.LeftLowVPT/3)
{
BDC_DOWN();
App.UpFlag = false;
- App.DownFlag = true;
App.StopFlag = false;
+ App.DownFlag = true;
+ App.EmeFlag = false;
+ App.RunCollDelay1mSCnt = RUN_TIME_MS;
+ App.EmergencyDelay1mSCnt = EME_START_MS;
}
- }
+// }
}
- else if(LeftSlopeSum < RightSlopeSum && LeftDiffSum < RightDiffSum)
+ else if(abs(Oll_diff1) < abs(Orl_diff1))
{
App.RightRunFalg = true;
- if(((App.RightUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT)
- && (App.RightUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT)
- && (App.RightUpCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)
- && (App.RightLowCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
- && (App.RightLowCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT)
- && (App.RightLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT))
- || ((Oru_diff1 >= App.ADTrack.RightUpVPT)
- && (Oru_diff2 >= App.ADTrack.RightUpVPT)
- && (Oru_diff3 >= App.ADTrack.RightUpVPT)
- && (Orl_diff1 <= -App.ADTrack.RightLowVPT)
- && (Orl_diff2 <= -App.ADTrack.RightLowVPT)
- && (Orl_diff3 <= -App.ADTrack.RightLowVPT)))
+ if((App.RightLowCS1237_SlopeArray[0] > App.ADTrack.Slope_VPT/3))
{
- if((abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT)
- && (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT)
- )
+ if(Orl_diff1 < -App.ADTrack.RightLowVPT/3)
{
BDC_UP();
App.UpFlag = true;
App.StopFlag = false;
App.DownFlag = false;
+ App.EmeFlag = false;
+ App.RunCollDelay1mSCnt = RUN_TIME_MS;
+ App.EmergencyDelay1mSCnt = EME_START_MS;
}
}
- else if(((App.RightUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
- && (App.RightUpCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT)
- && (App.RightUpCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)
- && (App.RightLowCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT)
- && (App.RightLowCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT)
- && (App.RightLowCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT))
- || ((Oru_diff1 <= -App.ADTrack.RightUpVPT)
- && (Oru_diff2 <= -App.ADTrack.RightUpVPT)
- && (Oru_diff3 <= -App.ADTrack.RightUpVPT)
- && (Orl_diff1 >= App.ADTrack.RightLowVPT)
- && (Orl_diff2 >= App.ADTrack.RightLowVPT)
- && (Orl_diff3 >= App.ADTrack.RightLowVPT))
+ else if((App.RightLowCS1237_SlopeArray[0] < -App.ADTrack.Slope_VPT/3))
+ {
+ if(Orl_diff1 > App.ADTrack.RightLowVPT/3)
+ {
+ BDC_DOWN();
+ App.UpFlag = false;
+ App.StopFlag = false;
+ App.DownFlag = true;
+ App.EmeFlag = false;
+ App.RunCollDelay1mSCnt = RUN_TIME_MS;
+ App.EmergencyDelay1mSCnt = EME_START_MS;
+ }
+ }
+ }
+ }
+ else if((LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum))
+ {
+ App.LeftRunFalg = true;
+ if(((App.LeftUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT)
+ && (App.LeftUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT)
+ && (App.LeftUpCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)
+ && (App.LeftLowCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
+ && (App.LeftLowCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT)
+ && (App.LeftLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT))
+ || (
+ (Olu_diff1 >= App.ADTrack.LeftUpVPT)
+ && (Olu_diff2 >= App.ADTrack.LeftUpVPT)
+ && (Olu_diff3 >= App.ADTrack.LeftUpVPT)
+ && (Oll_diff1 <= -App.ADTrack.LeftLowVPT)
+ && (Oll_diff2 <= -App.ADTrack.LeftLowVPT)
+ && (Oll_diff3 <= -App.ADTrack.LeftLowVPT)))
+ {
+ if((abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT)
+ // && (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT)
)
{
- if((abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT)
- && (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT)
- )
- {
- BDC_DOWN();
- App.DownFlag = true;
- App.StopFlag = false;
- App.UpFlag = false;
- }
+ BDC_UP();
+ App.UpFlag = true;
+ App.StopFlag = false;
+ App.DownFlag = false;
+ App.EmeFlag = false;
+ App.RunCollDelay1mSCnt = RUN_TIME_MS;
+ App.EmergencyDelay1mSCnt = EME_START_MS;
+ }
+ }
+ else if(((App.LeftUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
+ && (App.LeftUpCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT)
+ && (App.LeftUpCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)
+ && (App.LeftLowCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT)
+ && (App.LeftLowCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT)
+ && (App.LeftLowCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT))
+ || (
+ (Olu_diff1 <= -App.ADTrack.LeftUpVPT)
+ && (Olu_diff1 <= -App.ADTrack.LeftUpVPT)
+ && (Olu_diff1 <= -App.ADTrack.LeftUpVPT)
+ && (Oll_diff1 >= App.ADTrack.LeftLowVPT)
+ && (Oll_diff1 >= App.ADTrack.LeftLowVPT)
+ && (Oll_diff1 >= App.ADTrack.LeftLowVPT)))
+ {
+ if((abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT)
+ // && (abs(Olu_diff1) > abs(Orl_diff1) + App.ADTrack.LeftUpShieldVPT)
+ )
+ {
+ BDC_DOWN();
+ App.UpFlag = false;
+ App.DownFlag = true;
+ App.StopFlag = false;
+ App.EmeFlag = false;
+ App.RunCollDelay1mSCnt = RUN_TIME_MS;
+ App.EmergencyDelay1mSCnt = EME_START_MS;
+ }
+ }
+ }
+ else if(LeftSlopeSum < RightSlopeSum && LeftDiffSum < RightDiffSum)
+ {
+ App.RightRunFalg = true;
+ if(((App.RightUpCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT)
+ && (App.RightUpCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT)
+ && (App.RightUpCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)
+ && (App.RightLowCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
+ && (App.RightLowCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT)
+ && (App.RightLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT))
+ || ((Oru_diff1 >= App.ADTrack.RightUpVPT)
+ && (Oru_diff2 >= App.ADTrack.RightUpVPT)
+ && (Oru_diff3 >= App.ADTrack.RightUpVPT)
+ && (Orl_diff1 <= -App.ADTrack.RightLowVPT)
+ && (Orl_diff2 <= -App.ADTrack.RightLowVPT)
+ && (Orl_diff3 <= -App.ADTrack.RightLowVPT)))
+ {
+ if((abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT)
+ && (abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT)
+ )
+ {
+ BDC_UP();
+ App.UpFlag = true;
+ App.StopFlag = false;
+ App.DownFlag = false;
+ App.EmeFlag = false;
+ App.RunCollDelay1mSCnt = RUN_TIME_MS;
+ App.EmergencyDelay1mSCnt = EME_START_MS;
+ }
+ }
+ else if(((App.RightUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)
+ && (App.RightUpCS1237_SlopeArray[1] >= App.ADTrack.Slope_VPT)
+ && (App.RightUpCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)
+ && (App.RightLowCS1237_SlopeArray[0] <= -App.ADTrack.Slope_VPT)
+ && (App.RightLowCS1237_SlopeArray[1] <= -App.ADTrack.Slope_VPT)
+ && (App.RightLowCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT))
+ || ((Oru_diff1 <= -App.ADTrack.RightUpVPT)
+ && (Oru_diff2 <= -App.ADTrack.RightUpVPT)
+ && (Oru_diff3 <= -App.ADTrack.RightUpVPT)
+ && (Orl_diff1 >= App.ADTrack.RightLowVPT)
+ && (Orl_diff2 >= App.ADTrack.RightLowVPT)
+ && (Orl_diff3 >= App.ADTrack.RightLowVPT))
+ )
+ {
+ if((abs(Oru_diff1) > abs(Oll_diff1) + App.ADTrack.RightUpShieldVPT)
+ && (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT)
+ )
+ {
+ BDC_DOWN();
+ App.DownFlag = true;
+ App.StopFlag = false;
+ App.UpFlag = false;
+ App.EmeFlag = false;
+ App.RunCollDelay1mSCnt = RUN_TIME_MS;
+ App.EmergencyDelay1mSCnt = EME_START_MS;
}
}
}
@@ -396,6 +433,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
+ App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
else if(((App.CentreRunFalg == false ) && (Oll_diff1 < App.ADTrack.Stop_VPT))
@@ -405,8 +443,15 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
+ App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
+ else if((LeftLowDiff + EME_STOP_VPT) < RightLowDiff && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
+ {
+ BDC_STOP();
+ App.EmeFlag = true;
+ App.EmergencyDelay1mSCnt = EME_STOP_MS;
+ }
}
else if(App.RightRunFalg == true)
{
@@ -417,6 +462,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
+ App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
else if(((App.CentreRunFalg == false ) && (Orl_diff1 < App.ADTrack.Stop_VPT))
@@ -426,8 +472,15 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
+ App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
+ else if(LeftLowDiff > (RightLowDiff + 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)
@@ -441,6 +494,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
+ App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
else if(((App.CentreRunFalg == false ) && (Oll_diff1 > -App.ADTrack.Stop_VPT))
@@ -450,8 +504,15 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
+ App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
+ else if((LeftLowDiff + EME_STOP_VPT) < RightLowDiff && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
+ {
+ BDC_STOP();
+ App.EmeFlag = true;
+ App.EmergencyDelay1mSCnt = EME_STOP_MS;
+ }
}
else if(App.RightRunFalg == true)
{
@@ -462,6 +523,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
+ App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
else if(((App.CentreRunFalg == false ) && (Orl_diff1 > -App.ADTrack.Stop_VPT))
@@ -471,8 +533,15 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.StopFlag = true;
App.DownFlag = false;
App.UpFlag = false;
+ App.EmeFlag = false;
App.StopCollDelay1mSCnt = STOP_TIME_MS;
}
+ else if(LeftLowDiff > (RightLowDiff + EME_STOP_VPT) && App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
+ {
+ BDC_STOP();
+ App.EmeFlag = true;
+ App.EmergencyDelay1mSCnt = EME_STOP_MS;
+ }
}
}
@@ -515,25 +584,6 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
App.ADTrack.RightUp_Org_AdZero = ru_ad[0];
App.ADTrack.RightLow_Org_AdZero = rl_ad[0];
}
- else if(App.StopFlag == false)
- {
- App.EvenRunning = true;
- App.EvenRunDelay1mSCnt = 0;
- }
- }
- else
- {
- App.EvenRunning = false;
- App.EvenRunDelay1mSCnt = 0;
- }
- if(App.EvenRunDelay1mSCnt >= 2000)
- {
- App.EvenRunning = false;
- App.EvenRunDelay1mSCnt = 0;
- 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];
}
}
#endif
@@ -767,8 +817,8 @@ static void AppLoopHandler(void)
App.WaitFlag = true;
App.UpFlag = false;
App.DownFlag = false;
+ App.EmeFlag = false;
App.StopFlag = true;
- App.EvenRunning = false;
App.ADTrack.LeftUp_Org_AdZero = App.LeftUpCS1237_TrendArray[0];
App.ADTrack.LeftLow_Org_AdZero = App.LeftLowCS1237_TrendArray[0];
@@ -798,9 +848,8 @@ static void App1mSRoutine(void)
App.StopCollDelay1mSCnt--;
if(App.RunCollDelay1mSCnt > 0)
App.RunCollDelay1mSCnt--;
- if(App.EvenRunning == true)
- App.EvenRunDelay1mSCnt++;
-
+ if(App.EmergencyDelay1mSCnt > 0)
+ App.EmergencyDelay1mSCnt--;
App.FeedDogDlyCnt++;
}
void FeedDogHandler(void)
@@ -854,7 +903,7 @@ void SysTick_IrqHandler(void)
RIGHTUP_CS12371mSRoutine();
RIGHTLOW_CS12371mSRoutine();
Dcmotor1msRoutine();
- Adc1mSRoutine();
+ Adc1mSRoutine();
}
/*****************************************************************************************