From 91c4500d82a8480f2efebe3bcd1d52738259cfc6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E7=8E=96=E5=8F=81?= Date: Wed, 1 Jan 2025 17:53:59 +0800 Subject: [PATCH] feat: tracking --- .vscode/settings.json | 54 +++++++++++- include/handlers.h | 5 ++ include/ir.h | 7 +- include/mode.h | 1 + include/motor.h | 6 ++ include/storage.h | 2 + include/tracking.h | 11 +++ packet.md | 14 ++- src/handlers.cpp | 25 ++++-- src/ir.cpp | 70 ++++++++++++--- src/mode.cpp | 6 ++ src/motor.cpp | 32 +++---- src/storage.cpp | 35 ++++++++ src/tracking.cpp | 198 ++++++++++++++++++++++++++++++++++-------- src/transmit.cpp | 12 ++- 15 files changed, 394 insertions(+), 84 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 88844b9..181f2fc 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -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" } } \ No newline at end of file diff --git a/include/handlers.h b/include/handlers.h index 512aba0..6a26b2c 100644 --- a/include/handlers.h +++ b/include/handlers.h @@ -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 \ No newline at end of file diff --git a/include/ir.h b/include/ir.h index 0cfecb8..d7171b0 100644 --- a/include/ir.h +++ b/include/ir.h @@ -1,14 +1,15 @@ #ifndef IR_H #define IR_H -#include +#include #include +#include #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(); diff --git a/include/mode.h b/include/mode.h index f1329bd..f6a8aed 100644 --- a/include/mode.h +++ b/include/mode.h @@ -13,6 +13,7 @@ class Mode { public: static ModeType mode; + static ModeType lastMode; static void processDeviceMode(); }; diff --git a/include/motor.h b/include/motor.h index db2087c..4fa0eb3 100644 --- a/include/motor.h +++ b/include/motor.h @@ -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); diff --git a/include/storage.h b/include/storage.h index a11cf84..55f1689 100644 --- a/include/storage.h +++ b/include/storage.h @@ -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 diff --git a/include/tracking.h b/include/tracking.h index 78ac8c3..31586fa 100644 --- a/include/tracking.h +++ b/include/tracking.h @@ -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 \ No newline at end of file diff --git a/packet.md b/packet.md index 446c3bd..1c08e81 100644 --- a/packet.md +++ b/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` diff --git a/src/handlers.cpp b/src/handlers.cpp index bbfa685..a43089a 100644 --- a/src/handlers.cpp +++ b/src/handlers.cpp @@ -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"); diff --git a/src/ir.cpp b/src/ir.cpp index 36e0f02..56500a6 100644 --- a/src/ir.cpp +++ b/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(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(data.count, IR_COUNT) - 1 - i] = sensorValue; + #else + data.data[i] = sensorValue; + #endif + } + } else { + // 通信失败时清零数据 + memset(data.data, 0, IR_COUNT); } } } \ No newline at end of file diff --git a/src/mode.cpp b/src/mode.cpp index 8be4bae..e159781 100644 --- a/src/mode.cpp +++ b/src/mode.cpp @@ -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)); + } } \ No newline at end of file diff --git a/src/motor.cpp b/src/motor.cpp index 2991e2f..4dd8f72 100644 --- a/src/motor.cpp +++ b/src/motor.cpp @@ -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}; } diff --git a/src/storage.cpp b/src/storage.cpp index 80fbf70..310ef18 100644 --- a/src/storage.cpp +++ b/src/storage.cpp @@ -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(); +} diff --git a/src/tracking.cpp b/src/tracking.cpp index 73d147d..4890d6b 100644 --- a/src/tracking.cpp +++ b/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!"); - MotorController::motorXYRControl(0, 0, 0); + { + // 如果是因为全高超时导致的停止 + if (isAllHighState) { + Serial.println("Vehicle likely lifted, stopping all motors"); + MotorController::motorXYRControl(0, 0, 0); + return; + } + + // 正常丢线处理逻辑 + + isSearching = true; + MotorController::rotate(lastOffset < 0 ? ROTATE_CLOCKWISE : ROTATE_ANTICLOCKWISE, baseSpeed * rotateSensitive); return; } - // 正常巡线控制 - float correction = PIDController::update(0, offset); - MotorController::motorXYRControl(0, baseSpeed, -correction); + // 如果之前在搜索模式，现在找到线了，打印日志 + 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; - } - } - - // 如果所有传感器都是高电平或低电平，说明不在线上 - if (allHigh || allLow) { - return LOST_LINE; - } - - // 计算加权平均偏移 - for (int i = 0; i < irData.length; i++) + for (int i = 0; i < IR_COUNT; i++) { - if (irData.data[i]) - { - // 将传感器位置映射到 [-2, 2] 范围 - offset += (i - (irData.length - 1) / 2.0f) * (4.0f / (irData.length - 1)); - activeCount++; + 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 (activeCount > 0) - return offset / activeCount; + // 如果找到连续区域 + 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; + } + } - return LOST_LINE; + lastOffset = offset; + return offset; } \ No newline at end of file diff --git a/src/transmit.cpp b/src/transmit.cpp index 814f1f7..3231f93 100644 --- a/src/transmit.cpp +++ b/src/transmit.cpp @@ -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); }