feat: tracking
This commit is contained in:
玖叁
parent
80f7f8f234
commit
91c4500d82
|
|
@ -8,6 +8,58 @@
|
|||
"files.associations": {
|
||||
"*.toml": "toml",
|
||||
"Comfyfile": "raw",
|
||||
"*.tcc": "cpp"
|
||||
"*.tcc": "cpp",
|
||||
"array": "cpp",
|
||||
"atomic": "cpp",
|
||||
"cctype": "cpp",
|
||||
"chrono": "cpp",
|
||||
"clocale": "cpp",
|
||||
"cmath": "cpp",
|
||||
"condition_variable": "cpp",
|
||||
"cstdarg": "cpp",
|
||||
"cstddef": "cpp",
|
||||
"cstdint": "cpp",
|
||||
"cstdio": "cpp",
|
||||
"cstdlib": "cpp",
|
||||
"ctime": "cpp",
|
||||
"cwchar": "cpp",
|
||||
"cwctype": "cpp",
|
||||
"deque": "cpp",
|
||||
"unordered_map": "cpp",
|
||||
"unordered_set": "cpp",
|
||||
"vector": "cpp",
|
||||
"exception": "cpp",
|
||||
"algorithm": "cpp",
|
||||
"functional": "cpp",
|
||||
"iterator": "cpp",
|
||||
"map": "cpp",
|
||||
"memory": "cpp",
|
||||
"memory_resource": "cpp",
|
||||
"numeric": "cpp",
|
||||
"optional": "cpp",
|
||||
"random": "cpp",
|
||||
"ratio": "cpp",
|
||||
"string": "cpp",
|
||||
"string_view": "cpp",
|
||||
"system_error": "cpp",
|
||||
"tuple": "cpp",
|
||||
"type_traits": "cpp",
|
||||
"utility": "cpp",
|
||||
"fstream": "cpp",
|
||||
"initializer_list": "cpp",
|
||||
"iomanip": "cpp",
|
||||
"iosfwd": "cpp",
|
||||
"iostream": "cpp",
|
||||
"istream": "cpp",
|
||||
"limits": "cpp",
|
||||
"mutex": "cpp",
|
||||
"new": "cpp",
|
||||
"ostream": "cpp",
|
||||
"sstream": "cpp",
|
||||
"stdexcept": "cpp",
|
||||
"streambuf": "cpp",
|
||||
"thread": "cpp",
|
||||
"cinttypes": "cpp",
|
||||
"typeinfo": "cpp"
|
||||
}
|
||||
}
|
||||
|
|
@ -19,6 +19,9 @@
|
|||
|
||||
#define CMD_SET_NAME 0xA1
|
||||
#define CMD_SET_PID 0xA2
|
||||
#define CMD_SET_TRACKING_SPEED 0xA3
|
||||
#define CMD_SET_TRACKING_SENSITIVE 0xA4
|
||||
|
||||
#define CMD_MOTOR_MOVE_CONTROL 0x20
|
||||
#define CMD_MOTOR_STEER_CONTROL 0x21
|
||||
#define CMD_MOTOR_SINGLE_CONTROL 0x22
|
||||
|
|
@ -46,6 +49,8 @@ public:
|
|||
static void motorXYRControl(BLECharacteristic &characteristic, uint8_t *packet);
|
||||
static void modeControl(BLECharacteristic &characteristic, uint8_t *packet);
|
||||
static void demoPID(BLECharacteristic &characteristic, uint8_t *packet);
|
||||
static void setTrackingSensitive(BLECharacteristic &characteristic, uint8_t *packet);
|
||||
static void setTrackingSpeed(BLECharacteristic &characteristic, uint8_t *packet);
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
@ -1,14 +1,15 @@
|
|||
#ifndef IR_H
|
||||
#define IR_H
|
||||
|
||||
#include <SoftwareSerial.h>
|
||||
#include <Wire.h>
|
||||
#include <Arduino.h>
|
||||
#include <algorithm>
|
||||
#include "consts.h"
|
||||
|
||||
enum IRMode
|
||||
{
|
||||
IR_MODE_GPIO,
|
||||
IR_MODE_UART
|
||||
IR_MODE_I2C
|
||||
};
|
||||
|
||||
struct IRData
|
||||
|
|
@ -22,7 +23,7 @@ struct IRData
|
|||
class IR
|
||||
{
|
||||
public:
|
||||
static SoftwareSerial *uart;
|
||||
static TwoWire *i2c;
|
||||
static IRData data;
|
||||
static void init();
|
||||
static void update();
|
||||
|
|
|
|||
|
|
@ -13,6 +13,7 @@ class Mode
|
|||
{
|
||||
public:
|
||||
static ModeType mode;
|
||||
static ModeType lastMode;
|
||||
static void processDeviceMode();
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -19,6 +19,11 @@ struct MotorStatus
|
|||
unsigned char pwm;
|
||||
};
|
||||
|
||||
typedef enum {
|
||||
ROTATE_CLOCKWISE = 1,
|
||||
ROTATE_ANTICLOCKWISE = -1,
|
||||
} RotateDirection;
|
||||
|
||||
class MotorController
|
||||
{
|
||||
public:
|
||||
|
|
@ -26,6 +31,7 @@ public:
|
|||
static void pwmControl(char motor, unsigned char pwm);
|
||||
static void motorControl(char motor, int speed);
|
||||
static void motorXYRControl(int8_t x, int8_t y, int8_t r);
|
||||
static void rotate(RotateDirection direction, uint8_t speed);
|
||||
|
||||
// 获取电机状态
|
||||
static MotorStatus getMotorStatus(char motor);
|
||||
|
|
|
|||
|
|
@ -16,6 +16,8 @@ public:
|
|||
static unsigned int getSensitivity();
|
||||
static void setName(String name);
|
||||
static String getName();
|
||||
static void setTrackingParams(uint8_t baseSpeed, uint8_t turnSpeed, uint8_t rotateSensitive);
|
||||
static void getTrackingParams(uint8_t &baseSpeed, uint8_t &turnSpeed, uint8_t &rotateSensitive);
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -4,10 +4,14 @@
|
|||
#include "ir.h"
|
||||
#include "motor.h"
|
||||
#include "pid.h"
|
||||
#include "storage.h"
|
||||
|
||||
class TrackingController {
|
||||
public:
|
||||
static uint8_t baseSpeed; // 基础前进速度
|
||||
static uint8_t turnSpeed; // 转弯速度
|
||||
static uint8_t rotateSensitive; // 旋转灵敏度
|
||||
static float lastOffset; // 上一次的偏移量
|
||||
|
||||
// 初始化循迹控制器
|
||||
static void init();
|
||||
|
|
@ -18,11 +22,18 @@ public:
|
|||
// 设置循迹速度
|
||||
static void setSpeed(uint8_t baseSpeed, uint8_t turnSpeed);
|
||||
|
||||
// 设置旋转灵敏度
|
||||
static void setRotateSensitive(uint8_t sensitive);
|
||||
|
||||
// 计算偏移量
|
||||
static float calculateOffset(const IRData& irData);
|
||||
|
||||
private:
|
||||
static constexpr float LOST_LINE = 999.0f; // 使用一个特殊值表示丢线状态
|
||||
static bool isSearching; // 标记是否处于搜索模式
|
||||
static unsigned long allHighStartTime; // 记录全高状态开始的时间
|
||||
static bool isAllHighState; // 记录是否处于全高状态
|
||||
static constexpr unsigned long ALL_HIGH_TIMEOUT = 1000; // 全高状态超时时间（1秒）
|
||||
};
|
||||
|
||||
#endif
|
||||
14
packet.md
14
packet.md
|
|
@ -125,7 +125,7 @@
|
|||
|
||||
包体 `00 05 30 模式 FF`
|
||||
|
||||
控制示例 `00 05 30 01 FE`
|
||||
控制示例 `00 05 30 01 FF`
|
||||
|
||||
## 设置
|
||||
|
||||
|
|
@ -147,6 +147,18 @@
|
|||
|
||||
设置示例 `00 11 A2 01 01 01 01 01 01 01 01 01 01 01 01 01 FF`
|
||||
|
||||
### 设置循迹速度 `0xA3`
|
||||
|
||||
包体 `00 06 A3 基础速度 转弯速度 FF`
|
||||
|
||||
设置示例 `00 06 A3 50 50 FF`
|
||||
|
||||
### 设置循迹转弯灵敏度 `0xA4`
|
||||
|
||||
包体 `00 05 A4 灵敏度 FF`
|
||||
|
||||
设置示例 `00 05 A4 03 FF`
|
||||
|
||||
## 状态回报
|
||||
|
||||
### 设备状态回报 `0xE0`
|
||||
|
|
|
|||
|
|
@ -129,18 +129,12 @@ void Handlers::motorRotateControl(BLECharacteristic &characteristic, uint8_t *pa
|
|||
// 顺时针
|
||||
if (direction == 0x00)
|
||||
{
|
||||
MotorController::motorControl('A', 255);
|
||||
MotorController::motorControl('B', 255);
|
||||
MotorController::motorControl('C', -255);
|
||||
MotorController::motorControl('D', -255);
|
||||
MotorController::rotate(ROTATE_CLOCKWISE, 255);
|
||||
}
|
||||
// 逆时针
|
||||
else if (direction == 0x01)
|
||||
{
|
||||
MotorController::motorControl('A', -255);
|
||||
MotorController::motorControl('B', -255);
|
||||
MotorController::motorControl('C', 255);
|
||||
MotorController::motorControl('D', 255);
|
||||
MotorController::rotate(ROTATE_ANTICLOCKWISE, 255);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -194,6 +188,21 @@ void Handlers::modeControl(BLECharacteristic &characteristic, uint8_t *packet)
|
|||
}
|
||||
}
|
||||
|
||||
// 设置循迹转弯灵敏度
|
||||
void Handlers::setTrackingSensitive(BLECharacteristic &characteristic, uint8_t *packet)
|
||||
{
|
||||
uint8_t sensitive = packet[3];
|
||||
TrackingController::setRotateSensitive(sensitive);
|
||||
}
|
||||
|
||||
// 设置循迹速度
|
||||
void Handlers::setTrackingSpeed(BLECharacteristic &characteristic, uint8_t *packet)
|
||||
{
|
||||
uint8_t baseSpeed = packet[3];
|
||||
uint8_t turnSpeed = packet[4];
|
||||
TrackingController::setSpeed(baseSpeed, turnSpeed);
|
||||
}
|
||||
|
||||
void Handlers::demoPID(BLECharacteristic &characteristic, uint8_t *packet)
|
||||
{
|
||||
Serial.println("CMD_DEMO_PID");
|
||||
|
|
|
|||
70
src/ir.cpp
70
src/ir.cpp
|
|
@ -1,7 +1,7 @@
|
|||
#include "ir.h"
|
||||
|
||||
IRData IR::data;
|
||||
SoftwareSerial *IR::uart;
|
||||
TwoWire *IR::i2c;
|
||||
|
||||
void IR::init()
|
||||
{
|
||||
|
|
@ -16,13 +16,23 @@ void IR::init()
|
|||
data.pins[i] = pins[i];
|
||||
};
|
||||
}
|
||||
else if (IR_MODE == IRMode::IR_MODE_UART)
|
||||
else if (IR_MODE == IRMode::IR_MODE_I2C)
|
||||
{
|
||||
uint8_t pins[2] = IR_PINS;
|
||||
data.pins[0] = pins[0];
|
||||
data.pins[1] = pins[1];
|
||||
uart = new SoftwareSerial(pins[0], pins[1]);
|
||||
uart->begin(IR_BAUD);
|
||||
data.pins[0] = pins[0]; // SCL
|
||||
data.pins[1] = pins[1]; // SDA
|
||||
|
||||
// 检查引脚值是否有效
|
||||
if (data.pins[0] >= 0 && data.pins[1] >= 0) {
|
||||
i2c = new TwoWire(0);
|
||||
if (i2c->begin(data.pins[1], data.pins[0])) {
|
||||
Serial.println("I2C initialized successfully");
|
||||
} else {
|
||||
Serial.println("I2C initialization failed");
|
||||
}
|
||||
} else {
|
||||
Serial.println("Invalid I2C pins");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -32,14 +42,52 @@ void IR::update()
|
|||
{
|
||||
for (int i = 0; i < data.count; i++)
|
||||
{
|
||||
data.data[i] = digitalRead(data.pins[i]);
|
||||
// 读取传感器数据
|
||||
bool value = digitalRead(data.pins[i]);
|
||||
|
||||
// 根据 IR_REVERSE 判断是否需要反转电平
|
||||
#if IR_OUTPUT_REVERSE
|
||||
value = !value;
|
||||
#endif
|
||||
|
||||
// 根据传感器排列方向存储数据
|
||||
#ifdef IR_DIRECTION_REVERSE
|
||||
data.data[data.count - 1 - i] = value;
|
||||
#else
|
||||
data.data[i] = value;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
else if (data.mode == IRMode::IR_MODE_UART)
|
||||
else if (data.mode == IRMode::IR_MODE_I2C && i2c != nullptr)
|
||||
{
|
||||
if (uart->available())
|
||||
{
|
||||
data.data[0] = uart->read();
|
||||
byte value = 0;
|
||||
|
||||
delay(10);
|
||||
|
||||
i2c->requestFrom(0x12, 1);
|
||||
if(i2c->available()) {
|
||||
value = i2c->read();
|
||||
|
||||
// 根据 IR_COUNT 读取数据
|
||||
for(int i = 0; i < std::min<int>(data.count, IR_COUNT); i++) {
|
||||
// 读取传感器数据
|
||||
bool sensorValue = !((value >> (7-i)) & 0x01);
|
||||
|
||||
// 根据 IR_REVERSE 判断是否需要反转电平
|
||||
#if IR_OUTPUT_REVERSE
|
||||
sensorValue = !sensorValue;
|
||||
#endif
|
||||
|
||||
// 根据传感器排列方向存储数据
|
||||
#ifdef IR_DIRECTION_REVERSE
|
||||
data.data[std::min<int>(data.count, IR_COUNT) - 1 - i] = sensorValue;
|
||||
#else
|
||||
data.data[i] = sensorValue;
|
||||
#endif
|
||||
}
|
||||
} else {
|
||||
// 通信失败时清零数据
|
||||
memset(data.data, 0, IR_COUNT);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -8,4 +8,10 @@ void Mode::processDeviceMode()
|
|||
{
|
||||
TrackingController::update();
|
||||
}
|
||||
if (lastMode != mode)
|
||||
{
|
||||
lastMode = mode;
|
||||
MotorController::motorXYRControl(0, 0, 0);
|
||||
Serial.println("Mode changed to: " + String(mode));
|
||||
}
|
||||
}
|
||||
|
|
@ -176,34 +176,26 @@ void MotorController::motorXYRControl(int8_t x, int8_t y, int8_t r)
|
|||
motorControl('D', speedD);
|
||||
}
|
||||
|
||||
void MotorController::rotate(RotateDirection direction, uint8_t speed)
|
||||
{
|
||||
motorControl('A', direction == ROTATE_CLOCKWISE ? speed : -speed);
|
||||
motorControl('B', direction == ROTATE_CLOCKWISE ? speed : -speed);
|
||||
motorControl('C', direction == ROTATE_CLOCKWISE ? -speed : speed);
|
||||
motorControl('D', direction == ROTATE_CLOCKWISE ? -speed : speed);
|
||||
}
|
||||
|
||||
MotorStatus MotorController::getMotorStatus(char motor)
|
||||
{
|
||||
switch (motor)
|
||||
{
|
||||
case 'A':
|
||||
return {
|
||||
digitalRead(motorA.pin.in1),
|
||||
digitalRead(motorA.pin.in2),
|
||||
motorA.pwm,
|
||||
};
|
||||
return motorA;
|
||||
case 'B':
|
||||
return {
|
||||
digitalRead(motorB.pin.in1),
|
||||
digitalRead(motorB.pin.in2),
|
||||
motorB.pwm,
|
||||
};
|
||||
return motorB;
|
||||
case 'C':
|
||||
return {
|
||||
digitalRead(motorC.pin.in1),
|
||||
digitalRead(motorC.pin.in2),
|
||||
motorC.pwm,
|
||||
};
|
||||
return motorC;
|
||||
case 'D':
|
||||
return {
|
||||
digitalRead(motorD.pin.in1),
|
||||
digitalRead(motorD.pin.in2),
|
||||
motorD.pwm,
|
||||
};
|
||||
return motorD;
|
||||
default:
|
||||
return {false, false, 0};
|
||||
}
|
||||
|
|
|
|||
|
|
@ -86,3 +86,38 @@ String Storage::getName()
|
|||
|
||||
return name;
|
||||
}
|
||||
|
||||
void Storage::setTrackingParams(uint8_t baseSpeed, uint8_t turnSpeed, uint8_t rotateSensitive)
|
||||
{
|
||||
if (!isMounted)
|
||||
return;
|
||||
|
||||
File file = SPIFFS.open("/tracking.txt", "w");
|
||||
file.println(baseSpeed);
|
||||
file.println(turnSpeed);
|
||||
file.println(rotateSensitive);
|
||||
file.close();
|
||||
}
|
||||
|
||||
void Storage::getTrackingParams(uint8_t &baseSpeed, uint8_t &turnSpeed, uint8_t &rotateSensitive)
|
||||
{
|
||||
if (!isMounted) {
|
||||
baseSpeed = 50;
|
||||
turnSpeed = 50;
|
||||
rotateSensitive = 3;
|
||||
return;
|
||||
}
|
||||
|
||||
File file = SPIFFS.open("/tracking.txt", "r");
|
||||
if (!file) {
|
||||
baseSpeed = 50;
|
||||
turnSpeed = 50;
|
||||
rotateSensitive = 3;
|
||||
return;
|
||||
}
|
||||
|
||||
baseSpeed = file.readStringUntil('\n').toInt();
|
||||
turnSpeed = file.readStringUntil('\n').toInt();
|
||||
rotateSensitive = file.readStringUntil('\n').toInt();
|
||||
file.close();
|
||||
}
|
||||
|
|
|
|||
188
src/tracking.cpp
188
src/tracking.cpp
|
|
@ -1,17 +1,36 @@
|
|||
#include "tracking.h"
|
||||
|
||||
uint8_t TrackingController::baseSpeed = 10; // 默认速度50%
|
||||
uint8_t TrackingController::turnSpeed = 10; // 默认转弯速度30%
|
||||
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()
|
||||
|
|
@ -21,66 +40,169 @@ void TrackingController::update()
|
|||
const IRData &irData = IR::data;
|
||||
float offset = calculateOffset(irData);
|
||||
|
||||
Serial.print("Offset: ");
|
||||
Serial.println(offset);
|
||||
|
||||
// 检查是否丢线
|
||||
if (offset == LOST_LINE)
|
||||
{ // 需要定义 LOST_LINE 常量
|
||||
// 丢线时停止移动
|
||||
Serial.println("Lost line!");
|
||||
{
|
||||
// 如果是因为全高超时导致的停止
|
||||
if (isAllHighState) {
|
||||
Serial.println("Vehicle likely lifted, stopping all motors");
|
||||
MotorController::motorXYRControl(0, 0, 0);
|
||||
return;
|
||||
}
|
||||
|
||||
// 正常巡线控制
|
||||
float correction = PIDController::update(0, offset);
|
||||
MotorController::motorXYRControl(0, baseSpeed, -correction);
|
||||
// 正常丢线处理逻辑
|
||||
|
||||
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.2) {
|
||||
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);
|
||||
|
||||
// 直接控制电机
|
||||
MotorController::motorXYRControl(0, currentSpeed, -correction);
|
||||
}
|
||||
|
||||
// 计算偏移量，返回范围 [-2, 2]
|
||||
float TrackingController::calculateOffset(const IRData &irData)
|
||||
{
|
||||
float offset = 0;
|
||||
int activeCount = 0;
|
||||
bool allHigh = true;
|
||||
bool allLow = true;
|
||||
|
||||
Serial.print("IR Data: ");
|
||||
for (int i = 0; i < irData.length; i++) {
|
||||
for (int i = 0; i < IR_COUNT; i++)
|
||||
{
|
||||
Serial.print(irData.data[i]);
|
||||
Serial.print(" ");
|
||||
}
|
||||
Serial.println();
|
||||
|
||||
// 检查是否所有传感器都是高电平或低电平
|
||||
for (int i = 0; i < irData.length; i++) {
|
||||
if (irData.data[i]) {
|
||||
allLow = false;
|
||||
} else {
|
||||
allHigh = false;
|
||||
}
|
||||
for (int i = 0; i < IR_COUNT; i++)
|
||||
{
|
||||
if (irData.data[i]) allLow = false;
|
||||
else allHigh = false;
|
||||
}
|
||||
|
||||
// 如果所有传感器都是高电平或低电平，说明不在线上
|
||||
if (allHigh || allLow) {
|
||||
// 全高状态处理
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
// 计算加权平均偏移
|
||||
for (int i = 0; i < irData.length; i++)
|
||||
{
|
||||
if (irData.data[i])
|
||||
{
|
||||
// 将传感器位置映射到 [-2, 2] 范围
|
||||
offset += (i - (irData.length - 1) / 2.0f) * (4.0f / (irData.length - 1));
|
||||
// 如果找到连续区域
|
||||
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; // 大幅转弯
|
||||
}
|
||||
|
||||
offset *= weight;
|
||||
|
||||
Serial.print("Center at sensor: "); Serial.print(centerPos);
|
||||
Serial.print(" Consecutive count: "); Serial.println(maxConsecutiveCount);
|
||||
} 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;
|
||||
}
|
||||
}
|
||||
|
||||
// 如果有传感器检测到线，返回平均偏移
|
||||
if (activeCount > 0)
|
||||
return offset / activeCount;
|
||||
|
||||
return LOST_LINE;
|
||||
lastOffset = offset;
|
||||
return offset;
|
||||
}
|
||||
|
|
@ -78,6 +78,14 @@ void handleSerialPacket(uint8_t *packet, int length, BLECharacteristic &characte
|
|||
Handlers::setPID(characteristic, packet);
|
||||
break;
|
||||
|
||||
case CMD_SET_TRACKING_SENSITIVE:
|
||||
Handlers::setTrackingSensitive(characteristic, packet);
|
||||
break;
|
||||
|
||||
case CMD_SET_TRACKING_SPEED:
|
||||
Handlers::setTrackingSpeed(characteristic, packet);
|
||||
break;
|
||||
|
||||
case CMD_MOTOR_MOVE_CONTROL:
|
||||
Handlers::motorMoveControl(characteristic, packet);
|
||||
break;
|
||||
|
|
@ -132,9 +140,9 @@ void sendStatus()
|
|||
buffer[bufferIndex++] = MotorController::getMotorStatus('C').pwm;
|
||||
buffer[bufferIndex++] = (MotorController::getMotorStatus('D').in2 << 1) | MotorController::getMotorStatus('D').in1;
|
||||
buffer[bufferIndex++] = MotorController::getMotorStatus('D').pwm;
|
||||
buffer[bufferIndex++] = IR::data.length;
|
||||
buffer[bufferIndex++] = IR_COUNT;
|
||||
uint16_t irData = 0;
|
||||
for (uint8_t i = 0; i < IR::data.length; i++)
|
||||
for (uint8_t i = 0; i < IR_COUNT; i++)
|
||||
{
|
||||
irData |= (IR::data.data[i] << i);
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue