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

2
app/gl_garage.c
View File

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

1
app/gl_state.h
View File

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

50
app/gl_tracking.c
View File

@@ -1,6 +1,6 @@
#include "zf_common_headfile.h"
#include "gl_headfile.h"
#include "jj_blueteeth.h"
float (*mid_track)[2];
int32_t mid_track_count;
float pure_angle;
@@ -124,7 +124,6 @@ void MidLineTrack()
mid_track[begin_id][1] = cx;
rptsn_num = sizeof(rptsn) / sizeof(rptsn[0]);
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);
@@ -132,41 +131,37 @@ void MidLineTrack()
// 近锚点位置
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]);
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 dn1 = Q_sqrt(dx1 * dx1 + dy1 * dy1);
float dx2 = rptsn[aim_idx][0] - rptsn[aim_idx_near][0];
float dy2 = rptsn[aim_idx][1] - rptsn[aim_idx_near][1];
float dn2 = Q_sqrt(dx2 * dx2 + dy2 * dy2);
float c1 = dx1 / dn1;
float s1 = dy1 / dn1;
float c2 = dx2 / dn2;
float s2 = dy2 / dn2;
float angle_1= atan2f(c1 * s2 - c2 * s1, c2 * c1 + s2 * s1);
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 dy1 = rptsn[3 * (rptsn_num / 4)][1] - rptsn[aim_idx][1];
float dn1 = Q_sqrt(dx1 * dx1 + dy1 * dy1);
float dx2 = rptsn[aim_idx][0] - rptsn[aim_idx_near][0];
float dy2 = rptsn[aim_idx][1] - rptsn[aim_idx_near][1];
float dn2 = Q_sqrt(dx2 * dx2 + dy2 * dy2);
float c1 = dx1 / dn1;
float s1 = dy1 / dn1;
float c2 = dx2 / dn2;
float s2 = dy2 / dn2;
float angle_1 = atan2f(c1 * s2 - c2 * s1, c2 * c1 + s2 * s1);
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;
}
else
{
} else {
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)
{
state_type = TURN_STATE;
}
// 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;
//}
// 计算远锚点偏差值
float dx = rptsn[aim_idx][1] - cx;
float dy = cy - rptsn[aim_idx][0] + 0.2f * PIXPERMETER;
float dn = (float)Q_sqrt(dx * dx + dy * dy);
// float error = -atan2f(dx, dy) * 180 / PI32;
bt_printf("%d", barrier_type);
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;
} else if (barrier_type == BARRIER_RIGHT_BEGIN) {
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;
}
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];
va_list args;

2
app/jj_blueteeth.h
View File

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

2
app/main.c
View File

@@ -59,7 +59,7 @@ int main(void)
tiny_frame_param[0].f32 = pure_angle;
tiny_frame_param[1].f32 = dx_near;
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) {
// 该操作消耗大概 1970 个 tick，折合约 110us
memcpy(mt9v03x_image_copy[0], mt9v03x_image[0], (sizeof(mt9v03x_image_copy) / sizeof(uint8_t)));