fix

src/tracking.cpp

@@ -11,7 +11,7 @@ bool TrackingController::isAllHighState = false;
 void TrackingController::init()
 {
   Serial.println("Tracking Init");
-  
+
   // 从存储中读取参数
   Storage::getTrackingParams(baseSpeed, turnSpeed, rotateSensitive);
 }
@@ -20,7 +20,7 @@ void TrackingController::setSpeed(uint8_t base, uint8_t turn)
 {
   baseSpeed = base;
   turnSpeed = turn;
-  
+
   // 保存参数到存储
   Storage::setTrackingParams(baseSpeed, turnSpeed, rotateSensitive);
 }
@@ -28,7 +28,7 @@ void TrackingController::setSpeed(uint8_t base, uint8_t turn)
 void TrackingController::setRotateSensitive(uint8_t sensitive)
 {
   rotateSensitive = sensitive;
-  
+
   // 保存参数到存储
   Storage::setTrackingParams(baseSpeed, turnSpeed, rotateSensitive);
 }
@@ -44,58 +44,85 @@ void TrackingController::update()
   if (offset == LOST_LINE)
   {
     // 如果是因为全高超时导致的停止
-    if (isAllHighState) {
+    if (isAllHighState)
+    {
       Serial.println("Vehicle likely lifted, stopping all motors");
       MotorController::motorXYRControl(0, 0, 0);
       return;
     }
 
     // 正常丢线处理逻辑
-    
+    Serial.println("Lost line! Rotating to search...");
+    float rotateDirection = (lastOffset != 0) ? (lastOffset > 0 ? 1.0f : -1.0f) : 1.0f;
+
     isSearching = true;
     MotorController::rotate(lastOffset < 0 ? ROTATE_CLOCKWISE : ROTATE_ANTICLOCKWISE, baseSpeed * rotateSensitive);
     return;
   }
 
   // 如果之前在搜索模式，现在找到线了，打印日志
-  if (isSearching) {
+  if (isSearching)
+  {
     Serial.println("Line found!");
     isSearching = false;
   }
 
   // 添加防抖：如果偏移量很小，认为是直线
-  if (abs(offset) < 0.2) {
+  if (abs(offset) < 0.5)
+  {
     offset = 0;
   }
 
   // 更新最后的偏移量，用于丢线时的方向判断
   lastOffset = offset;
 
-  // 直接使用偏移量控制转向，降低灵敏度
+  // 计算基础速度和转向调整
-  float correction = offset * 30.0f;  // 降低转向系数
+  float leftSpeed = baseSpeed;
-  
+  float rightSpeed = baseSpeed;
-  // 调整速度控制逻辑
-  float speedFactor;
-  if (abs(offset) > 1.5) {
-    // 大转弯时的速度因子
-    speedFactor = 0.6;
-  } else if (abs(offset) > 0.5) {
-    // 小转弯时的速度因子
-    speedFactor = 0.8;
-  } else {
-    // 直线时全速
-    speedFactor = 1.0;
-  }
-  
-  float currentSpeed = baseSpeed * speedFactor;
-  
-  // 调试信息
-  Serial.print("Offset: "); Serial.print(offset);
-  Serial.print(" Correction: "); Serial.print(correction);
-  Serial.print(" Speed: "); Serial.println(currentSpeed);
 
-  // 直接控制电机
+  if (offset != 0)
-  MotorController::motorXYRControl(0, currentSpeed, -correction);
+  {
+    // 根据偏移量调整左右轮速度
+    float speedDiff = baseSpeed * (abs(offset) / 2.0f); // 最大差速为基础速度的一半
+
+    // 增加最小速度差以确保能够转向
+    float minSpeedDiff = baseSpeed * 0.5f; // 最小速度差为基础速度的10%
+    if (speedDiff < minSpeedDiff)
+    {
+      speedDiff = minSpeedDiff;
+    }
+
+    if (offset < 0)
+    {
+      // 向左偏，左轮加速，右轮减速
+      leftSpeed += speedDiff;
+      rightSpeed -= speedDiff;
+    }
+    else
+    {
+      // 向右偏，右轮加速，左轮减速
+      leftSpeed -= speedDiff;
+      rightSpeed += speedDiff;
+    }
+
+    // 确保速度不超过限制
+    leftSpeed = constrain(leftSpeed, -baseSpeed, baseSpeed);
+    rightSpeed = constrain(rightSpeed, -baseSpeed, baseSpeed);
+  }
+
+  // 调试信息
+  Serial.print("Offset: ");
+  Serial.print(offset);
+  Serial.print(" Left: ");
+  Serial.print(leftSpeed);
+  Serial.print(" Right: ");
+  Serial.println(rightSpeed);
+
+  // 设置电机速度
+  MotorController::motorControl('A', leftSpeed);  // 左前
+  MotorController::motorControl('B', leftSpeed);  // 左后
+  MotorController::motorControl('C', rightSpeed); // 右后
+  MotorController::motorControl('D', rightSpeed); // 右前
 }
 
 // 计算偏移量，返回范围 [-2, 2]
@@ -116,30 +143,36 @@ float TrackingController::calculateOffset(const IRData &irData)
   // 检查是否所有传感器都是高电平或低电平
   for (int i = 0; i < IR_COUNT; i++)
   {
-    if (irData.data[i]) allLow = false;
+    if (irData.data[i])
-    else allHigh = false;
+      allLow = false;
+    else
+      allHigh = false;
   }
 
   // 全高状态处理
-  if (allHigh) {
+  if (allHigh)
+  {
     // 如果刚进入全高状态
-    if (!isAllHighState) {
+    if (!isAllHighState)
+    {
       isAllHighState = true;
       allHighStartTime = millis();
       Serial.println("All sensors high, continue moving forward");
-      return 0;  // 返回0表示直行
+      return 0; // 返回0表示直行
     }
-    
+
     // 检查是否超过超时时间
-    if (millis() - allHighStartTime >= ALL_HIGH_TIMEOUT) {
+    if (millis() - allHighStartTime >= ALL_HIGH_TIMEOUT)
+    {
       Serial.println("All sensors high for too long, stopping (possibly lifted)");
-      return LOST_LINE;  // 触发停止
+      return LOST_LINE; // 触发停止
     }
-    
+
-    return 0;  // 继续直行
+    return 0; // 继续直行
   }
-  
+
-  if (isAllHighState) {
+  if (isAllHighState)
+  {
     isAllHighState = false;
   }
 
@@ -153,52 +186,85 @@ float TrackingController::calculateOffset(const IRData &irData)
   int currentConsecutiveCount = 0;
   int currentConsecutiveStart = -1;
 
-  for (int i = 0; i < IR_COUNT; i++) {
+  for (int i = 0; i < IR_COUNT; i++)
-    if (irData.data[i]) {
+  {
-      if (currentConsecutiveCount == 0) {
+    if (irData.data[i])
+    {
+      if (currentConsecutiveCount == 0)
+      {
         currentConsecutiveStart = i;
       }
       currentConsecutiveCount++;
-      
+
-      if (currentConsecutiveCount > maxConsecutiveCount) {
+      if (currentConsecutiveCount > maxConsecutiveCount)
+      {
         maxConsecutiveCount = currentConsecutiveCount;
         maxConsecutiveStart = currentConsecutiveStart;
       }
-    } else {
+    }
+    else
+    {
       currentConsecutiveCount = 0;
     }
   }
 
   // 如果找到连续区域
-  if (maxConsecutiveCount > 0) {
+  if (maxConsecutiveCount > 0)
+  {
     // 计算连续区域的中心位置
     float centerPos = maxConsecutiveStart + (maxConsecutiveCount - 1) / 2.0f;
     // 将中心位置转换为偏移量
     offset = (centerPos - (IR_COUNT - 1) / 2.0f) * (4.0f / (IR_COUNT - 1));
-    
+
-    // 根据连续激活的数量调整权重
+    // 修改权重计算逻辑
     float weight = 1.0;
-    if (maxConsecutiveCount == 2) {
+    if (maxConsecutiveCount == 1)
-      weight = 1.2;  // 小幅转弯
+    {
-    } else if (maxConsecutiveCount >= 3) {
+      // 单点检测到，说明可能在边缘，需要较大的修正
-      weight = 1.5;  // 大幅转弯
+      weight = 2.0;
     }
-    
+    else if (maxConsecutiveCount == 2)
+    {
+      // 两点检测到，需要中等程度的修正
+      weight = 1.5;
+    }
+    else if (maxConsecutiveCount >= 3)
+    {
+      // 多点检测到，可能接近中心，使用较小的修正
+      weight = 1.0;
+    }
+
+    // 根据检测点的位置调整权重
+    if (maxConsecutiveStart == 0 || maxConsecutiveStart + maxConsecutiveCount >= IR_COUNT - 1)
+    {
+      // 如果在边缘位置，增加权重以加快回中
+      weight *= 1.5;
+    }
+
     offset *= weight;
-    
+
-    Serial.print("Center at sensor: "); Serial.print(centerPos);
+    Serial.print("Center at sensor: ");
-    Serial.print(" Consecutive count: "); Serial.println(maxConsecutiveCount);
+    Serial.print(centerPos);
-  } else {
+    Serial.print(" Count: ");
+    Serial.print(maxConsecutiveCount);
+    Serial.print(" Weight: ");
+    Serial.println(weight);
+  }
+  else
+  {
     // 如果没有找到连续区域，使用所有激活点的加权平均
     int activeCount = 0;
-    for (int i = 0; i < IR_COUNT; i++) {
+    for (int i = 0; i < IR_COUNT; i++)
-      if (irData.data[i]) {
+    {
+      if (irData.data[i])
+      {
         float positionOffset = (i - (IR_COUNT - 1) / 2.0f) * (4.0f / (IR_COUNT - 1));
         offset += positionOffset;
         activeCount++;
       }
     }
-    if (activeCount > 0) {
+    if (activeCount > 0)
+    {
       offset /= activeCount;
     }
   }