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-08-10 21:02:20 +08:00
parent 5f7b200854
commit 30c9ee1f76
2 changed files with 49 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
app/gl_cross.c
View File

@@ -46,8 +46,8 @@ void RunCross() {
}
if (far_Lpt1_found) { track_type = TRACK_RIGHT; }
else if (far_Lpt0_found) { track_type = TRACK_LEFT; }
else if (Both_Boder_None_Cross > 0 && pts_far_resample_right_count < 5) { track_type = TRACK_RIGHT; }
else if (Both_Boder_None_Cross > 0 && pts_far_resample_left_count < 5) { track_type = TRACK_LEFT; }
else if (Both_Boder_None_Cross > 0 && pts_resample_right_count < 5) { track_type = TRACK_RIGHT; }
else if (Both_Boder_None_Cross > 0 && pts_resample_left_count < 5) { track_type = TRACK_LEFT; }
}
}

108
app/gl_tracking.c
View File

@@ -15,6 +15,8 @@ int rpts_num;
float last_pure_angle = 0.0f;
int8_t turn_flag = 0;
int circle_flag = 1;
int pingbi_num = 0;
int last_garage_type = GARAGE_NONE;
// 计算最小二乘法斜率的函数
float leastSquaresSlope(float points[][2], int n)
{
@@ -113,12 +115,6 @@ void aim_distance_select(void)
void ElementJudge()
{
// if(begin_flag == 1){
// shield_type = SHIELD_BEGIN;
// begin_flag = 0;
// return;
// }
if (shield_type == SHIELD_NONE) {
CheckGarage();
if (garage_type == GARAGE_NONE) {
@@ -139,16 +135,13 @@ 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;
}
}
@@ -186,8 +179,7 @@ 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 if (barrier_type == BARRIER_LEFT_RUNNING || barrier_type == BARRIER_RIGHT_RUNNING)
{
} 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;
@@ -195,7 +187,7 @@ void MidLineTrack()
mid_track = mid_right_barrier;
mid_track_count = mid_right_barrier_count;
}
}else {
} else {
if (track_type == TRACK_LEFT) {
mid_track = mid_left;
mid_track_count = mid_left_count;
@@ -317,28 +309,11 @@ void MidLineTrack()
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;
// pure_angle = -25.f;
// } else if (barrier_type == BARRIER_LEFT_RUNNING) {
// pure_angle = 25.f;
// } else if (barrier_type == BARRIER_RIGHT_BEGIN) {
// pure_angle = 25.f;
// } else if (barrier_type == BARRIER_RIGHT_RUNNING) {
// // dx_near = mid_track[aim_idx_near][1] - cx - barrier_offset;
// pure_angle = -25.f;
// } else {
// // pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f;
// if (dy > 0) {
// pure_angle = -atanf(dx / dy) / PI32 * 180.0f;
// last_pure_angle = pure_angle;
// // last_pure_angle_half = pure_angle_half;
// } else {
// pure_angle = last_pure_angle;
// // pure_angle_half = last_pure_angle_half;
// }
// }
if (cross_type == CROSS_IN)
{
pure_angle = -atanf(dx / (dy / 2)) / PI32 * 180.0f;
}
// // 计算近锚点偏差值
// dx_near = rptsn[aim_idx_near][1] - cx;
@@ -373,11 +348,9 @@ void MidLineTrack()
circle_flag = 1;
}
if (circle_type == CIRCLE_LEFT_END || circle_type == CIRCLE_RIGHT_END)
{
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;
@@ -386,35 +359,48 @@ void MidLineTrack()
if (cross_type == CROSS_BEGIN || cross_type == CROSS_IN) {
state_type = STRAIGHT_STATE;
}
// last_state = state_type;
if (state_type == STRAIGHT_STATE) {
aim_distance = straight_aim;
} else if (state_type == TURN_STATE) {
aim_distance = turn_aim;
}
// if (last_state == STRAIGHT_STATE && (state_type == TURN_STATE || state_type == CIRCLE_STATE)) {
// turn_flag = 1;
// timer_clear(TIM_3);
// timer_start(TIM_3);
// }
// if (turn_flag == 1) {
// aim_distance = turn_aim;
// uint16 ti = timer_get(TIM_3);
// if (ti >= 2000) {
// turn_flag = 0;
// timer_stop(TIM_3);
// 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;
}
if (cross_type == CROSS_IN)
{
aim_distance = cross_aim;
}
else{
garage_type = GARAGE_NONE;
if (garage_type == GARAGE_FOUND) {
last_garage_type = GARAGE_FOUND;
}
if (last_garage_type == GARAGE_FOUND) {
pingbi_num++;
if (pingbi_num >= 300)
{
pingbi_num = 300;
}
if (pingbi_num >= 100) {
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{
if (garage_type == OUT_STOP)
{
garage_type = GARAGE_FOUND;
}
}
} else {
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;
}
}
}
}