diff --git a/include/tracking.h b/include/tracking.h index 31586fa..d0eb09e 100644 --- a/include/tracking.h +++ b/include/tracking.h @@ -5,6 +5,7 @@ #include "motor.h" #include "pid.h" #include "storage.h" +#include "ultrasound.h" class TrackingController { public: @@ -13,6 +14,10 @@ public: static uint8_t rotateSensitive; // 旋转灵敏度 static float lastOffset; // 上一次的偏移量 + // 添加避障相关常量 + static constexpr float OBSTACLE_DISTANCE = 30.0f; // 避障距离阈值（厘米） + static bool isAvoiding; // 是否正在避障 + // 初始化循迹控制器 static void init(); diff --git a/include/ultrasound.h b/include/ultrasound.h index 8e54992..86103f7 100644 --- a/include/ultrasound.h +++ b/include/ultrasound.h @@ -13,10 +13,12 @@ struct UltrasonicPin struct Ultrasonic { static UltrasonicPin pin; + static float distance; static void init(UltrasonicPin pin, int servoPin); - static float getDistance(); + static void update(); static int servoPin; + static int angle; static Servo servo; static void servoControl(int angle); }; diff --git a/include/utils.h b/include/utils.h index 5286b36..3bcaf55 100644 --- a/include/utils.h +++ b/include/utils.h @@ -7,5 +7,4 @@ void floatToBytes(float val, uint8_t *bytes); float bytesToFloat(uint8_t *bytes); -void updateStatusLED(bool deviceConnected, int ledPin); #endif diff --git a/packet.md b/packet.md index 1c08e81..1e32bbe 100644 --- a/packet.md +++ b/packet.md @@ -163,7 +163,7 @@ ### 设备状态回报 `0xE0` -包体 `01 0F E0 运行模式 电机A_IN_1_2 电机A_PWM 电机B_IN_1_2 电机B_PWM 电机C_IN_1_2 电机C_PWM 电机D_IN_1_2 电机D_PWM 红外引脚数量 红外数据 FE` +包体 `01 13 E0 运行模式 电机A_IN_1_2 电机A_PWM 电机B_IN_1_2 电机B_PWM 电机C_IN_1_2 电机C_PWM 电机D_IN_1_2 电机D_PWM 红外引脚数量 红外数据 超声波距离3 超声波距离2 超声波距离1 超声波距离0 FE` | 数据 | 类型 | 表示 | | -------- | ---- | ---- | @@ -172,5 +172,6 @@ | PWM | 无符号整数 | - | | 红外引脚数量 | 无符号整数 | - | | 红外数据 | 无符号整数 | 按位表示红外循迹模块数据， 0001 1111 | +| 超声波距离 | 无符号整数 | 4 字节浮点数，表示距离，单位 m | -示例 `01 0F E0 00 01 FF 02 FF 02 FF 01 FF 05 1F FE` +示例 `01 13 E0 00 01 FF 02 FF 02 FF 01 FF 05 1F FE 01 01 01 01 FE` diff --git a/src/handlers.cpp b/src/handlers.cpp index 22d342a..772e3e2 100644 --- a/src/handlers.cpp +++ b/src/handlers.cpp @@ -31,7 +31,7 @@ void Handlers::getSPIFFSStatus(BLECharacteristic &characteristic) void Handlers::getDistance(BLECharacteristic &characteristic) { - float distance = Ultrasonic::getDistance(); + float distance = Ultrasonic::distance; uint8_t bufferIndex = 0; Serial.println("CMD_GET_DISTANCE, distance: " + String(distance)); // 构建响应数据 diff --git a/src/main.cpp b/src/main.cpp index 3002860..9506921 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -62,4 +62,6 @@ void loop() { sendStatus(); } + + delay(500); } \ No newline at end of file diff --git a/src/tracking.cpp b/src/tracking.cpp index e3b0ea8..cf028d5 100644 --- a/src/tracking.cpp +++ b/src/tracking.cpp @@ -7,6 +7,7 @@ float TrackingController::lastOffset = 0; bool TrackingController::isSearching = false; unsigned long TrackingController::allHighStartTime = 0; bool TrackingController::isAllHighState = false; +bool TrackingController::isAvoiding = false; void TrackingController::init() { @@ -40,6 +41,9 @@ void TrackingController::update() const IRData &irData = IR::data; float offset = calculateOffset(irData); + Serial.print("Ultrasonic Distance: "); + Serial.println(Ultrasonic::distance); + // 检查是否丢线 if (offset == LOST_LINE) { @@ -60,11 +64,32 @@ void TrackingController::update() return; } - // 如果之前在搜索模式，现在找到线了，打印日志 + // 检查前方障碍物 + if (!isAvoiding && (Ultrasonic::distance < OBSTACLE_DISTANCE || Ultrasonic::distance > 500 )) + { + Serial.print("Obstacle detected at "); + Serial.print(Ultrasonic::distance); + Serial.println("cm, turning back"); + + isAvoiding = true; + // 原地旋转180度 + MotorController::rotate(ROTATE_CLOCKWISE, baseSpeed * rotateSensitive); + return; + } + + // 如果正在避障但已经丢线，说明已经转过头，可以开始寻找新的线 + if (isAvoiding && offset == LOST_LINE) + { + isAvoiding = false; + isSearching = true; + } + + // 如果之前在搜索模式，现在找到线了 if (isSearching) { Serial.println("Line found!"); isSearching = false; + isAvoiding = false; // 确保避障标志被清除 } // 添加防抖：如果偏移量很小，认为是直线 @@ -76,61 +101,67 @@ void TrackingController::update() // 更新最后的偏移量，用于丢线时的方向判断 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; + // 如果没有在避障，则正常循迹 + if (!isAvoiding) + { + // 计算基础速度和转向调整 + 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); } - // 根据偏移量调整左右轮速度 - float speedDiff = baseSpeed * (abs(offset) / 2.0f) * speedFactor; // 差速也要相应降低 - - // 增加最小速度差以确保能够转向 - float minSpeedDiff = baseSpeed * 0.5f * speedFactor; // 最小速度差也要相应降低 - if (speedDiff < minSpeedDiff) - { - speedDiff = minSpeedDiff; - } + // 调试信息 + Serial.print("Offset: "); + Serial.print(offset); + Serial.print(" Left: "); + Serial.print(leftSpeed); + Serial.print(" Right: "); + Serial.println(rightSpeed); - 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); + // 设置电机速度 + MotorController::motorControl('A', leftSpeed); // 左前 + MotorController::motorControl('B', leftSpeed); // 左后 + MotorController::motorControl('C', rightSpeed); // 右后 + MotorController::motorControl('D', rightSpeed); // 右前 } - - // 调试信息 - 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] diff --git a/src/transmit.cpp b/src/transmit.cpp index 3231f93..b0be944 100644 --- a/src/transmit.cpp +++ b/src/transmit.cpp @@ -122,14 +122,22 @@ void handleSerialPacket(uint8_t *packet, int length, BLECharacteristic &characte void sendStatus() { unsigned long currentMillis = millis(); + static unsigned long lastUltrasonicUpdate = 0; + if (currentMillis - lastMotorStatusUpdate >= MOTOR_STATUS_INTERVAL) { lastMotorStatusUpdate = currentMillis; + // 每1秒更新一次超声波数据 + if (currentMillis - lastUltrasonicUpdate >= 1000) { + lastUltrasonicUpdate = currentMillis; + Ultrasonic::update(); + } + uint8_t buffer[PACKET_MAX_LENGTH]; uint8_t bufferIndex = 0; buffer[bufferIndex++] = PACKET_T_HEAD; - buffer[bufferIndex++] = 0x0F; + buffer[bufferIndex++] = 0x13; buffer[bufferIndex++] = CMD_STATUS_MOTOR; buffer[bufferIndex++] = Mode::mode; buffer[bufferIndex++] = (MotorController::getMotorStatus('A').in2 << 1) | MotorController::getMotorStatus('A').in1; @@ -147,6 +155,8 @@ void sendStatus() irData |= (IR::data.data[i] << i); } buffer[bufferIndex++] = irData; + floatToBytes(Ultrasonic::distance, &buffer[bufferIndex]); + bufferIndex += 4; buffer[bufferIndex++] = PACKET_T_TAIL; BLEManager::pTxCharacteristic->setValue(buffer, bufferIndex); BLEManager::pTxCharacteristic->notify(); diff --git a/src/ultrasound.cpp b/src/ultrasound.cpp index 9c03f87..04375ab 100644 --- a/src/ultrasound.cpp +++ b/src/ultrasound.cpp @@ -1,9 +1,11 @@ #include "ultrasound.h" Servo Ultrasonic::servo; +int Ultrasonic::servoPin = 0; UltrasonicPin Ultrasonic::pin = {0, 0}; -int Ultrasonic::servoPin = 0; +float Ultrasonic::distance = 0; +int Ultrasonic::angle = 0; void Ultrasonic::init(UltrasonicPin pin, int servoPin) { @@ -11,29 +13,29 @@ void Ultrasonic::init(UltrasonicPin pin, int servoPin) Ultrasonic::servoPin = servoPin; pinMode(pin.trig, OUTPUT); pinMode(pin.echo, INPUT); - + servo.attach(servoPin); - servo.write(0); - delay(500); - servo.write(180); - delay(500); servo.write(90); } -float Ultrasonic::getDistance() +void Ultrasonic::update() { digitalWrite(pin.trig, HIGH); delayMicroseconds(1); digitalWrite(pin.trig, LOW); float distance = pulseIn(pin.echo, HIGH); // 计数接收高电平时间 distance = distance * 340 / 2 / 10000; // 计算距离 1：声速：340M/S 2：实际距离为1/2声速距离 3：计数时钟为1US//温补公式：c=(331.45+0.61t/℃)m•s-1 (其中331.45是在0度） - return distance; + Ultrasonic::distance = distance; } void Ultrasonic::servoControl(int angle) { - if (angle < 0) angle = 0; - if (angle > 180) angle = 180; - + if (angle < 0) + angle = 0; + if (angle > 180) + angle = 180; + + Ultrasonic::angle = angle; + servo.write(angle); } \ No newline at end of file