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