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

fix: 避障bug修复

Browse Source
This commit is contained in:
井明江
2024-03-30 11:43:43 +08:00
parent 77558d2a31
commit 9e23f1968d
7 changed files with 32 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
app/gl_barrier.c
View File

@@ -20,7 +20,7 @@ void RunBarrier()
if (Lpt0_found) { if (Lpt0_found) {
Lpt0_found_count++; Lpt0_found_count++;
} }
if (Lpt0_found_count >= 1 && time_barrier >= 5000) { if (Lpt0_found_count >= 1) {
Lpt0_found_count = 0; Lpt0_found_count = 0;
barrier_type = BARRIER_LEFT_RUNNING; barrier_type = BARRIER_LEFT_RUNNING;
track_type = TRACK_RIGHT; track_type = TRACK_RIGHT;
@@ -30,7 +30,7 @@ void RunBarrier()
} else if (barrier_type == BARRIER_LEFT_RUNNING) { } else if (barrier_type == BARRIER_LEFT_RUNNING) {
track_type = TRACK_RIGHT; track_type = TRACK_RIGHT;
time_barrier = timer_get(TIM_3); time_barrier = timer_get(TIM_3);
if (time_barrier >= 5000) { if (time_barrier >= 1000) {
barrier_type = BARRIER_NONE; barrier_type = BARRIER_NONE;
track_type = TRACK_RIGHT; track_type = TRACK_RIGHT;
timer_start(TIM_3); timer_start(TIM_3);
@@ -53,7 +53,7 @@ void RunBarrier()
} else if (barrier_type == BARRIER_RIGHT_RUNNING) { } else if (barrier_type == BARRIER_RIGHT_RUNNING) {
track_type = TRACK_LEFT; track_type = TRACK_LEFT;
time_barrier = timer_get(TIM_3); time_barrier = timer_get(TIM_3);
if (time_barrier >= 5000) { if (time_barrier >= 1000) {
timer_start(TIM_3); timer_start(TIM_3);
timer_clear(TIM_3); timer_clear(TIM_3);
barrier_type = BARRIER_NONE; barrier_type = BARRIER_NONE;

2
app/gl_garage.c
View File

@@ -88,7 +88,7 @@ void CheckGarage()
} }
} }
//如果色块的个数在6~9之间则认为这一行的斑马线满足要求，在去扫下一行 //如果色块的个数在6~9之间则认为这一行的斑马线满足要求，在去扫下一行
if (banmaxian_geshu >= 2 ){banmaxian_hangshu++;} if (banmaxian_geshu >= 6 ){banmaxian_hangshu++;}
} }
//如果有大于等于4行的有效斑马线 //如果有大于等于4行的有效斑马线
if(banmaxian_hangshu>=2) if(banmaxian_hangshu>=2)

1
app/gl_state.h
View File

@@ -5,6 +5,7 @@ enum state_type_e {
COMMON_STATE, COMMON_STATE,
TURN_STATE, TURN_STATE,
STRAIGHT_STATE, STRAIGHT_STATE,
}; };
extern enum state_type_e state_type; extern enum state_type_e state_type;

50
app/gl_tracking.c
View File

@@ -1,6 +1,6 @@
#include "zf_common_headfile.h" #include "zf_common_headfile.h"
#include "gl_headfile.h" #include "gl_headfile.h"
#include "jj_blueteeth.h"
float (*mid_track)[2]; float (*mid_track)[2];
int32_t mid_track_count; int32_t mid_track_count;
float pure_angle; float pure_angle;
@@ -124,7 +124,6 @@ void MidLineTrack()
mid_track[begin_id][1] = cx; mid_track[begin_id][1] = cx;
rptsn_num = sizeof(rptsn) / sizeof(rptsn[0]); rptsn_num = sizeof(rptsn) / sizeof(rptsn[0]);
GetLinesResample(mid_track + begin_id, mid_track_count - begin_id, rptsn, &rptsn_num, RESAMPLEDIST * PIXPERMETER); GetLinesResample(mid_track + begin_id, mid_track_count - begin_id, rptsn, &rptsn_num, RESAMPLEDIST * PIXPERMETER);
// 远预锚点位置- // 远预锚点位置-
int aim_idx = clip(round(aim_distance / RESAMPLEDIST), 0, rptsn_num - 1); int aim_idx = clip(round(aim_distance / RESAMPLEDIST), 0, rptsn_num - 1);
@@ -132,41 +131,37 @@ void MidLineTrack()
// 近锚点位置 // 近锚点位置
int aim_idx_near = clip(round(0.09 / RESAMPLEDIST), 0, rptsn_num - 1); int aim_idx_near = clip(round(0.09 / RESAMPLEDIST), 0, rptsn_num - 1);
int gap_1 = fabs(rptsn[3 * (rptsn_num / 4)][1] - rptsn[0][1]); int gap_1 = fabs(rptsn[3 * (rptsn_num / 4)][1] - rptsn[0][1]);
float dx1 = rptsn[3 * (rptsn_num / 4)][0] - rptsn[aim_idx][0]; float dx1 = rptsn[3 * (rptsn_num / 4)][0] - rptsn[aim_idx][0];
float dy1 = rptsn[3 * (rptsn_num / 4)][1] - rptsn[aim_idx][1]; float dy1 = rptsn[3 * (rptsn_num / 4)][1] - rptsn[aim_idx][1];
float dn1 = Q_sqrt(dx1 * dx1 + dy1 * dy1); float dn1 = Q_sqrt(dx1 * dx1 + dy1 * dy1);
float dx2 = rptsn[aim_idx][0] - rptsn[aim_idx_near][0]; float dx2 = rptsn[aim_idx][0] - rptsn[aim_idx_near][0];
float dy2 = rptsn[aim_idx][1] - rptsn[aim_idx_near][1]; float dy2 = rptsn[aim_idx][1] - rptsn[aim_idx_near][1];
float dn2 = Q_sqrt(dx2 * dx2 + dy2 * dy2); float dn2 = Q_sqrt(dx2 * dx2 + dy2 * dy2);
float c1 = dx1 / dn1; float c1 = dx1 / dn1;
float s1 = dy1 / dn1; float s1 = dy1 / dn1;
float c2 = dx2 / dn2; float c2 = dx2 / dn2;
float s2 = dy2 / dn2; float s2 = dy2 / dn2;
float angle_1= atan2f(c1 * s2 - c2 * s1, c2 * c1 + s2 * s1); float angle_1 = atan2f(c1 * s2 - c2 * s1, c2 * c1 + s2 * s1);
ips114_show_float(120, 80, angle_1, 3, 4); ips114_show_float(120, 80, angle_1, 3, 4);
if (-0.3 <= angle_1 <= 0.3 ) if (angle_1 >= 0.3f || angle_1 <= -0.3f) {
{
state_type = TURN_STATE; state_type = TURN_STATE;
} } else {
else
{
state_type = STRAIGHT_STATE; state_type = STRAIGHT_STATE;
} }
if (circle_type == CIRCLE_LEFT_IN || circle_type == CIRCLE_LEFT_OUT || circle_type == CIRCLE_RIGHT_IN || circle_type == CIRCLE_RIGHT_OUT) // if (circle_type == CIRCLE_LEFT_IN || circle_type == CIRCLE_LEFT_OUT || circle_type == CIRCLE_RIGHT_IN || circle_type == CIRCLE_RIGHT_OUT || circle_type == CIRCLE_LEFT_RUNNING || circle_type == CIRCLE_RIGHT_RUNNING)
{ //{
state_type = TURN_STATE; // state_type = TURN_STATE;
} //}
// 计算远锚点偏差值 // 计算远锚点偏差值
float dx = rptsn[aim_idx][1] - cx; float dx = rptsn[aim_idx][1] - cx;
float dy = cy - rptsn[aim_idx][0] + 0.2f * PIXPERMETER; float dy = cy - rptsn[aim_idx][0] + 0.2f * PIXPERMETER;
float dn = (float)Q_sqrt(dx * dx + dy * dy); float dn = (float)Q_sqrt(dx * dx + dy * dy);
// float error = -atan2f(dx, dy) * 180 / PI32; // float error = -atan2f(dx, dy) * 180 / PI32;
bt_printf("%d", barrier_type);
if (barrier_type == BARRIER_LEFT_BEGIN) { if (barrier_type == BARRIER_LEFT_BEGIN) {
dx_near = rptsn[aim_idx_near][1] - cx +barrier_offset; dx_near = rptsn[aim_idx_near][1] - cx + barrier_offset;
pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f; pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f;
} else if (barrier_type == BARRIER_RIGHT_BEGIN) { } else if (barrier_type == BARRIER_RIGHT_BEGIN) {
dx_near = rptsn[aim_idx_near][1] - cx - barrier_offset; dx_near = rptsn[aim_idx_near][1] - cx - barrier_offset;
@@ -187,4 +182,7 @@ void MidLineTrack()
// // //考虑远点 // // //考虑远点
// pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f; // pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f;
} }
if (circle_type == CIRCLE_LEFT_IN || circle_type == CIRCLE_LEFT_OUT || circle_type == CIRCLE_RIGHT_IN || circle_type == CIRCLE_RIGHT_OUT || circle_type == CIRCLE_LEFT_RUNNING || circle_type == CIRCLE_RIGHT_RUNNING) {
state_type = TURN_STATE;
}
} }

2
app/jj_blueteeth.c
View File

@@ -21,7 +21,7 @@ void jj_bt_run()
{ {
} }
void jj_bt_printf(const char *format, ...) void bt_printf(const char *format, ...)
{ {
char sbuf[40]; char sbuf[40];
va_list args; va_list args;

2
app/jj_blueteeth.h
View File

@@ -10,5 +10,5 @@
void jj_bt_init(); void jj_bt_init();
void jj_bt_run(); void jj_bt_run();
void jj_bt_printf(const char *format, ...); void bt_printf(const char *format, ...);
#endif #endif

2
app/main.c
View File

@@ -59,7 +59,7 @@ int main(void)
tiny_frame_param[0].f32 = pure_angle; tiny_frame_param[0].f32 = pure_angle;
tiny_frame_param[1].f32 = dx_near; tiny_frame_param[1].f32 = dx_near;
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[3].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)));