From fb7b0660a319a9c54ff35c3944548348fae11b60 Mon Sep 17 00:00:00 2001 From: yy1717 <fctom1215@outlook.com> Date: 星期一, 24 八月 2020 18:53:23 +0800 Subject: [PATCH] 坐标 --- lib/src/main/cpp/test_items2/drive_straight.cpp | 11 ++++++++--- 1 files changed, 8 insertions(+), 3 deletions(-) diff --git a/lib/src/main/cpp/test_items2/drive_straight.cpp b/lib/src/main/cpp/test_items2/drive_straight.cpp index 8c69204..94ba625 100644 --- a/lib/src/main/cpp/test_items2/drive_straight.cpp +++ b/lib/src/main/cpp/test_items2/drive_straight.cpp @@ -45,17 +45,22 @@ yaw += car->yaw; } yaw_stat++; + + DEBUG("瑙掑害鐭 car %f yaw %f", car->yaw, yaw); + if (yaw_stat == 5) { yaw = fmod(yaw, 360) / 5; - + DEBUG("鍒涘缓鍩虹嚎 yaw %f", yaw); PointF extPoint = PointExtend(car->basePoint, 100, yaw); MakeLine(&baseLine, &car->basePoint, &extPoint); beginOdo = ReadOdo(); setup = 2; } } else if (setup == 2) { - if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], baseLine) > MAX_OFFSET_DISTANCE) { - DEBUG("鐩寸嚎鍋忕Щ澶т簬30鍘樼背"); + double offset = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], baseLine); + + if (offset > MAX_OFFSET_DISTANCE) { + DEBUG("鐩寸嚎鍋忕Щ澶т簬30鍘樼背 offset = %f", offset); // 鍋忕Щ澶т簬30鍘樼背锛屼笉鍚堟牸 AddExamFault(30, rtkTime); return false; -- Gitblit v1.8.0