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

Compare commits

base: btl143:1166f718834624a5a78d482486c83aa98edd8d1f
Branches Tags
btl143:master
..
compare: btl143:61a92c862093055d4c6d77c38cb2bb072cbc3d59
Branches Tags
btl143:master

2 Commits
1166f71883 ... 61a92c8620

Author SHA1 Message Date
井明江
61a92c8620 日常 2024-07-03 16:32:45 +08:00
井明江
a94db790a6 避障 2024-07-03 16:30:37 +08:00
4 changed files with 97 additions and 100 deletions
Expand all files Collapse all files

66
app/gl_barrier.c
View File

@@ -24,32 +24,26 @@ void RunBarrier()
{ {
if (barrier_type == BARRIER_LEFT_BEGIN) { if (barrier_type == BARRIER_LEFT_BEGIN) {
track_type = TRACK_RIGHT; track_type = TRACK_RIGHT;
if (barrier_begin_flag == 0) if (barrier_begin_flag == 0) {
{
timer_clear(TIM_3); timer_clear(TIM_3);
timer_start(TIM_3); timer_start(TIM_3);
barrier_begin_flag = 1; barrier_begin_flag = 1;
} }
}
if (barrier_begin_flag == 1) if (barrier_type == BARRIER_LEFT_BEGIN || barrier_type == BARRIER_LEFT_RUNNING) {
{ if (barrier_begin_flag == 1) {
barrier_type = BARRIER_LEFT_BEGIN; barrier_type = BARRIER_LEFT_BEGIN;
time_barrier = timer_get(TIM_3); time_barrier = timer_get(TIM_3);
if (time_barrier >= 1000 && time_barrier < 2000) if (time_barrier >= 300 && time_barrier < 600) {
{
barrier_type = BARRIER_LEFT_RUNNING; barrier_type = BARRIER_LEFT_RUNNING;
} } else if (time_barrier >= 600) {
else if (time_barrier >= 1500)
{
barrier_type = BARRIER_NONE; barrier_type = BARRIER_NONE;
barrier_begin_flag = 0; barrier_begin_flag = 0;
timer_stop(TIM_3); timer_stop(TIM_3);
timer_clear(TIM_3); timer_clear(TIM_3);
} }
} }
}
// if (Lpt0_found) { // if (Lpt0_found) {
// Lpt0_found_count++; // Lpt0_found_count++;
@@ -70,34 +64,28 @@ void RunBarrier()
// timer_start(TIM_3); // timer_start(TIM_3);
// timer_clear(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 (barrier_begin_flag == 0) if (barrier_begin_flag == 0) {
{
timer_clear(TIM_3); timer_clear(TIM_3);
timer_start(TIM_3); timer_start(TIM_3);
barrier_begin_flag = 1; barrier_begin_flag = 1;
} }
}
if (barrier_begin_flag == 1) if (barrier_type == BARRIER_RIGHT_BEGIN || barrier_type == BARRIER_RIGHT_RUNNING) {
{ if (barrier_begin_flag == 1) {
barrier_type = BARRIER_RIGHT_BEGIN; barrier_type = BARRIER_RIGHT_BEGIN;
time_barrier = timer_get(TIM_3); time_barrier = timer_get(TIM_3);
if (time_barrier >= 1000) if (time_barrier >= 300 && time_barrier < 600) {
{
barrier_type = BARRIER_RIGHT_RUNNING; barrier_type = BARRIER_RIGHT_RUNNING;
if (time_barrier >= 2000) } else if (time_barrier >= 600) {
{
barrier_type = BARRIER_NONE; barrier_type = BARRIER_NONE;
barrier_begin_flag = 0; barrier_begin_flag = 0;
timer_stop(TIM_3); timer_stop(TIM_3);
timer_clear(TIM_3); timer_clear(TIM_3);
} }
} }
} }
// if (Lpt1_found) { // if (Lpt1_found) {
@@ -120,10 +108,9 @@ void RunBarrier()
// track_type = TRACK_LEFT; // track_type = TRACK_LEFT;
// } // }
} }
}
void Check_s()
void Check_s(){ {
if (track_type == TRACK_LEFT) { if (track_type == TRACK_LEFT) {
mid_track_s = mid_left; mid_track_s = mid_left;
mid_track_count_s = mid_left_count; mid_track_count_s = mid_left_count;
@@ -135,35 +122,26 @@ void Check_s(){
int min_s = 1000; int min_s = 1000;
int max_s = 1; int max_s = 1;
for (int i = 0; i < mid_track_count_s; i++) for (int i = 0; i < mid_track_count_s; i++) {
{ if (mid_track_s[i][1] < min_s) {
if (mid_track_s[i][1] < min_s)
{
min_s = mid_track_s[i][1]; min_s = mid_track_s[i][1];
temp_min_s = min_s; temp_min_s = min_s;
} }
if (mid_track_s[i][1] > max_s) if (mid_track_s[i][1] > max_s) {
{
max_s = mid_track_s[i][1]; max_s = mid_track_s[i][1];
temp_max_s = max_s; temp_max_s = max_s;
} }
} }
if (is_straight0 == false && is_straight1 == false && (max_s - min_s) <= 30 && (max_s - min_s) >= 5) {
if (is_straight0 == false && is_straight1 == false && (max_s - min_s) <= 30 && (max_s - min_s) >= 5)
{
s_type = S_BEGIN; s_type = S_BEGIN;
} }
} }
void RunS()
void RunS(){
if (temp_max_s - temp_min_s >30 || (is_straight0 && is_straight1))
{ {
if (temp_max_s - temp_min_s > 30 || (is_straight0 && is_straight1)) {
s_type = S_NONE; s_type = S_NONE;
} }
} }

4
app/gl_get_corners.c
View File

@@ -59,7 +59,7 @@ void get_corners()
float conf = fabs(angle_left_barrier[i]) - (fabs(angle_left_barrier[im1]) + fabs(angle_left_barrier[ip1])) / 2; float conf = fabs(angle_left_barrier[i]) - (fabs(angle_left_barrier[im1]) + fabs(angle_left_barrier[ip1])) / 2;
// L 角点阈值 // L 角点阈值
if (Lpt0_found_barrier == false && (66. / 180. * PI32) < conf && conf < (140. / 180. * PI32) && i < 0.5 / RESAMPLEDIST) { if (Lpt0_found_barrier == false && (40. / 180. * PI32) < conf && conf < (160. / 180. * PI32) && i < 0.6 / RESAMPLEDIST) {
Lpt0_rpts0s_id_barrier = i; Lpt0_rpts0s_id_barrier = i;
Lpt0_found_barrier = true; Lpt0_found_barrier = true;
} }
@@ -116,7 +116,7 @@ void get_corners()
int ip1 = clip(i + (int)round(ANGLEDIST_barrier / RESAMPLEDIST), 0, pts_resample_right_count - 1); int ip1 = clip(i + (int)round(ANGLEDIST_barrier / RESAMPLEDIST), 0, pts_resample_right_count - 1);
float conf = fabs(angle_right_barrier[i]) - (fabs(angle_right_barrier[im1]) + fabs(angle_right_barrier[ip1])) / 2; float conf = fabs(angle_right_barrier[i]) - (fabs(angle_right_barrier[im1]) + fabs(angle_right_barrier[ip1])) / 2;
if (Lpt1_found_barrier == false && (66. / 180. * PI32) < conf && conf < 140. / 180. * PI32 && i < 0.5 / RESAMPLEDIST) { if (Lpt1_found_barrier == false && (40. / 180. * PI32) < conf && conf < 160. / 180. * PI32 && i < 0.6 / RESAMPLEDIST) {
Lpt1_rpts1s_id_barrier = i; Lpt1_rpts1s_id_barrier = i;
Lpt1_found_barrier = true; Lpt1_found_barrier = true;
} }

57
app/gl_tracking.c
View File

@@ -270,20 +270,22 @@ 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) {
switch (barrier_type) {
case 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 = -25.f; pure_angle = -45.f;
}else if (barrier_type == BARRIER_LEFT_RUNNING) break;
{ case BARRIER_LEFT_RUNNING:
pure_angle = 25.f; pure_angle = 25.f;
}else if (barrier_type == BARRIER_RIGHT_BEGIN) break;
{ case BARRIER_RIGHT_BEGIN:
pure_angle = 25.f; pure_angle = 45.f;
} break;
else if (barrier_type == BARRIER_RIGHT_RUNNING) { case BARRIER_RIGHT_RUNNING:
// dx_near = mid_track[aim_idx_near][1] - cx - barrier_offset; // dx_near = mid_track[aim_idx_near][1] - cx - barrier_offset;
pure_angle = -25.f; pure_angle = -25.f;
} else { default:
// 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) {
pure_angle = -atanf(dx / dy) / PI32 * 180.0f; pure_angle = -atanf(dx / dy) / PI32 * 180.0f;
@@ -295,6 +297,28 @@ void MidLineTrack()
} }
} }
// if (barrier_type == BARRIER_LEFT_BEGIN) {
// // dx_near = mid_track[aim_idx_near][1] - cx + barrier_offset;
// pure_angle = -25.f;
// } else if (barrier_type == BARRIER_LEFT_RUNNING) {
// 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 {
// // pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f;
// if (dy > 0) {
// pure_angle = -atanf(dx / dy) / PI32 * 180.0f;
// last_pure_angle = pure_angle;
// // last_pure_angle_half = pure_angle_half;
// } else {
// pure_angle = last_pure_angle;
// // pure_angle_half = last_pure_angle_half;
// }
// }
// // 计算近锚点偏差值 // // 计算近锚点偏差值
// dx_near = rptsn[aim_idx_near][1] - cx; // dx_near = rptsn[aim_idx_near][1] - cx;
// // float dy_near = cy - rptsn[aim_idx_near][0] + 0.2 * PIXPERMETER; // // float dy_near = cy - rptsn[aim_idx_near][0] + 0.2 * PIXPERMETER;
@@ -308,8 +332,7 @@ void MidLineTrack()
timer_start(TIM_3); timer_start(TIM_3);
} }
if (circle_type == CIRCLE_LEFT_RUNNING || circle_type == CIRCLE_RIGHT_RUNNING || circle_type == CIRCLE_RIGHT_OUT || circle_type == CIRCLE_LEFT_OUT || circle_type == CIRCLE_LEFT_END || circle_type == CIRCLE_RIGHT_END) { if (circle_type == CIRCLE_LEFT_RUNNING || circle_type == CIRCLE_RIGHT_RUNNING || circle_type == CIRCLE_RIGHT_OUT || circle_type == CIRCLE_LEFT_OUT || circle_type == CIRCLE_LEFT_END || circle_type == CIRCLE_RIGHT_END) {
if (circle_flag == 1) if (circle_flag == 1) {
{
state_type = CIRCLE_STATE; state_type = CIRCLE_STATE;
} }
@@ -320,21 +343,15 @@ void MidLineTrack()
circle_flag = 0; circle_flag = 0;
} }
if (circle_flag == 0) if (circle_flag == 0) {
{
state_type = CIRCLE_RUNNING_STATE; state_type = CIRCLE_RUNNING_STATE;
} }
} }
if (circle_type == CIRCLE_NONE) if (circle_type == CIRCLE_NONE) {
{
circle_flag = 1; circle_flag = 1;
} }
if (barrier_type == BARRIER_LEFT_BEGIN || barrier_type == BARRIER_LEFT_RUNNING || barrier_type == BARRIER_RIGHT_BEGIN || barrier_type == BARRIER_RIGHT_RUNNING) { if (barrier_type == BARRIER_LEFT_BEGIN || barrier_type == BARRIER_LEFT_RUNNING || barrier_type == BARRIER_RIGHT_BEGIN || barrier_type == BARRIER_RIGHT_RUNNING) {
state_type = BARRIER_STATE; state_type = BARRIER_STATE;
} }

4
app/main.c
View File

@@ -73,7 +73,9 @@ int main(void)
ElementRun(); ElementRun();
aim_distance_select(); aim_distance_select();
MidLineTrack(); MidLineTrack();
//bt_printf("type= %d", (int)curvature); // if (barrier_type) {
// bt_printf("type= %d", barrier_type);
// }
} }
} }
} }