282 lines
7.0 KiB
C++
282 lines
7.0 KiB
C++
#include "tracking.h"
|
|
|
|
uint8_t TrackingController::baseSpeed = 50;
|
|
uint8_t TrackingController::turnSpeed = 50;
|
|
uint8_t TrackingController::rotateSensitive = 3;
|
|
float TrackingController::lastOffset = 0;
|
|
bool TrackingController::isSearching = false;
|
|
unsigned long TrackingController::allHighStartTime = 0;
|
|
bool TrackingController::isAllHighState = false;
|
|
|
|
void TrackingController::init()
|
|
{
|
|
Serial.println("Tracking Init");
|
|
|
|
// 从存储中读取参数
|
|
Storage::getTrackingParams(baseSpeed, turnSpeed, rotateSensitive);
|
|
}
|
|
|
|
void TrackingController::setSpeed(uint8_t base, uint8_t turn)
|
|
{
|
|
baseSpeed = base;
|
|
turnSpeed = turn;
|
|
|
|
// 保存参数到存储
|
|
Storage::setTrackingParams(baseSpeed, turnSpeed, rotateSensitive);
|
|
}
|
|
|
|
void TrackingController::setRotateSensitive(uint8_t sensitive)
|
|
{
|
|
rotateSensitive = sensitive;
|
|
|
|
// 保存参数到存储
|
|
Storage::setTrackingParams(baseSpeed, turnSpeed, rotateSensitive);
|
|
}
|
|
|
|
void TrackingController::update()
|
|
{
|
|
IR::update();
|
|
|
|
const IRData &irData = IR::data;
|
|
float offset = calculateOffset(irData);
|
|
|
|
// 检查是否丢线
|
|
if (offset == LOST_LINE)
|
|
{
|
|
// 如果是因为全高超时导致的停止
|
|
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)
|
|
{
|
|
Serial.println("Line found!");
|
|
isSearching = false;
|
|
}
|
|
|
|
// 添加防抖：如果偏移量很小，认为是直线
|
|
if (abs(offset) < 0.5)
|
|
{
|
|
offset = 0;
|
|
}
|
|
|
|
// 更新最后的偏移量，用于丢线时的方向判断
|
|
lastOffset = offset;
|
|
|
|
// 计算基础速度和转向调整
|
|
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;
|
|
}
|
|
|
|
// 根据偏移量调整左右轮速度
|
|
float speedDiff = baseSpeed * (abs(offset) / 2.0f) * speedFactor; // 差速也要相应降低
|
|
|
|
// 增加最小速度差以确保能够转向
|
|
float minSpeedDiff = baseSpeed * 0.5f * speedFactor; // 最小速度差也要相应降低
|
|
if (speedDiff < minSpeedDiff)
|
|
{
|
|
speedDiff = minSpeedDiff;
|
|
}
|
|
|
|
if (offset < 0)
|
|
{
|
|
// 向左偏，左轮加速，右轮减速
|
|
leftSpeed += speedDiff;
|
|
rightSpeed -= speedDiff;
|
|
}
|
|
else
|
|
{
|
|
// 向右偏，右轮加速，左轮减速
|
|
leftSpeed -= speedDiff;
|
|
rightSpeed += speedDiff;
|
|
}
|
|
|
|
// 确保速度不超过限制
|
|
float maxSpeed = baseSpeed * speedFactor; // 最大速度也要相应降低
|
|
leftSpeed = constrain(leftSpeed, -maxSpeed, maxSpeed);
|
|
rightSpeed = constrain(rightSpeed, -maxSpeed, maxSpeed);
|
|
}
|
|
|
|
// 调试信息
|
|
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]
|
|
float TrackingController::calculateOffset(const IRData &irData)
|
|
{
|
|
float offset = 0;
|
|
bool allHigh = true;
|
|
bool allLow = true;
|
|
|
|
Serial.print("IR Data: ");
|
|
for (int i = 0; i < IR_COUNT; i++)
|
|
{
|
|
Serial.print(irData.data[i]);
|
|
Serial.print(" ");
|
|
}
|
|
Serial.println();
|
|
|
|
// 检查是否所有传感器都是高电平或低电平
|
|
for (int i = 0; i < IR_COUNT; i++)
|
|
{
|
|
if (irData.data[i])
|
|
allLow = false;
|
|
else
|
|
allHigh = false;
|
|
}
|
|
|
|
// 全高状态处理
|
|
if (allHigh)
|
|
{
|
|
// 如果刚进入全高状态
|
|
if (!isAllHighState)
|
|
{
|
|
isAllHighState = true;
|
|
allHighStartTime = millis();
|
|
Serial.println("All sensors high, continue moving forward");
|
|
return 0; // 返回0表示直行
|
|
}
|
|
|
|
// 检查是否超过超时时间
|
|
if (millis() - allHighStartTime >= ALL_HIGH_TIMEOUT)
|
|
{
|
|
Serial.println("All sensors high for too long, stopping (possibly lifted)");
|
|
return LOST_LINE; // 触发停止
|
|
}
|
|
|
|
return 0; // 继续直行
|
|
}
|
|
|
|
if (isAllHighState)
|
|
{
|
|
isAllHighState = false;
|
|
}
|
|
|
|
// 全低说明全离线
|
|
if (allLow)
|
|
return LOST_LINE;
|
|
|
|
// 寻找最长的连续激活区域
|
|
int maxConsecutiveCount = 0;
|
|
int maxConsecutiveStart = -1;
|
|
int currentConsecutiveCount = 0;
|
|
int currentConsecutiveStart = -1;
|
|
|
|
for (int i = 0; i < IR_COUNT; i++)
|
|
{
|
|
if (irData.data[i])
|
|
{
|
|
if (currentConsecutiveCount == 0)
|
|
{
|
|
currentConsecutiveStart = i;
|
|
}
|
|
currentConsecutiveCount++;
|
|
|
|
if (currentConsecutiveCount > maxConsecutiveCount)
|
|
{
|
|
maxConsecutiveCount = currentConsecutiveCount;
|
|
maxConsecutiveStart = currentConsecutiveStart;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
currentConsecutiveCount = 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 == 1)
|
|
{
|
|
// 单点检测到，说明可能在边缘，需要较大的修正
|
|
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(" Count: ");
|
|
Serial.print(maxConsecutiveCount);
|
|
Serial.print(" Weight: ");
|
|
Serial.println(weight);
|
|
}
|
|
else
|
|
{
|
|
// 如果没有找到连续区域，使用所有激活点的加权平均
|
|
int activeCount = 0;
|
|
for (int i = 0; i < IR_COUNT; i++)
|
|
{
|
|
if (irData.data[i])
|
|
{
|
|
float positionOffset = (i - (IR_COUNT - 1) / 2.0f) * (4.0f / (IR_COUNT - 1));
|
|
offset += positionOffset;
|
|
activeCount++;
|
|
}
|
|
}
|
|
if (activeCount > 0)
|
|
{
|
|
offset /= activeCount;
|
|
}
|
|
}
|
|
|
|
lastOffset = offset;
|
|
return offset;
|
|
} |