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

避障修改

Browse Source
This commit is contained in:
Glz
2024-03-24 16:07:39 +08:00
parent 7d5bee30d1
commit 1c65b7752e
4 changed files with 42 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
app/gl_barrier.c
View File

@@ -14,24 +14,53 @@ void CheckBarrier() {
} }
void RunBarrier() { void RunBarrier() {
if (barrier_type == BARRIER_LEFT_BEGIN) // 左环开始，寻外直道右线 if (barrier_type == BARRIER_LEFT_BEGIN)
{ {
track_type = TRACK_RIGHT; track_type = TRACK_RIGHT;
if (Lpt0_found)
{
Lpt0_found_count++;
} }
else if (barrier_type == BARRIER_LEFT_BEGIN && ((Lpt0_found_barrier && is_straight1) || Lpt0_found) ) 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; barrier_type = BARRIER_NONE;
track_type = TRACK_RIGHT; track_type = TRACK_RIGHT;
} }
}
if (barrier_type == BARRIER_RIGHT_BEGIN) // 左环开始，寻外直道右线
if (barrier_type == BARRIER_RIGHT_BEGIN)
{ {
track_type = TRACK_LEFT; track_type = TRACK_LEFT;
if (Lpt1_found)
{
Lpt1_found_count++;
} }
else if (barrier_type == BARRIER_RIGHT_BEGIN && ((Lpt1_found_barrier && is_straight0) || Lpt1_found) ) 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; barrier_type = BARRIER_NONE;
track_type = TRACK_LEFT; track_type = TRACK_LEFT;
} }
}
} }

2
app/gl_data.c
View File

@@ -33,6 +33,8 @@ float pts_resample_left[PT_MAXLEN][2];
float pts_resample_right[PT_MAXLEN][2]; float pts_resample_right[PT_MAXLEN][2];
int32_t pts_resample_left_count; int32_t pts_resample_left_count;
int32_t pts_resample_right_count; int32_t pts_resample_right_count;
int32_t Lpt0_found_count;
int32_t Lpt1_found_count;
float pts_far_resample_left[PT_MAXLEN][2]; float pts_far_resample_left[PT_MAXLEN][2];
float pts_far_resample_right[PT_MAXLEN][2]; float pts_far_resample_right[PT_MAXLEN][2];
int32_t pts_far_resample_left_count; int32_t pts_far_resample_left_count;

2
app/gl_data.h
View File

@@ -111,6 +111,8 @@ extern bool Lpt0_found_barrier_in;
extern bool Lpt1_found_barrier_in; extern bool Lpt1_found_barrier_in;
extern int Lpt0_found_barrier_in_id; extern int Lpt0_found_barrier_in_id;
extern int Lpt1_found_barrier_in_id; extern int Lpt1_found_barrier_in_id;
extern int32_t Lpt0_found_count;
extern int32_t Lpt1_found_count;
extern bool is_turn0; extern bool is_turn0;
extern bool is_turn1; extern bool is_turn1;

2
app/gl_state.c
View File

@@ -5,5 +5,5 @@ enum state_type_e state_type =0;
float cricle_aim=0.2f; float cricle_aim=0.2f;
float cross_aim=0.49f; float cross_aim=0.49f;
float common_aim=0.35f; float common_aim=0.35f;
float barrier_aim=0.1f; float barrier_aim=0.5f;
float barrier_offset=4.0f; float barrier_offset=4.0f;