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() {
if (barrier_type == BARRIER_LEFT_BEGIN) // 左环开始，寻外直道右线
if (barrier_type == BARRIER_LEFT_BEGIN)
{
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;
track_type = TRACK_RIGHT;
}
}
if (barrier_type == BARRIER_RIGHT_BEGIN) // 左环开始，寻外直道右线
if (barrier_type == BARRIER_RIGHT_BEGIN)
{
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;
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];
int32_t pts_resample_left_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_right[PT_MAXLEN][2];
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 int Lpt0_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_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 cross_aim=0.49f;
float common_aim=0.35f;
float barrier_aim=0.1f;
float barrier_aim=0.5f;
float barrier_offset=4.0f;