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