From 0701276b4fec856d5427e4776eec3cc7c56ec065 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期二, 25 八月 2020 17:51:10 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_items2/drive_straight.cpp | 13 +++++++------
1 files changed, 7 insertions(+), 6 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/drive_straight.cpp b/lib/src/main/cpp/test_items2/drive_straight.cpp
index 94ba625..04ec8fa 100644
--- a/lib/src/main/cpp/test_items2/drive_straight.cpp
+++ b/lib/src/main/cpp/test_items2/drive_straight.cpp
@@ -39,17 +39,18 @@
if (setup == 1) {
// 鍋忚埅瑙掑钩鍧囧��
+ static vector<double> yaws;
+
if (yaw_stat == 0) {
- yaw = car->yaw;
- } else {
- yaw += car->yaw;
+ yaws.clear();
}
+ yaws.push_back(car->yaw);
yaw_stat++;
- DEBUG("瑙掑害鐭 car %f yaw %f", car->yaw, yaw);
-
if (yaw_stat == 5) {
- yaw = fmod(yaw, 360) / 5;
+ yaw = AvgYaw(yaws);
+ vector<double>().swap(yaws);
+
DEBUG("鍒涘缓鍩虹嚎 yaw %f", yaw);
PointF extPoint = PointExtend(car->basePoint, 100, yaw);
MakeLine(&baseLine, &car->basePoint, &extPoint);
--
Gitblit v1.8.0