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

避障bug测试

Browse Source
This commit is contained in:
Glz
2024-07-02 16:56:43 +08:00
parent cb87de5508
commit 047a907eac
2 changed files with 96 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

113
app/gl_barrier.c
View File

@@ -8,6 +8,7 @@ float (*mid_track_s)[2];
int32_t mid_track_count_s; int32_t mid_track_count_s;
int temp_min_s; int temp_min_s;
int temp_max_s; int temp_max_s;
int barrier_begin_flag = 0;
void CheckBarrier() void CheckBarrier()
{ {
@@ -23,48 +24,100 @@ 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) { if (barrier_begin_flag == 0)
Lpt0_found_count++; {
}
if (Lpt0_found_count >= 1) {
Lpt0_found_count = 0;
barrier_type = BARRIER_LEFT_RUNNING;
track_type = TRACK_RIGHT;
timer_clear(TIM_3); timer_clear(TIM_3);
timer_start(TIM_3); timer_start(TIM_3);
barrier_begin_flag = 1;
} }
} else if (barrier_type == BARRIER_LEFT_RUNNING) {
track_type = TRACK_RIGHT; if (barrier_begin_flag == 1)
time_barrier = timer_get(TIM_3); {
if (time_barrier >= 220) { barrier_type = BARRIER_LEFT_BEGIN;
barrier_type = BARRIER_NONE; time_barrier = timer_get(TIM_3);
track_type = TRACK_RIGHT; if (time_barrier >= 1000 && time_barrier < 2000)
timer_start(TIM_3); {
timer_clear(TIM_3); barrier_type = BARRIER_LEFT_RUNNING;
}
else if (time_barrier >= 2000)
{
barrier_type = BARRIER_NONE;
barrier_begin_flag = 0;
timer_stop(TIM_3);
timer_clear(TIM_3);
}
} }
// if (Lpt0_found) {
// Lpt0_found_count++;
// }
// if (Lpt0_found_count >= 1) {
// 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 >= 220) {
// barrier_type = BARRIER_NONE;
// track_type = TRACK_RIGHT;
// timer_start(TIM_3);
// timer_clear(TIM_3);
// }
} }
if (barrier_type == BARRIER_RIGHT_BEGIN) { if (barrier_type == BARRIER_RIGHT_BEGIN) {
track_type = TRACK_LEFT; track_type = TRACK_LEFT;
if (Lpt1_found) { if (barrier_begin_flag == 0)
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_clear(TIM_3);
timer_start(TIM_3); timer_start(TIM_3);
barrier_begin_flag = 1;
} }
} else if (barrier_type == BARRIER_RIGHT_RUNNING) {
track_type = TRACK_LEFT; if (barrier_begin_flag == 1)
time_barrier = timer_get(TIM_3); {
if (time_barrier >= 220) { barrier_type = BARRIER_RIGHT_BEGIN;
timer_start(TIM_3); time_barrier = timer_get(TIM_3);
timer_clear(TIM_3); if (time_barrier >= 1000)
barrier_type = BARRIER_NONE; {
track_type = TRACK_LEFT; barrier_type = BARRIER_RIGHT_RUNNING;
if (time_barrier >= 2000)
{
barrier_type = BARRIER_NONE;
barrier_begin_flag = 0;
timer_stop(TIM_3);
timer_clear(TIM_3);
}
}
} }
// 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 >= 220) {
// timer_start(TIM_3);
// timer_clear(TIM_3);
// barrier_type = BARRIER_NONE;
// track_type = TRACK_LEFT;
// }
} }
} }

19
app/gl_tracking.c
View File

@@ -270,12 +270,19 @@ void MidLineTrack()
float dy = cy - rptsn[aim_idx][0]; // + 0.2f * PIXPERMETER; float dy = cy - rptsn[aim_idx][0]; // + 0.2f * PIXPERMETER;
float dn = (dx * dx + dy * dy); float dn = (dx * dx + dy * dy);
float temp_near = 0; float temp_near = 0;
if (barrier_type == BARRIER_LEFT_BEGIN || barrier_type == BARRIER_LEFT_RUNNING) { if (barrier_type == BARRIER_LEFT_BEGIN) {
dx_near = mid_track[aim_idx_near][1] - cx + barrier_offset; //dx_near = mid_track[aim_idx_near][1] - cx + barrier_offset;
pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn) / PI32 * 180.0f - 25; pure_angle = -25.f;
} else if (barrier_type == BARRIER_RIGHT_BEGIN || barrier_type == BARRIER_RIGHT_RUNNING) { }else if (barrier_type == BARRIER_LEFT_RUNNING)
dx_near = mid_track[aim_idx_near][1] - cx - barrier_offset; {
pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn) / PI32 * 180.0f + 25; pure_angle = 25.f;
}else if (barrier_type == BARRIER_RIGHT_BEGIN)
{
pure_angle = 25.f;
}
else if (barrier_type == BARRIER_RIGHT_RUNNING) {
//dx_near = mid_track[aim_idx_near][1] - cx - barrier_offset;
pure_angle = -25.f;
} else { } else {
// pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f; // pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f;
if (dy > 0) { if (dy > 0) {