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

测试bug

Browse Source
This commit is contained in:
Glz
2024-06-26 17:27:38 +08:00
parent 57e132c535
commit 2b5ea8434e
6 changed files with 31 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
app/gl_tracking.c
View File

@@ -177,19 +177,19 @@ void MidLineTrack()
mid_track = mid_right;
mid_track_count = mid_right_count;
}
float x[mid_track_count / 2];
float y[mid_track_count / 2];
float x[mid_track_count];
float y[mid_track_count];
for (int i = 0; i < mid_track_count / 2; i++) {
for (int i = 0; i < mid_track_count; i++) {
x[i] = mid_track[i][1];
y[i] = mid_track[i][0];
}
if (mid_track_count <= 4) {
if (mid_track_count <= 5) {
curvature = last_curvature;
/* code */
} else {
curvature = calculate_curvature(x, y, mid_track_count / 2);
curvature = calculate_curvature(x, y, mid_track_count);
}
last_curvature = curvature;
}
@@ -223,18 +223,18 @@ void MidLineTrack()
GetLinesResample(mid_track + begin_id, mid_track_count - begin_id, rptsn, &rptsn_num, RESAMPLEDIST * PIXPERMETER);
if (cross_type == CROSS_IN) {
float x[rptsn_num / 2];
float y[rptsn_num / 2];
float x[rptsn_num];
float y[rptsn_num];
for (int i = 0; i < rptsn_num / 2; i++) {
for (int i = 0; i < rptsn_num; i++) {
x[i] = rptsn[i][1];
y[i] = rptsn[i][0];
}
if (rptsn_num <= 4) {
if (rptsn_num <= 5) {
curvature = last_curvature;
/* code */
} else {
curvature = calculate_curvature(x, y, rptsn_num / 2);
curvature = calculate_curvature(x, y, rptsn_num);
}
last_curvature = curvature;
}
@@ -271,10 +271,10 @@ void MidLineTrack()
float temp_near = 0;
if (barrier_type == BARRIER_LEFT_BEGIN || barrier_type == BARRIER_LEFT_RUNNING) {
dx_near = mid_track[aim_idx_near][1] - cx + barrier_offset;
pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn) / PI32 * 180.0f - 20;
pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn) / PI32 * 180.0f - 25;
} else if (barrier_type == BARRIER_RIGHT_BEGIN || barrier_type == BARRIER_RIGHT_RUNNING) {
dx_near = mid_track[aim_idx_near][1] - cx - barrier_offset;
pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn) / PI32 * 180.0f + 20;
pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn) / PI32 * 180.0f + 25;
} else {
// pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f;
if (dy > 0) {
@@ -293,9 +293,19 @@ void MidLineTrack()
// // float dn_near = Q_sqrt(dx_near * dx_near + dy_near * dy_near);
// // float error_near = -atan2f(dx_near, dy_near) * 180 / PI32;
}
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 || circle_type == CIRCLE_LEFT_BEGIN || circle_type == CIRCLE_RIGHT_BEGIN) {
if (circle_type == CIRCLE_LEFT_IN || circle_type == CIRCLE_RIGHT_IN ) {
state_type = CIRCLE_STATE;
}
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)
{
state_type = CIRCLE_RUNNING_STATE;
}
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;
}
if (cross_type == CROSS_BEGIN || cross_type == CROSS_IN) {
state_type = STRAIGHT_STATE;
}