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

日常更新

Browse Source
This commit is contained in:
Glz
2024-07-13 19:00:45 +08:00
parent af031e23d8
commit b88aaf47e5
13 changed files with 171 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files

48
app/gl_tracking.c
View File

@@ -112,7 +112,7 @@ void aim_distance_select(void)
void ElementJudge()
{
// if(begin_flag == 1){
// shield_type = SHIELD_BEGIN;
// begin_flag = 0;
@@ -139,13 +139,16 @@ void ElementJudge()
cross_type = CROSS_NONE;
circle_type = CIRCLE_NONE;
barrier_type = BARRIER_NONE;
}
if (cross_type != CROSS_NONE) {
circle_type = CIRCLE_NONE;
barrier_type = BARRIER_NONE;
}
if (barrier_type != BARRIER_NONE) {
circle_type = CIRCLE_NONE;
}
}
@@ -183,7 +186,16 @@ void MidLineTrack()
GetMidLine_Right(pts_far_resample_right + far_Lpt1_rpts1s_id, pts_far_resample_right_count - far_Lpt1_rpts1s_id, mid_right, (int)round(ANGLEDIST / RESAMPLEDIST), PIXPERMETER * ROADWIDTH / 2);
mid_track_count = pts_far_resample_right_count - far_Lpt1_rpts1s_id;
}
} else {
} else if (barrier_type == BARRIER_LEFT_RUNNING || barrier_type == BARRIER_RIGHT_RUNNING)
{
if (track_type == TRACK_LEFT) {
mid_track = mid_left_barrier;
mid_track_count = mid_left_barrier_count;
} else {
mid_track = mid_right_barrier;
mid_track_count = mid_right_barrier_count;
}
}else {
if (track_type == TRACK_LEFT) {
mid_track = mid_left;
mid_track_count = mid_left_count;
@@ -287,17 +299,10 @@ void MidLineTrack()
switch (barrier_type) {
case BARRIER_LEFT_BEGIN:
// dx_near = mid_track[aim_idx_near][1] - cx + barrier_offset;
pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f - 20;
break;
case BARRIER_LEFT_RUNNING:
pure_angle = 15.f;
pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f;
break;
case BARRIER_RIGHT_BEGIN:
pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f + 20;
break;
case BARRIER_RIGHT_RUNNING:
// dx_near = mid_track[aim_idx_near][1] - cx - barrier_offset;
pure_angle = -15.f;
pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f;
default:
// pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f;
if (dy > 0) {
@@ -309,11 +314,9 @@ void MidLineTrack()
// pure_angle_half = last_pure_angle_half;
}
}
if (garage_type == GARAGE_FOUND)
{
if (garage_type == GARAGE_FOUND) {
pure_angle = 0.f;
}
// if (barrier_type == BARRIER_LEFT_BEGIN) {
// // dx_near = mid_track[aim_idx_near][1] - cx + barrier_offset;
@@ -349,7 +352,7 @@ void MidLineTrack()
timer_clear(TIM_3);
timer_start(TIM_3);
}
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) {
if (circle_type == CIRCLE_LEFT_RUNNING || circle_type == CIRCLE_RIGHT_RUNNING || circle_type == CIRCLE_RIGHT_OUT || circle_type == CIRCLE_LEFT_OUT) {
if (circle_flag == 1) {
state_type = CIRCLE_STATE;
}
@@ -370,6 +373,12 @@ void MidLineTrack()
circle_flag = 1;
}
if (circle_type == CIRCLE_LEFT_END || circle_type == CIRCLE_RIGHT_END)
{
state_type = STRAIGHT_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;
}
@@ -399,4 +408,13 @@ void MidLineTrack()
// timer_clear(TIM_3);
// }
// }
if (state_type != BARRIER_STATE || garage_type != GARAGE_FOUND) {
if ((pts_left_count <= 2) && (pts_right_count <= 2) && (GET_PIX_1C(mt9v03x_image_copy[0], BEGINH_L, IMAGE_W / 2 - BEGINW_R) <= FIX_BINTHRESHOLD) && (GET_PIX_1C(mt9v03x_image_copy[0], BEGINH_L, IMAGE_W / 2 + BEGINW_L) <= FIX_BINTHRESHOLD)) {
garage_type = OUT_STOP;
}
}
else{
garage_type = GARAGE_NONE;
}
}