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

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;
}
}