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

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