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

4
app/gl_barrier.c
View File

@@ -36,7 +36,7 @@ void RunBarrier()
} else if (barrier_type == BARRIER_LEFT_RUNNING) {
track_type = TRACK_RIGHT;
time_barrier = timer_get(TIM_3);
if (time_barrier >= 250) {
if (time_barrier >= 220) {
barrier_type = BARRIER_NONE;
track_type = TRACK_RIGHT;
timer_start(TIM_3);
@@ -59,7 +59,7 @@ void RunBarrier()
} else if (barrier_type == BARRIER_RIGHT_RUNNING) {
track_type = TRACK_LEFT;
time_barrier = timer_get(TIM_3);
if (time_barrier >= 250) {
if (time_barrier >= 220) {
timer_start(TIM_3);
timer_clear(TIM_3);
barrier_type = BARRIER_NONE;

2
app/gl_state.c
View File

@@ -10,4 +10,4 @@ float straight_aim=0.35f;
float turn_aim=0.35f;
float mid_aim=0.5f;
float barrier_aim=0.5f;
float barrier_offset=17.0f;
float barrier_offset=10.0f;

2
app/gl_state.h
View File

@@ -6,6 +6,8 @@ enum state_type_e {
TURN_STATE,
STRAIGHT_STATE,
CIRCLE_STATE,
CIRCLE_RUNNING_STATE,
BARRIER_STATE,
};
extern enum state_type_e last_state;
extern enum state_type_e state_type;

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

2
app/main.c
View File

@@ -68,10 +68,10 @@ int main(void)
state_type = COMMON_STATE;
img_processing();
get_corners();
aim_distance_select();
tracking();
ElementJudge();
ElementRun();
aim_distance_select();
MidLineTrack();
//bt_printf("type= %d", (int)curvature);
}

2
app/page/page_dparam.c
View File

@@ -56,6 +56,8 @@ static void Loop()
ips200_show_int(90, 18 + 92, circle_type, 1);
ips200_show_int(90, 18 + 110, s_type, 1);
ips200_show_float(90, 18 + 128, curvature, 4, 5);
ips200_show_int(90, 18 + 146, Lpt0_found_barrier_in, 1);
ips200_show_int(90, 18 + 164, Lpt1_found_barrier_in, 1);
}
/**
* @brief 页面事件