From 2453391b57009ee37b82808f80de31f7e15464e4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E7=8E=96=E5=8F=81?= <colour_93@furry.top>
Date: Thu, 2 Jan 2025 01:05:02 +0800
Subject: [PATCH] feat: turn dec speed

---
 src/tracking.cpp | 22 +++++++++++++++-------
 1 file changed, 15 insertions(+), 7 deletions(-)

diff --git a/src/tracking.cpp b/src/tracking.cpp
index 4bbc2b3..e3b0ea8 100644
--- a/src/tracking.cpp
+++ b/src/tracking.cpp
@@ -79,14 +79,21 @@ void TrackingController::update()
   // 计算基础速度和转向调整
   float leftSpeed = baseSpeed;
   float rightSpeed = baseSpeed;
+  
+  if (offset != 0) {
+    // 在大偏移时降低基础速度
+    float speedFactor = 1.0f;
+    if (abs(offset) > 1.0f) {
+      speedFactor = 0.7f;
+      leftSpeed *= speedFactor;
+      rightSpeed *= speedFactor;
+    }
 
-  if (offset != 0)
-  {
     // 根据偏移量调整左右轮速度
-    float speedDiff = baseSpeed * (abs(offset) / 2.0f); // 最大差速为基础速度的一半
-
+    float speedDiff = baseSpeed * (abs(offset) / 2.0f) * speedFactor;  // 差速也要相应降低
+    
     // 增加最小速度差以确保能够转向
-    float minSpeedDiff = baseSpeed * 0.5f; // 最小速度差为基础速度的10%
+    float minSpeedDiff = baseSpeed * 0.5f * speedFactor; // 最小速度差也要相应降低
     if (speedDiff < minSpeedDiff)
     {
       speedDiff = minSpeedDiff;
@@ -106,8 +113,9 @@ void TrackingController::update()
     }
 
     // 确保速度不超过限制
-    leftSpeed = constrain(leftSpeed, -baseSpeed, baseSpeed);
-    rightSpeed = constrain(rightSpeed, -baseSpeed, baseSpeed);
+    float maxSpeed = baseSpeed * speedFactor;  // 最大速度也要相应降低
+    leftSpeed = constrain(leftSpeed, -maxSpeed, maxSpeed);
+    rightSpeed = constrain(rightSpeed, -maxSpeed, maxSpeed);
   }
 
   // 调试信息