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;
void CheckBarrier(void);
void RunBarrier(void);
#endif /* COMMON_H_ */

45
app/gl_tracking.c
View File

@@ -28,6 +28,8 @@ void aim_distance_select(void)
aim_distance = 0.49f;
} else if (circle_type != CIRCLE_NONE) {
aim_distance = 0.2f;
} else if (barrier_type != BARRIER_NONE) {
aim_distance = 0.3f;
} else {
aim_distance = COMMON_AIM;
}
@@ -40,7 +42,7 @@ void ElementJudge()
CheckCross();
if (cross_type == CROSS_NONE) {
CheckBarrier();
if (barrier_type == BARRIER_NONE){
if (barrier_type == BARRIER_NONE) {
CheckCircle();
}
}
@@ -50,17 +52,13 @@ void ElementJudge()
cross_type = CROSS_NONE;
circle_type = CIRCLE_NONE;
}
if (cross_type != CROSS_NONE)
{
circle_type = CIRCLE_NONE;
if (cross_type != CROSS_NONE) {
circle_type = CIRCLE_NONE;
barrier_type = BARRIER_NONE;
}
if (barrier_type != BARRIER_NONE)
{
if (barrier_type != BARRIER_NONE) {
circle_type = CIRCLE_NONE;
}
}
void ElementRun()
@@ -75,6 +73,8 @@ void ElementRun()
else if (circle_type != CIRCLE_NONE) {
RunCircle();
} else if (barrier_type != BARRIER_NONE) {
RunBarrier();
}
}
@@ -137,15 +137,26 @@ void MidLineTrack()
float dn = (float)Q_sqrt(dx * dx + dy * dy);
// float error = -atan2f(dx, dy) * 180 / PI32;
// 计算近锚点偏差值
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;
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;
pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f;
}
// //考虑近点
// 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;
// // 计算近锚点偏差值
// 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;
}
}