btl143/QDAC-firmware
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: 增加避障状态计时清除

Browse Source
This commit is contained in:
井明江
2024-03-24 18:26:36 +08:00
parent 1c65b7752e
commit cbea30033a
5 changed files with 179 additions and 183 deletions
Download Patch File Download Diff File Expand all files Collapse all files

97
app/gl_barrier.c
View File

@@ -2,9 +2,9 @@
#include "gl_headfile.h"
enum barrier_type_e barrier_type = BARRIER_NONE;
void CheckBarrier() {
uint16 time_barrier;
void CheckBarrier()
{
if (barrier_type == BARRIER_NONE && Lpt0_found_barrier_in && !Lpt1_found_barrier_in && is_straight1) {
barrier_type = BARRIER_LEFT_BEGIN;
}
@@ -13,54 +13,51 @@ void CheckBarrier() {
}
}
void RunBarrier() {
if (barrier_type == BARRIER_LEFT_BEGIN)
{
track_type = TRACK_RIGHT;
if (Lpt0_found)
{
Lpt0_found_count++;
void RunBarrier()
{
if (barrier_type == BARRIER_LEFT_BEGIN) {
track_type = TRACK_RIGHT;
if (Lpt0_found) {
Lpt0_found_count++;
}
if (Lpt0_found_count >= 1 && time_barrier >= 5000) {
Lpt0_found_count = 0;
barrier_type = BARRIER_LEFT_RUNNING;
track_type = TRACK_RIGHT;
timer_clear(TIM_3);
timer_start(TIM_3);
}
} else if (barrier_type == BARRIER_LEFT_RUNNING) {
track_type = TRACK_RIGHT;
time_barrier = timer_get(TIM_3);
if (time_barrier >= 5000) {
barrier_type = BARRIER_NONE;
track_type = TRACK_RIGHT;
timer_start(TIM_3);
timer_clear(TIM_3);
}
}
if (Lpt0_found_count >= 1)
{
Lpt0_found_count = 0;
barrier_type = BARRIER_LEFT_RUNNING;
track_type = TRACK_RIGHT;
}
}
else if (barrier_type == BARRIER_LEFT_RUNNING)
{
track_type = TRACK_RIGHT;
if (Lpt0_found)
{
barrier_type = BARRIER_NONE;
track_type = TRACK_RIGHT;
}
}
if (barrier_type == BARRIER_RIGHT_BEGIN)
{
track_type = TRACK_LEFT;
if (Lpt1_found)
{
Lpt1_found_count++;
if (barrier_type == BARRIER_RIGHT_BEGIN) {
track_type = TRACK_LEFT;
if (Lpt1_found) {
Lpt1_found_count++;
}
if (Lpt1_found_count >= 1) {
Lpt1_found_count = 0;
barrier_type = BARRIER_RIGHT_RUNNING;
track_type = TRACK_LEFT;
timer_clear(TIM_3);
timer_start(TIM_3);
}
} else if (barrier_type == BARRIER_RIGHT_RUNNING) {
track_type = TRACK_LEFT;
time_barrier = timer_get(TIM_3);
if (time_barrier >= 5000) {
timer_start(TIM_3);
timer_clear(TIM_3);
barrier_type = BARRIER_NONE;
track_type = TRACK_LEFT;
}
}
if (Lpt1_found_count >= 1)
{
Lpt1_found_count = 0;
barrier_type = BARRIER_RIGHT_RUNNING;
track_type = TRACK_LEFT;
}
}
else if (barrier_type == BARRIER_RIGHT_RUNNING)
{
track_type = TRACK_LEFT;
if (Lpt1_found)
{
barrier_type = BARRIER_NONE;
track_type = TRACK_LEFT;
}
}
}

2
app/gl_barrier.h
View File

@@ -10,7 +10,7 @@ enum barrier_type_e {
BARRIER_LEFT_OUT,
BARRIER_RIGHT_OUT,
};
extern uint16 time_barrier;
extern enum barrier_type_e barrier_type;
void CheckBarrier(void);

260
app/gl_circle.c
View File

@@ -3,21 +3,20 @@
enum circle_type_e circle_type = CIRCLE_NONE;
int32_t Left_Border_None_Circle = 0;
int32_t Left_Border_None_Circle = 0;
int32_t Right_Border_None_Circle = 0;
int32_t Left_Border_Have_Circle = 0;
int32_t Left_Border_Have_Circle = 0;
int32_t Right_Border_Have_Circle = 0;
int32_t Left_Border_ToLeft_Circle = 0;
int32_t Left_Border_ToLeft_Circle = 0;
int32_t Right_Border_ToLeft_Circle = 0;
int32_t Left_Border_ToRight_Circle = 0;
int32_t Left_Border_ToRight_Circle = 0;
int32_t Right_Border_ToRight_Circle = 0;
void CheckCircle() {
void CheckCircle()
{
// 非圆环模式下，单边L角点, 单边长直道
if (circle_type == CIRCLE_NONE && Lpt0_found && !Lpt1_found && is_straight1) {
circle_type = CIRCLE_LEFT_BEGIN;
@@ -27,137 +26,136 @@ void CheckCircle() {
}
}
void RunCircle() {
if (circle_type == CIRCLE_LEFT_BEGIN) // 左环开始，寻外直道右线
{
track_type = TRACK_RIGHT;
// 先丢左线后有线
if (pts_resample_left_count < 0.1 / RESAMPLEDIST) {
Left_Border_None_Circle++;
}
if (pts_resample_left_count > 0.5 / RESAMPLEDIST &&Left_Border_None_Circle > FRAMENONE) {
Left_Border_Have_Circle++;
if (Left_Border_Have_Circle > FRAMENONE) {
circle_type = CIRCLE_LEFT_IN;
Left_Border_None_Circle = 0;
Left_Border_Have_Circle = 0;
}
}
} else if (circle_type == CIRCLE_LEFT_IN) // 入环，寻内圆左线
{
track_type = TRACK_LEFT;
if (pts_resample_right[(int32_t)(0.2 / RESAMPLEDIST)][1] -
pts_resample_right[0][1] <
-2) {
Right_Border_ToLeft_Circle++;
}
if (Right_Border_ToLeft_Circle > FRAMETOLEFT) {
circle_type = CIRCLE_LEFT_RUNNING;
Right_Border_ToLeft_Circle = 0;
}
} else if (circle_type == CIRCLE_LEFT_RUNNING) // 正常巡线，寻外圆右线
{
track_type = TRACK_RIGHT;
// track_type = TRACK_LEFT; // 看看加一个如果丢线才切换
if (Lpt1_found) {
pts_resample_right_count = mid_right_count = Lpt1_rpts1s_id;
}
if (Lpt1_found && Lpt1_rpts1s_id < 0.4 / RESAMPLEDIST) {
circle_type = CIRCLE_LEFT_OUT;
}
} else if (circle_type == CIRCLE_LEFT_OUT) // 出环，寻内圆
{
track_type = TRACK_LEFT;
if (is_straight1) // 右线为长直道
void RunCircle()
{
if (circle_type == CIRCLE_LEFT_BEGIN) // 左环开始，寻外直道右线
{
circle_type = CIRCLE_LEFT_END;
}
} else if (circle_type == CIRCLE_LEFT_END) // 走过圆环，寻右线
{
track_type = TRACK_RIGHT;
track_type = TRACK_RIGHT;
if (pts_resample_left_count < 0.2 / RESAMPLEDIST) // 左线先丢后有
// 先丢左线后有线
if (pts_resample_left_count < 0.1 / RESAMPLEDIST) {
Left_Border_None_Circle++;
}
if (pts_resample_left_count > 0.5 / RESAMPLEDIST && Left_Border_None_Circle > FRAMENONE) {
Left_Border_Have_Circle++;
if (Left_Border_Have_Circle > FRAMENONE) {
circle_type = CIRCLE_LEFT_IN;
Left_Border_None_Circle = 0;
Left_Border_Have_Circle = 0;
}
}
} else if (circle_type == CIRCLE_LEFT_IN) // 入环，寻内圆左线
{
Left_Border_None_Circle++;
}
if (pts_resample_left_count > 0.6 / RESAMPLEDIST &&
Left_Border_None_Circle > FRAMENONE) {
circle_type = CIRCLE_NONE;
Left_Border_None_Circle = 0;
Left_Border_Have_Circle = 0;
Right_Border_ToLeft_Circle = 0;
Right_Border_ToRight_Circle = 0;
}
track_type = TRACK_LEFT;
} else if (circle_type == CIRCLE_RIGHT_BEGIN) // 右环控制，前期寻左直道
{
track_type = TRACK_LEFT;
// 先丢右线后有线
if (pts_resample_right_count < 0.3 / RESAMPLEDIST) {
Right_Border_None_Circle++;
}
if (pts_resample_right_count > 0.5 / RESAMPLEDIST &&
Right_Border_None_Circle > FRAMENONE) {
Right_Border_Have_Circle++;
if (Right_Border_Have_Circle > FRAMENONE) {
circle_type = CIRCLE_RIGHT_IN;
Right_Border_None_Circle = 0;
Right_Border_Have_Circle = 0;
}
}
} else if (circle_type == CIRCLE_RIGHT_IN) // 入右环，寻右内圆环
{
track_type = TRACK_RIGHT;
if (pts_resample_left[(int32_t)(0.2 / RESAMPLEDIST)][1] -
pts_resample_left[0][1] >
2) {
Left_Border_ToRight_Circle++;
}
if (Left_Border_ToRight_Circle > FRAMETORIGHT) {
circle_type = CIRCLE_RIGHT_RUNNING;
Left_Border_ToRight_Circle = 0;
}
} else if (circle_type == CIRCLE_RIGHT_RUNNING) // 正常巡线，寻外圆左线
{
track_type = TRACK_LEFT;
// track_type = TRACK_RIGHT; // 看看加一个如果丢线才切换
if (Lpt0_found) // 外环存在拐点，可再加拐点距离判据 (左 L 点)
if (pts_resample_right[(int32_t)(0.2 / RESAMPLEDIST)][1] -
pts_resample_right[0][1] <
-2) {
Right_Border_ToLeft_Circle++;
}
if (Right_Border_ToLeft_Circle > FRAMETOLEFT) {
circle_type = CIRCLE_LEFT_RUNNING;
Right_Border_ToLeft_Circle = 0;
}
} else if (circle_type == CIRCLE_LEFT_RUNNING) // 正常巡线，寻外圆右线
{
pts_resample_left_count = mid_left_count = Lpt0_rpts0s_id;
}
if (Lpt0_found && Lpt0_rpts0s_id < 0.4 / RESAMPLEDIST) {
circle_type = CIRCLE_RIGHT_OUT;
}
} else if (circle_type == CIRCLE_RIGHT_OUT) // 出环，寻内圆
{
track_type = TRACK_RIGHT;
if (is_straight0) // 加个有线长度判断
track_type = TRACK_RIGHT;
// track_type = TRACK_LEFT; // 看看加一个如果丢线才切换
if (Lpt1_found) {
pts_resample_right_count = mid_right_count = Lpt1_rpts1s_id;
}
if (Lpt1_found && Lpt1_rpts1s_id < 0.4 / RESAMPLEDIST) {
circle_type = CIRCLE_LEFT_OUT;
}
} else if (circle_type == CIRCLE_LEFT_OUT) // 出环，寻内圆
{
circle_type = CIRCLE_RIGHT_END;
}
} else if (circle_type == CIRCLE_RIGHT_END) // 走过圆环，寻左线
{
track_type = TRACK_LEFT;
track_type = TRACK_LEFT;
if (pts_resample_right_count < 0.2 / RESAMPLEDIST) // 左线先丢后有
if (is_straight1) // 右线为长直道
{
circle_type = CIRCLE_LEFT_END;
}
} else if (circle_type == CIRCLE_LEFT_END) // 走过圆环，寻右线
{
Right_Border_None_Circle++;
track_type = TRACK_RIGHT;
if (pts_resample_left_count < 0.2 / RESAMPLEDIST) // 左线先丢后有
{
Left_Border_None_Circle++;
}
if (pts_resample_left_count > 0.6 / RESAMPLEDIST &&
Left_Border_None_Circle > FRAMENONE) {
circle_type = CIRCLE_NONE;
Left_Border_None_Circle = 0;
Left_Border_Have_Circle = 0;
Right_Border_ToLeft_Circle = 0;
Right_Border_ToRight_Circle = 0;
}
} else if (circle_type == CIRCLE_RIGHT_BEGIN) // 右环控制，前期寻左直道
{
track_type = TRACK_LEFT;
// 先丢右线后有线
if (pts_resample_right_count < 0.3 / RESAMPLEDIST) {
Right_Border_None_Circle++;
}
if (pts_resample_right_count > 0.5 / RESAMPLEDIST &&
Right_Border_None_Circle > FRAMENONE) {
Right_Border_Have_Circle++;
if (Right_Border_Have_Circle > FRAMENONE) {
circle_type = CIRCLE_RIGHT_IN;
Right_Border_None_Circle = 0;
Right_Border_Have_Circle = 0;
}
}
} else if (circle_type == CIRCLE_RIGHT_IN) // 入右环，寻右内圆环
{
track_type = TRACK_RIGHT;
if (pts_resample_left[(int32_t)(0.2 / RESAMPLEDIST)][1] -
pts_resample_left[0][1] >
2) {
Left_Border_ToRight_Circle++;
}
if (Left_Border_ToRight_Circle > FRAMETORIGHT) {
circle_type = CIRCLE_RIGHT_RUNNING;
Left_Border_ToRight_Circle = 0;
}
} else if (circle_type == CIRCLE_RIGHT_RUNNING) // 正常巡线，寻外圆左线
{
// track_type = TRACK_RIGHT;
track_type = TRACK_LEFT; // 看看加一个如果丢线才切换
if (Lpt0_found) // 外环存在拐点，可再加拐点距离判据 (左 L 点)
{
pts_resample_left_count = mid_left_count = Lpt0_rpts0s_id;
}
if (Lpt0_found && Lpt0_rpts0s_id < 0.4 / RESAMPLEDIST) {
circle_type = CIRCLE_RIGHT_OUT;
}
} else if (circle_type == CIRCLE_RIGHT_OUT) // 出环，寻内圆
{
track_type = TRACK_RIGHT;
if (is_straight0) // 加个有线长度判断
{
circle_type = CIRCLE_RIGHT_END;
}
} else if (circle_type == CIRCLE_RIGHT_END) // 走过圆环，寻左线
{
track_type = TRACK_LEFT;
if (pts_resample_right_count < 0.2 / RESAMPLEDIST) // 左线先丢后有
{
Right_Border_None_Circle++;
}
if (pts_resample_right_count > 0.7 / RESAMPLEDIST &&
Right_Border_None_Circle > FRAMENONE) {
circle_type = CIRCLE_NONE;
Right_Border_None_Circle = 0;
Right_Border_Have_Circle = 0;
Left_Border_ToLeft_Circle = 0;
Left_Border_ToRight_Circle = 0;
}
}
if (pts_resample_right_count > 0.7 / RESAMPLEDIST &&
Right_Border_None_Circle > FRAMENONE) {
circle_type = CIRCLE_NONE;
Right_Border_None_Circle = 0;
Right_Border_Have_Circle = 0;
Left_Border_ToLeft_Circle = 0;
Left_Border_ToRight_Circle = 0;
}
}
}

2
app/main.c
View File

@@ -49,7 +49,7 @@ int main(void)
Page_Init();
pit_ms_init(TIM1_PIT, 10);
timer_init(TIM_3, TIMER_MS);
by_frame_init();
printf("start running\r\n");

1
app/page/page_rtcam.c
View File

@@ -62,6 +62,7 @@ static void Loop()
ips200_show_uint(160, 205, Lpt0_found_barrier_in, 3);
ips200_show_uint(160, 224, Lpt1_found_barrier_in, 3);
ips200_show_uint(200, 165, barrier_type, 3);
ips200_show_uint(200, 185, time_barrier, 5);
}
/**