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-03-23 17:37:31 +08:00
parent c557c01f32
commit 872f35613e
3 changed files with 51 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
app/gl_barrier.c
View File

@@ -13,3 +13,25 @@ void CheckBarrier() {
} }
} }
void RunBarrier() {
if (barrier_type == BARRIER_LEFT_BEGIN) // 左环开始，寻外直道右线
{
track_type = TRACK_RIGHT;
}
else if (barrier_type == BARRIER_LEFT_BEGIN && ((Lpt0_found_barrier && is_straight1) || Lpt0_found) )
{
barrier_type = BARRIER_NONE;
track_type = TRACK_RIGHT;
}
if (barrier_type == BARRIER_RIGHT_BEGIN) // 左环开始，寻外直道右线
{
track_type = TRACK_LEFT;
}
else if (barrier_type == BARRIER_RIGHT_BEGIN && ((Lpt1_found_barrier && is_straight0) || Lpt1_found) )
{
barrier_type = BARRIER_NONE;
track_type = TRACK_LEFT;
}
}

1
app/gl_barrier.h
View File

@@ -14,6 +14,7 @@ enum barrier_type_e {
extern enum barrier_type_e barrier_type; extern enum barrier_type_e barrier_type;
void CheckBarrier(void); void CheckBarrier(void);
void RunBarrier(void);
#endif /* COMMON_H_ */ #endif /* COMMON_H_ */

39
app/gl_tracking.c
View File

@@ -28,6 +28,8 @@ void aim_distance_select(void)
aim_distance = 0.49f; aim_distance = 0.49f;
} else if (circle_type != CIRCLE_NONE) { } else if (circle_type != CIRCLE_NONE) {
aim_distance = 0.2f; aim_distance = 0.2f;
} else if (barrier_type != BARRIER_NONE) {
aim_distance = 0.3f;
} else { } else {
aim_distance = COMMON_AIM; aim_distance = COMMON_AIM;
} }
@@ -50,17 +52,13 @@ void ElementJudge()
cross_type = CROSS_NONE; cross_type = CROSS_NONE;
circle_type = CIRCLE_NONE; circle_type = CIRCLE_NONE;
} }
if (cross_type != CROSS_NONE) if (cross_type != CROSS_NONE) {
{
circle_type = CIRCLE_NONE; circle_type = CIRCLE_NONE;
barrier_type = BARRIER_NONE; barrier_type = BARRIER_NONE;
} }
if (barrier_type != BARRIER_NONE) if (barrier_type != BARRIER_NONE) {
{
circle_type = CIRCLE_NONE; circle_type = CIRCLE_NONE;
} }
} }
void ElementRun() void ElementRun()
@@ -75,6 +73,8 @@ void ElementRun()
else if (circle_type != CIRCLE_NONE) { else if (circle_type != CIRCLE_NONE) {
RunCircle(); RunCircle();
} else if (barrier_type != BARRIER_NONE) {
RunBarrier();
} }
} }
@@ -137,15 +137,26 @@ void MidLineTrack()
float dn = (float)Q_sqrt(dx * dx + dy * dy); float dn = (float)Q_sqrt(dx * dx + dy * dy);
// float error = -atan2f(dx, dy) * 180 / PI32; // float error = -atan2f(dx, dy) * 180 / PI32;
// 计算近锚点偏差值 if (barrier_type == BARRIER_LEFT_BEGIN) {
dx_near = rptsn[aim_idx_near][1] - cx;
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;
pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f;
} else {
dx_near = rptsn[aim_idx_near][1] - cx; dx_near = rptsn[aim_idx_near][1] - cx;
// float dy_near = cy - rptsn[aim_idx_near][0] + 0.2 * PIXPERMETER;
// float dn_near = Q_sqrt(dx_near * dx_near + dy_near * dy_near);
// float error_near = -atan2f(dx_near, dy_near) * 180 / PI32;
// //考虑近点
// near_angle = -atanf(PIXPERMETER * 2 * 0.2 * dx_near / dn_near / dn_near) / PI32 * 180 ;
// //考虑远点
pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f; pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f;
} }
// // 计算近锚点偏差值
// dx_near = rptsn[aim_idx_near][1] - cx;
// // float dy_near = cy - rptsn[aim_idx_near][0] + 0.2 * PIXPERMETER;
// // float dn_near = Q_sqrt(dx_near * dx_near + dy_near * dy_near);
// // float error_near = -atan2f(dx_near, dy_near) * 180 / PI32;
// // //考虑近点
// // near_angle = -atanf(PIXPERMETER * 2 * 0.2 * dx_near / dn_near / dn_near) / PI32 * 180 ;
// // //考虑远点
// pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f;
}
} }