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

90
app/gl_barrier.c
View File

@@ -2,7 +2,7 @@
#include "gl_headfile.h" #include "gl_headfile.h"
enum barrier_type_e barrier_type = BARRIER_NONE; enum barrier_type_e barrier_type = BARRIER_NONE;
enum s_type_e s_type = S_NONE; enum s_type_e s_type = S_NONE;
uint16 time_barrier; uint16 time_barrier;
float (*mid_track_s)[2]; float (*mid_track_s)[2];
int32_t mid_track_count_s; int32_t mid_track_count_s;
@@ -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) {
barrier_type = BARRIER_NONE;
barrier_begin_flag = 0;
timer_stop(TIM_3);
timer_clear(TIM_3);
} }
else if (time_barrier >= 1500)
{
barrier_type = BARRIER_NONE;
barrier_begin_flag = 0;
timer_stop(TIM_3);
timer_clear(TIM_3);
}
} }
}
// if (Lpt0_found) { // if (Lpt0_found) {
// Lpt0_found_count++; // Lpt0_found_count++;
@@ -70,35 +64,29 @@ 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) {
// Lpt1_found_count++; // Lpt1_found_count++;
@@ -119,11 +107,10 @@ void RunBarrier()
// barrier_type = BARRIER_NONE; // barrier_type = BARRIER_NONE;
// 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;
} }

85
app/gl_tracking.c
View File

@@ -14,7 +14,7 @@ float (*rpts)[2];
int rpts_num; int rpts_num;
float last_pure_angle = 0.0f; float last_pure_angle = 0.0f;
int8_t turn_flag = 0; int8_t turn_flag = 0;
int circle_flag = 1; int circle_flag = 1;
// 计算最小二乘法斜率的函数 // 计算最小二乘法斜率的函数
float leastSquaresSlope(float points[][2], int n) float leastSquaresSlope(float points[][2], int n)
{ {
@@ -270,31 +270,55 @@ 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) {
//dx_near = mid_track[aim_idx_near][1] - cx + barrier_offset; switch (barrier_type) {
pure_angle = -25.f; case BARRIER_LEFT_BEGIN:
}else if (barrier_type == BARRIER_LEFT_RUNNING) // dx_near = mid_track[aim_idx_near][1] - cx + barrier_offset;
{ pure_angle = -45.f;
pure_angle = 25.f; break;
}else if (barrier_type == BARRIER_RIGHT_BEGIN) case BARRIER_LEFT_RUNNING:
{ pure_angle = 25.f;
pure_angle = 25.f; break;
} case BARRIER_RIGHT_BEGIN:
else if (barrier_type == BARRIER_RIGHT_RUNNING) { pure_angle = 45.f;
//dx_near = mid_track[aim_idx_near][1] - cx - barrier_offset; break;
pure_angle = -25.f; case BARRIER_RIGHT_RUNNING:
} else { // dx_near = mid_track[aim_idx_near][1] - cx - barrier_offset;
// pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f; pure_angle = -25.f;
if (dy > 0) { default:
pure_angle = -atanf(dx / dy) / PI32 * 180.0f; // pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f;
last_pure_angle = pure_angle; if (dy > 0) {
// last_pure_angle_half = pure_angle_half; pure_angle = -atanf(dx / dy) / PI32 * 180.0f;
} else { last_pure_angle = pure_angle;
pure_angle = last_pure_angle; // last_pure_angle_half = pure_angle_half;
// pure_angle_half = last_pure_angle_half; } else {
} pure_angle = last_pure_angle;
// pure_angle_half = last_pure_angle_half;
}
} }
// 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;
} }

12
app/main.c
View File

@@ -56,10 +56,10 @@ int main(void)
while (1) { while (1) {
Page_Run(); Page_Run();
by_buzzer_run(); by_buzzer_run();
tiny_frame_param[0].f32 = pure_angle; tiny_frame_param[0].f32 = pure_angle;
tiny_frame_param[1].f32 = curvature; tiny_frame_param[1].f32 = curvature;
tiny_frame_param[2].u8[0] = (uint8_t)state_type; // 0: 无状态 1: 弯道 2: 直行 tiny_frame_param[2].u8[0] = (uint8_t)state_type; // 0: 无状态 1: 弯道 2: 直行
tiny_frame_param[2].u8[1] = (uint8_t)garage_type; // 0: 无车库 1: 有车库 tiny_frame_param[2].u8[1] = (uint8_t)garage_type; // 0: 无车库 1: 有车库
if (mt9v03x_finish_flag) { if (mt9v03x_finish_flag) {
// 该操作消耗大概 1970 个 tick，折合约 110us // 该操作消耗大概 1970 个 tick，折合约 110us
memcpy(mt9v03x_image_copy[0], mt9v03x_image[0], (sizeof(mt9v03x_image_copy) / sizeof(uint8_t))); memcpy(mt9v03x_image_copy[0], mt9v03x_image[0], (sizeof(mt9v03x_image_copy) / sizeof(uint8_t)));
@@ -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);
// }
} }
} }
} }