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

避障bug测试

Browse Source
This commit is contained in:
Glz
2024-07-02 16:56:43 +08:00
parent cb87de5508
commit 047a907eac
2 changed files with 96 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
app/gl_tracking.c
View File

@@ -270,12 +270,19 @@ void MidLineTrack()
float dy = cy - rptsn[aim_idx][0]; // + 0.2f * PIXPERMETER;
float dn = (dx * dx + dy * dy);
float temp_near = 0;
if (barrier_type == BARRIER_LEFT_BEGIN || barrier_type == BARRIER_LEFT_RUNNING) {
dx_near = mid_track[aim_idx_near][1] - cx + barrier_offset;
pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn) / PI32 * 180.0f - 25;
} else if (barrier_type == BARRIER_RIGHT_BEGIN || barrier_type == BARRIER_RIGHT_RUNNING) {
dx_near = mid_track[aim_idx_near][1] - cx - barrier_offset;
pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn) / PI32 * 180.0f + 25;
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) {