fix
This commit is contained in:
玖叁
parent
0039e4fab5
commit
10c60159dc
194
src/tracking.cpp
194
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 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);
|
||||
// 计算基础速度和转向调整
|
||||
float leftSpeed = baseSpeed;
|
||||
float rightSpeed = baseSpeed;
|
||||
|
||||
// 直接控制电机
|
||||
MotorController::motorXYRControl(0, currentSpeed, -correction);
|
||||
if (offset != 0)
|
||||
{
|
||||
// 根据偏移量调整左右轮速度
|
||||
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;
|
||||
else allHigh = false;
|
||||
if (irData.data[i])
|
||||
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++) {
|
||||
if (irData.data[i]) {
|
||||
if (currentConsecutiveCount == 0) {
|
||||
for (int i = 0; i < IR_COUNT; i++)
|
||||
{
|
||||
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) {
|
||||
weight = 1.2; // 小幅转弯
|
||||
} else if (maxConsecutiveCount >= 3) {
|
||||
weight = 1.5; // 大幅转弯
|
||||
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(" Consecutive count: "); Serial.println(maxConsecutiveCount);
|
||||
} else {
|
||||
|
||||
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]) {
|
||||
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) {
|
||||
if (activeCount > 0)
|
||||
{
|
||||
offset /= activeCount;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue