diff --git a/.gitignore b/.gitignore index be98dc1..0e0ad04 100644 --- a/.gitignore +++ b/.gitignore @@ -3,6 +3,8 @@ .vscode/c_cpp_properties.json .vscode/launch.json .vscode/ipch -src/consts.h .DS_Store -src/consts-*.h \ No newline at end of file +src/consts.h +src/consts-*.h +include/consts.h +include/consts-*.h diff --git a/README.md b/README.md index f82cf02..74284e8 100644 --- a/README.md +++ b/README.md @@ -25,7 +25,7 @@ git clone https://github.com/colour93/esp32-car.git ### 打开项目并修改初始化设置 -找到 `src/consts.example.h` 文件，复制一份并命名为 `src/consts.h`，根据实际情况，修改其中的内容。 +找到 `include/consts.example.h` 文件，复制一份并命名为 `include/consts.h`，根据实际情况，修改其中的内容。 ### 上传 @@ -33,7 +33,7 @@ git clone https://github.com/colour93/esp32-car.git ## BLE 串口通信协议 -其他设备与 ESP32 通信采用 BLE 的 GATT 协议，其服务默认为 Nordic 串口服务，可以在 `src/consts.h` 中修改。 +其他设备与 ESP32 通信采用 BLE 的 GATT 协议，其服务默认为 Nordic 串口服务，可以在 `include/consts.h` 中修改。 ```cpp # 服务与特征 UUID diff --git a/include/ble.h b/include/ble.h new file mode 100644 index 0000000..3480a42 --- /dev/null +++ b/include/ble.h @@ -0,0 +1,22 @@ +#ifndef BLE_H +#define BLE_H + +#include +#include +#include +#include +#include +#include +#include "consts.h" + +class BLEManager +{ +public: + static bool deviceConnected; + static BLEServer *pServer; + static BLECharacteristic *pTxCharacteristic; + + static void init(String deviceName); +}; + +#endif diff --git a/src/consts.example.h b/include/consts.example.h similarity index 100% rename from src/consts.example.h rename to include/consts.example.h diff --git a/include/handlers.h b/include/handlers.h new file mode 100644 index 0000000..7bfbc29 --- /dev/null +++ b/include/handlers.h @@ -0,0 +1,39 @@ +#ifndef HANDLERS_H +#define HANDLERS_H + +#include "motor.h" +#include "storage.h" +#include "ble.h" +#include "ultrasound.h" +#include "transmit.h" +#include "utils.h" + +// 指令定义 +#define CMD_GET_BT_STATUS 0x10 +#define CMD_GET_SPIFFS_STATUS 0x11 +#define CMD_GET_DISTANCE 0x12 +#define CMD_MOTOR_MOVE_CONTROL 0x20 +#define CMD_MOTOR_STEER_CONTROL 0x21 +#define CMD_MOTOR_SINGLE_CONTROL 0x22 +#define CMD_MOTOR_ROTATE_CONTROL 0x23 +#define CMD_DEMO_PID 0xf0 +#define CMD_DEMO_PATH 0xf1 +#define CMD_MOTOR_XYR_CONTROL 0x24 // XYR轴向控制指令 + +#define CMD_STATUS_MOTOR 0xE0 + +class Handlers +{ +public: + static void getBTStatus(BLECharacteristic &characteristic); + static void getSPIFFSStatus(BLECharacteristic &characteristic); + static void getDistance(BLECharacteristic &characteristic); + static void motorMoveControl(BLECharacteristic &characteristic, uint8_t *packet); + static void motorSteerControl(BLECharacteristic &characteristic, uint8_t *packet); + static void motorRotateControl(BLECharacteristic &characteristic, uint8_t *packet); + static void motorSingleControl(BLECharacteristic &characteristic, uint8_t *packet); + static void motorXYRControl(BLECharacteristic &characteristic, uint8_t *packet); + static void demoPID(BLECharacteristic &characteristic, uint8_t *packet); +}; + +#endif \ No newline at end of file diff --git a/include/motor.h b/include/motor.h new file mode 100644 index 0000000..db2087c --- /dev/null +++ b/include/motor.h @@ -0,0 +1,40 @@ +#ifndef MOTOR_H +#define MOTOR_H + +#include + +struct MotorPin +{ + int pwm; + int in1; + int in2; +}; + +// 电机状态结构体 +struct MotorStatus +{ + MotorPin pin; + bool in1; + bool in2; + unsigned char pwm; +}; + +class MotorController +{ +public: + static void init(MotorPin aPin, MotorPin bPin, MotorPin cPin, MotorPin dPin); + 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 MotorStatus getMotorStatus(char motor); + +private: + static MotorStatus motorA; + static MotorStatus motorB; + static MotorStatus motorC; + static MotorStatus motorD; +}; + +#endif \ No newline at end of file diff --git a/include/storage.h b/include/storage.h new file mode 100644 index 0000000..31747ed --- /dev/null +++ b/include/storage.h @@ -0,0 +1,20 @@ +#ifndef STORAGE_H +#define STORAGE_H + +#include +#include + +class Storage +{ +public: + static bool isMounted; + static void init(); + static void setPID(float kp, float ki, float kd); + static void getPID(float &kp, float &ki, float &kd); + static void setSensitivity(unsigned int sensitivity); + static unsigned int getSensitivity(); + static void setName(String name); + static String getName(); +}; + +#endif diff --git a/include/transmit.h b/include/transmit.h new file mode 100644 index 0000000..6872e57 --- /dev/null +++ b/include/transmit.h @@ -0,0 +1,19 @@ +#ifndef TRANSMIT_H +#define TRANSMIT_H + +#include +#include +#include +#include "handlers.h" + +#define PACKET_R_HEAD 0x00 +#define PACKET_R_TAIL 0xFF +#define PACKET_T_HEAD 0x01 +#define PACKET_T_TAIL 0xFE +#define PACKET_MAX_LENGTH 32 // 数据包最大长度 + +void handleSerialPacket(uint8_t *packet, int length, BLECharacteristic &characteristic); +void processSerialIncomingByte(uint8_t incomingByte, BLECharacteristic &characteristic); +void sendStatus(BLECharacteristic &characteristic); + +#endif \ No newline at end of file diff --git a/include/ultrasound.h b/include/ultrasound.h new file mode 100644 index 0000000..ec0273a --- /dev/null +++ b/include/ultrasound.h @@ -0,0 +1,19 @@ +#ifndef ULTRASOUND_H +#define ULTRASOUND_H + +#include + +struct UltrasonicPin +{ + int trig; + int echo; +}; + +struct Ultrasonic +{ + static UltrasonicPin pin; + static void init(UltrasonicPin pin); + static float getDistance(); +}; + +#endif \ No newline at end of file diff --git a/include/utils.h b/include/utils.h new file mode 100644 index 0000000..f6a5556 --- /dev/null +++ b/include/utils.h @@ -0,0 +1,8 @@ +#ifndef UTILS_H +#define UTILS_H + +#include + +void floatToBytes(float val, uint8_t *bytes); +void updateStatusLED(bool deviceConnected, int ledPin); +#endif \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 4b30716..0e33a20 100644 --- a/platformio.ini +++ b/platformio.ini @@ -8,7 +8,12 @@ ; Please visit documentation for the other options and examples ; https://docs.platformio.org/page/projectconf.html -[env:esp32dev] +[env:esp32] platform = espressif32 board = esp32dev framework = arduino + +[env:esp32s3] +platform = espressif32 +board = adafruit_feather_esp32s3_tft +framework = arduino \ No newline at end of file diff --git a/src/ble.cpp b/src/ble.cpp new file mode 100644 index 0000000..67b6ae7 --- /dev/null +++ b/src/ble.cpp @@ -0,0 +1,59 @@ +#include "ble.h" + +bool BLEManager::deviceConnected = false; +BLEServer* BLEManager::pServer = nullptr; +BLECharacteristic* BLEManager::pTxCharacteristic = nullptr; + +class BLEManagerServerCallbacks : public BLEServerCallbacks +{ + void onConnect(BLEServer *pServer) + { + BLEManager::deviceConnected = true; + }; + + void onDisconnect(BLEServer *pServer) + { + BLEManager::deviceConnected = false; + // 重新开始广播 + pServer->getAdvertising()->start(); + } +}; + +class BLEManagerCharacteristicCallbacks : public BLECharacteristicCallbacks +{ + void onWrite(BLECharacteristic *pCharacteristic) + { + std::string rxValue = pCharacteristic->getValue(); + if (rxValue.length() > 0) + { + for (int i = 0; i < rxValue.length(); i++) + { + processSerialIncomingByte((uint8_t)rxValue[i], *BLEManager::pTxCharacteristic); + } + } + } +}; + +void BLEManager::init(String deviceName) +{ + BLEDevice::init(deviceName.c_str()); + pServer = BLEDevice::createServer(); + pServer->setCallbacks(new BLEManagerServerCallbacks()); + + BLEService *pService = pServer->createService(SERVICE_UUID); + + // 创建 RX 特征值 (用于接收数据) + BLECharacteristic *pRxCharacteristic = pService->createCharacteristic( + CHARACTERISTIC_UUID_RX, + BLECharacteristic::PROPERTY_WRITE); + pRxCharacteristic->setCallbacks(new BLEManagerCharacteristicCallbacks()); + + // 创建 TX 特征值 (用于发送数据) + pTxCharacteristic = pService->createCharacteristic( + CHARACTERISTIC_UUID_TX, + BLECharacteristic::PROPERTY_NOTIFY); + pTxCharacteristic->addDescriptor(new BLE2902()); + + pService->start(); + pServer->getAdvertising()->start(); +} diff --git a/src/handlers.cpp b/src/handlers.cpp new file mode 100644 index 0000000..eca7241 --- /dev/null +++ b/src/handlers.cpp @@ -0,0 +1,219 @@ +#include "handlers.h" + +uint8_t buffer[PACKET_MAX_LENGTH]; + +void Handlers::getBTStatus(BLECharacteristic &characteristic) +{ + Serial.println("CMD_GET_BT_STATUS"); + // 构建响应数据 + buffer[0] = PACKET_T_HEAD; + buffer[1] = 0x05; + buffer[2] = CMD_GET_BT_STATUS; + buffer[3] = (uint8_t)(BLEManager::deviceConnected ? 0x01 : 0x00); + buffer[4] = PACKET_T_TAIL; + characteristic.setValue(buffer, 5); + characteristic.notify(); +} + +void Handlers::getSPIFFSStatus(BLECharacteristic &characteristic) +{ + Serial.println("CMD_GET_SPIFFS_STATUS"); + // 构建响应数据 + buffer[0] = PACKET_T_HEAD; + buffer[1] = 0x05; + buffer[2] = CMD_GET_SPIFFS_STATUS; + buffer[3] = (uint8_t)(Storage::isMounted ? 0x01 : 0x00); + buffer[4] = PACKET_T_TAIL; + characteristic.setValue(buffer, 5); + characteristic.notify(); +} + +void Handlers::getDistance(BLECharacteristic &characteristic) +{ + float distance = Ultrasonic::getDistance(); + Serial.println("CMD_GET_DISTANCE, distance: " + String(distance)); + // 构建响应数据 + buffer[0] = PACKET_T_HEAD; + buffer[1] = 0x08; + buffer[2] = CMD_GET_DISTANCE; + floatToBytes(distance, &buffer[3]); + buffer[7] = PACKET_T_TAIL; + characteristic.setValue(buffer, 8); + characteristic.notify(); +} + +void Handlers::motorMoveControl(BLECharacteristic &characteristic, uint8_t *packet) +{ + uint8_t direction = packet[3]; + uint8_t speed = packet[4]; + Serial.println("CMD_MOTOR_MOVE_CONTROL, direction: " + String(direction) + ", speed: " + String(speed)); + // 移动 + if (direction == 0x00) + { + MotorController::motorControl('A', 0); + MotorController::motorControl('B', 0); + MotorController::motorControl('C', 0); + MotorController::motorControl('D', 0); + } + // 前进 + else if (direction == 0x01) + { + MotorController::motorControl('A', speed); + MotorController::motorControl('B', speed); + MotorController::motorControl('C', speed); + MotorController::motorControl('D', speed); + } + // 后退 + else if (direction == 0x02) + { + MotorController::motorControl('A', -speed); + MotorController::motorControl('B', -speed); + MotorController::motorControl('C', -speed); + MotorController::motorControl('D', -speed); + } +} + +void Handlers::motorSteerControl(BLECharacteristic &characteristic, uint8_t *packet) +{ + uint8_t direction = packet[3]; + uint8_t diffSpeed = packet[4]; + Serial.println("CMD_MOTOR_STEER_CONTROL, direction: " + String(direction) + ", diffSpeed: " + String(diffSpeed)); + // 左转 + if (direction == 0x00) + { + MotorController::motorControl('A', MotorController::getMotorStatus('A').pwm - diffSpeed); + MotorController::motorControl('B', MotorController::getMotorStatus('B').pwm - diffSpeed); + } + // 右转 + else if (direction == 0x01) + { + MotorController::motorControl('C', MotorController::getMotorStatus('C').pwm - diffSpeed); + MotorController::motorControl('D', MotorController::getMotorStatus('D').pwm - diffSpeed); + } +} + +void Handlers::motorRotateControl(BLECharacteristic &characteristic, uint8_t *packet) +{ + uint8_t direction = packet[3]; + uint8_t time = packet[4]; + Serial.println("CMD_MOTOR_ROTATE_CONTROL, direction: " + String(direction) + ", time: " + String(time)); + // 顺时针 + if (direction == 0x00) + { + MotorController::motorControl('A', 255); + MotorController::motorControl('B', 255); + MotorController::motorControl('C', -255); + MotorController::motorControl('D', -255); + } + // 逆时针 + else if (direction == 0x01) + { + MotorController::motorControl('A', -255); + MotorController::motorControl('B', -255); + MotorController::motorControl('C', 255); + MotorController::motorControl('D', 255); + } +} + +void Handlers::motorSingleControl(BLECharacteristic &characteristic, uint8_t *packet) +{ + uint8_t wheel = packet[3]; + uint8_t direction = packet[4]; + uint8_t speed = packet[5]; + Serial.println("CMD_MOTOR_SINGLE_CONTROL, wheel: " + String(wheel) + ", direction: " + String(direction) + ", speed: " + String(speed)); + // 单轮控制 + switch (wheel) + { + case 0x00: + MotorController::motorControl('A', speed); + break; + case 0x01: + MotorController::motorControl('B', speed); + break; + case 0x02: + MotorController::motorControl('C', speed); + break; + case 0x03: + MotorController::motorControl('D', speed); + break; + } +} + +void Handlers::motorXYRControl(BLECharacteristic &characteristic, uint8_t *packet) +{ + int8_t x = (int8_t)packet[3]; // X轴向速度 + int8_t y = (int8_t)packet[4]; // Y轴向速度 + int8_t r = (int8_t)packet[5]; // 旋转速度 + + Serial.printf("CMD_MOTOR_XYR_CONTROL: X=%d, Y=%d, R=%d\n", x, y, r); + MotorController::motorXYRControl(x, y, r); +} + +void Handlers::demoPID(BLECharacteristic &characteristic, uint8_t *packet) +{ + Serial.println("CMD_DEMO_PID"); + + // PID 参数 + float Kp = 2.0; + float Ki = 0.5; + float Kd = 0.1; + + float targetSpeed = 255; + float currentSpeed = 0; + float lastError = 0; + float integral = 0; + unsigned long lastTime = millis(); + + // 加速阶段 - PID控制 + while (millis() - lastTime < 2000) + { // 2秒加速过程 + float error = targetSpeed - currentSpeed; + integral += error; + float derivative = error - lastError; + + float output = Kp * error + Ki * integral + Kd * derivative; + output = constrain(output, 0, 255); // 限制输出范围 + + currentSpeed = output; + + // 应用到所有电机 + MotorController::motorControl('A', output); + MotorController::motorControl('B', output); + MotorController::motorControl('C', output); + MotorController::motorControl('D', output); + + lastError = error; + delay(10); // 控制周期 + } + + // 停止阶段 - PID控制 + lastTime = millis(); + targetSpeed = 0; // 目标速度设为0 + + while (millis() - lastTime < 1000) + { // 1秒减速过程 + float error = targetSpeed - currentSpeed; + integral += error; + float derivative = error - lastError; + + float output = Kp * error + Ki * integral + Kd * derivative; + output = constrain(output, 0, 255); + + currentSpeed = output; + + // 应用到所有电机 + MotorController::motorControl('A', output); + MotorController::motorControl('B', output); + MotorController::motorControl('C', output); + MotorController::motorControl('D', output); + + lastError = error; + delay(10); + } + + // 确保完全停止 + MotorController::motorControl('A', 0); + MotorController::motorControl('B', 0); + MotorController::motorControl('C', 0); + MotorController::motorControl('D', 0); +} \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 204410b..9533800 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,736 +1,44 @@ #include -#include -#include -#include -#include -#include +#include +#include +#include +#include #include "consts.h" -// 使用 2 片 TB6612 控制 4 个电机 -// 使用差速控制转向 - -#define PACKET_R_HEAD 0x00 -#define PACKET_R_TAIL 0xFF -#define PACKET_T_HEAD 0x01 -#define PACKET_T_TAIL 0xFE -#define PACKET_MAX_LENGTH 32 // 数据包最大长度 - -// 指令定义 -#define CMD_GET_BT_STATUS 0x10 -#define CMD_GET_SPIFFS_STATUS 0x11 -#define CMD_GET_DISTANCE 0x12 -#define CMD_MOTOR_MOVE_CONTROL 0x20 -#define CMD_MOTOR_STEER_CONTROL 0x21 -#define CMD_MOTOR_SINGLE_CONTROL 0x22 -#define CMD_MOTOR_ROTATE_CONTROL 0x23 -#define CMD_DEMO_PID 0xf0 -#define CMD_DEMO_PATH 0xf1 -#define CMD_MOTOR_XYR_CONTROL 0x24 // XYR轴向控制指令 - -#define CMD_STATUS_MOTOR 0xE0 - // 全局变量 int currentSpeed = 0; // 当前速度 int turnOffset = 0; // 转向偏移量 (-100 到 100) bool isMoving = false; // 运动状态 bool isTurning = false; // 转向状态 -// BLE 相关 -BLEServer *pServer = nullptr; -BLECharacteristic *pTxCharacteristic = nullptr; -bool deviceConnected = false; - -// 数据包缓冲区 -uint8_t packetBuffer[PACKET_MAX_LENGTH]; -int packetIndex = 0; -bool isReceivingPacket = false; - -// 在全局变量定义区域添加 -#define LED_FLASH_INTERVAL 1000 // LED闪烁间隔(ms) -unsigned long lastLedToggle = 0; // 上次LED切换状态的时间 -bool ledState = false; // LED当前状态 - -#define MOTOR_STATUS_INTERVAL 200 // 发送电机状态的时间间隔(ms) -unsigned long lastMotorStatusUpdate = 0; // 上次发送电机状态的时间 - -// 在全局变量区域添加电机方向状态跟踪 -struct MotorStatus -{ - bool in1; - bool in2; - unsigned char pwm; -}; - -// 定义四个电机的状态 -MotorStatus motorA = {false, false, 0}; -MotorStatus motorB = {false, false, 0}; -MotorStatus motorC = {false, false, 0}; -MotorStatus motorD = {false, false, 0}; - -void processSerialIncomingByte(uint8_t incomingByte, BLECharacteristic &characteristic); - -class CarBLEServerCallbacks : public BLEServerCallbacks -{ - void onConnect(BLEServer *pServer) - { - deviceConnected = true; - }; - - void onDisconnect(BLEServer *pServer) - { - deviceConnected = false; - // 重新开始广播 - pServer->getAdvertising()->start(); - } -}; - -class CarBLECharacteristicCallbacks : public BLECharacteristicCallbacks -{ - void onWrite(BLECharacteristic *pCharacteristic) - { - std::string rxValue = pCharacteristic->getValue(); - if (rxValue.length() > 0) - { - for (int i = 0; i < rxValue.length(); i++) - { - processSerialIncomingByte((uint8_t)rxValue[i], *pTxCharacteristic); - } - } - } -}; - -void floatToBytes(float val, uint8_t *bytes) -{ - union - { - float f; - uint8_t bytes[4]; - } u; - u.f = val; - - // 考虑大小端问题 - for (int i = 0; i < 4; i++) - { - bytes[i] = u.bytes[i]; - } -} - -void pwmControl(char motor, unsigned char pwm) -{ - int pwmPin; - switch (motor) - { - case 'A': - pwmPin = MOTOR_A_PWMA; - motorA.pwm = pwm; - break; - case 'B': - pwmPin = MOTOR_B_PWMB; - motorB.pwm = pwm; - break; - case 'C': - pwmPin = MOTOR_C_PWMA; - motorC.pwm = pwm; - break; - case 'D': - pwmPin = MOTOR_D_PWMB; - motorD.pwm = pwm; - break; - } - analogWrite(pwmPin, pwm); -} - -// 电机控制函数 -void motorControl(char motor, int speed) -{ - int in1Pin, in2Pin; - MotorStatus *status; - - switch (motor) - { - case 'A': - in1Pin = MOTOR_A_AIN1; - in2Pin = MOTOR_A_AIN2; - status = &motorA; - break; - case 'B': - in1Pin = MOTOR_B_BIN2; - in2Pin = MOTOR_B_BIN1; - status = &motorB; - break; - case 'C': - in1Pin = MOTOR_C_AIN2; - in2Pin = MOTOR_C_AIN1; - status = &motorC; - break; - case 'D': - in1Pin = MOTOR_D_BIN1; - in2Pin = MOTOR_D_BIN2; - status = &motorD; - break; - } - - if (speed > 0) - { - digitalWrite(in1Pin, HIGH); - digitalWrite(in2Pin, LOW); - if (motor == 'A' || motor == 'D') - { - status->in1 = true; - status->in2 = false; - } - else - { - status->in1 = false; - status->in2 = true; - } - } - else if (speed < 0) - { - digitalWrite(in1Pin, LOW); - digitalWrite(in2Pin, HIGH); - if (motor == 'A' || motor == 'D') - { - status->in1 = false; - status->in2 = true; - } - else - { - status->in1 = true; - status->in2 = false; - } - - speed = -speed; - } - else - { - digitalWrite(in1Pin, LOW); - digitalWrite(in2Pin, LOW); - status->in1 = false; - status->in2 = false; - } - pwmControl(motor, speed); -} - -void motorXYRControl(int8_t x, int8_t y, int8_t r) { - // 将输入范围限制在 -100 到 100 - x = constrain(x, -100, 100); - y = constrain(y, -100, 100); - r = constrain(r, -100, 100); - - // 将 -100~100 映射到 -255~255 - int16_t mappedX = map(x, -100, 100, -255, 255); - int16_t mappedY = map(y, -100, 100, -255, 255); - int16_t mappedR = map(r, -100, 100, -255, 255); - - // 麦克纳姆轮运动学方程 - // 左前轮 = Y + X + R - // 右前轮 = Y - X - R - // 右后轮 = Y + X - R - // 左后轮 = Y - X + R - int16_t speedA = mappedY + mappedX + mappedR; // 左前轮 - int16_t speedB = mappedY - mappedX - mappedR; // 右前轮 - int16_t speedC = mappedY + mappedX - mappedR; // 右后轮 - int16_t speedD = mappedY - mappedX + mappedR; // 左后轮 - - // 找出最大速度的绝对值 - int16_t maxSpeed = max(max(abs(speedA), abs(speedB)), - max(abs(speedC), abs(speedD))); - - // 如果最大速度超过255，等比例缩放所有速度 - if (maxSpeed > 255) { - float scale = 255.0f / maxSpeed; - speedA *= scale; - speedB *= scale; - speedC *= scale; - speedD *= scale; - } - - // 控制各个电机 - motorControl('A', speedA); - motorControl('B', speedB); - motorControl('C', speedC); - motorControl('D', speedD); -} - - -float getDistance() -{ - digitalWrite(HC_SR04_TRIG, HIGH); - delayMicroseconds(1); - digitalWrite(HC_SR04_TRIG, LOW); - float distance = pulseIn(HC_SR04_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; -} - -class CarStorage -{ -public: - static bool isMounted; - - static void init() - { - if (!SPIFFS.begin(true)) - { - Serial.println("SPIFFS Mount Failed"); - isMounted = false; - } - else - isMounted = true; - } - - static unsigned int getSensitivity() - { - if (!isMounted) - return 0xFF; - - File file = SPIFFS.open("/sensitivity.txt", "r"); - if (!file) - return 0xFF; - - String sensitivity = file.readStringUntil('\n'); - file.close(); - - return sensitivity.toInt(); - } - - static void setSensitivity(unsigned int sensitivity) - { - if (!isMounted) - return; - - File file = SPIFFS.open("/sensitivity.txt", "w"); - file.println(sensitivity); - file.close(); - } -}; -bool CarStorage::isMounted = false; - void setup() { // 初始化口 Serial.begin(115200); // 初始化 BLE - BLEDevice::init(DEVICE_NAME); - pServer = BLEDevice::createServer(); - pServer->setCallbacks(new CarBLEServerCallbacks()); - - BLEService *pService = pServer->createService(SERVICE_UUID); - - // 创建 RX 特征值 (用于接收数据) - BLECharacteristic *pRxCharacteristic = pService->createCharacteristic( - CHARACTERISTIC_UUID_RX, - BLECharacteristic::PROPERTY_WRITE); - pRxCharacteristic->setCallbacks(new CarBLECharacteristicCallbacks()); - - // 创建 TX 特征值 (用于发送数据) - pTxCharacteristic = pService->createCharacteristic( - CHARACTERISTIC_UUID_TX, - BLECharacteristic::PROPERTY_NOTIFY); - pTxCharacteristic->addDescriptor(new BLE2902()); - - pService->start(); - pServer->getAdvertising()->start(); + BLEManager::init(DEVICE_NAME); // 初始化 EEPROM - CarStorage::init(); + Storage::init(); + + // 初始化电机 + MotorController::init( + {MOTOR_A_PWMA, MOTOR_A_AIN1, MOTOR_A_AIN2}, + {MOTOR_B_PWMB, MOTOR_B_BIN1, MOTOR_B_BIN2}, + {MOTOR_C_PWMA, MOTOR_C_AIN1, MOTOR_C_AIN2}, + {MOTOR_D_PWMB, MOTOR_D_BIN1, MOTOR_D_BIN2}); // 设置引脚模式 pinMode(STATUS_LED, OUTPUT); digitalWrite(STATUS_LED, HIGH); - - pinMode(MOTOR_A_PWMA, OUTPUT); - pinMode(MOTOR_A_AIN1, OUTPUT); - pinMode(MOTOR_A_AIN2, OUTPUT); - - pinMode(MOTOR_B_PWMB, OUTPUT); - pinMode(MOTOR_B_BIN1, OUTPUT); - pinMode(MOTOR_B_BIN2, OUTPUT); - - pinMode(MOTOR_C_PWMA, OUTPUT); - pinMode(MOTOR_C_AIN1, OUTPUT); - pinMode(MOTOR_C_AIN2, OUTPUT); - - pinMode(MOTOR_D_PWMB, OUTPUT); - pinMode(MOTOR_D_BIN1, OUTPUT); - pinMode(MOTOR_D_BIN2, OUTPUT); - - pinMode(HC_SR04_TRIG, OUTPUT); - pinMode(HC_SR04_ECHO, INPUT); -} - -void handleSerialPacket(uint8_t *packet, int length, BLECharacteristic &characteristic) -{ - if (!deviceConnected || length < 4) - return; - - uint8_t packetLength = packet[1]; - uint8_t cmd = packet[2]; - uint8_t direction, speed, time, wheel, diffSpeed; - float distance; - uint8_t buffer[PACKET_MAX_LENGTH]; - int bufferIndex = 0; - - // PID 相关变量声明移到这里 - float Kp, Ki, Kd; - float targetSpeed, currentSpeed, lastError, integral; - unsigned long lastTime; - unsigned long startTime; - - switch (cmd) - { - case CMD_GET_BT_STATUS: - Serial.println("CMD_GET_BT_STATUS"); - // 构建响应数据 - buffer[0] = PACKET_T_HEAD; - buffer[1] = 0x05; - buffer[2] = CMD_GET_BT_STATUS; - buffer[3] = (uint8_t)(deviceConnected ? 0x01 : 0x00); - buffer[4] = PACKET_T_TAIL; - characteristic.setValue(buffer, 5); - characteristic.notify(); - break; - - case CMD_GET_SPIFFS_STATUS: - Serial.println("CMD_GET_SPIFFS_STATUS"); - // 构建响应数据 - buffer[0] = PACKET_T_HEAD; - buffer[1] = 0x05; - buffer[2] = CMD_GET_SPIFFS_STATUS; - buffer[3] = (uint8_t)(CarStorage::isMounted ? 0x01 : 0x00); - buffer[4] = PACKET_T_TAIL; - characteristic.setValue(buffer, 5); - characteristic.notify(); - break; - - case CMD_GET_DISTANCE: - distance = getDistance(); - Serial.println("CMD_GET_DISTANCE, distance: " + String(distance)); - // 构建响应数据 - buffer[0] = PACKET_T_HEAD; - buffer[1] = 0x08; - buffer[2] = CMD_GET_DISTANCE; - floatToBytes(distance, &buffer[3]); - buffer[7] = PACKET_T_TAIL; - characteristic.setValue(buffer, 8); - characteristic.notify(); - break; - - case CMD_MOTOR_MOVE_CONTROL: - direction = packet[3]; - speed = packet[4]; - Serial.println("CMD_MOTOR_MOVE_CONTROL, direction: " + String(direction) + ", speed: " + String(speed)); - // 移动 - if (direction == 0x00) - { - motorControl('A', 0); - motorControl('B', 0); - motorControl('C', 0); - motorControl('D', 0); - } - // 前进 - else if (direction == 0x01) - { - motorControl('A', speed); - motorControl('B', speed); - motorControl('C', speed); - motorControl('D', speed); - } - // 后退 - else if (direction == 0x02) - { - motorControl('A', -speed); - motorControl('B', -speed); - motorControl('C', -speed); - motorControl('D', -speed); - } - break; - - case CMD_MOTOR_STEER_CONTROL: - direction = packet[3]; - diffSpeed = packet[4]; - Serial.println("CMD_MOTOR_STEER_CONTROL, direction: " + String(direction) + ", diffSpeed: " + String(diffSpeed)); - // 左转 - if (direction == 0x00) - { - motorControl('A', motorA.pwm - diffSpeed); - motorControl('B', motorB.pwm - diffSpeed); - } - // 右转 - else if (direction == 0x01) - { - motorControl('C', motorC.pwm - diffSpeed); - motorControl('D', motorD.pwm - diffSpeed); - } - break; - - case CMD_MOTOR_ROTATE_CONTROL: - direction = packet[3]; - time = packet[4]; - Serial.println("CMD_MOTOR_ROTATE_CONTROL, direction: " + String(direction) + ", time: " + String(time)); - // 顺时针 - if (direction == 0x00) - { - motorControl('A', 255); - motorControl('B', 255); - motorControl('C', -255); - motorControl('D', -255); - } - // 逆时针 - else if (direction == 0x01) - { - motorControl('A', -255); - motorControl('B', -255); - motorControl('C', 255); - motorControl('D', 255); - } - break; - - case CMD_MOTOR_SINGLE_CONTROL: - wheel = packet[3]; - direction = packet[4]; - speed = packet[5]; - Serial.println("CMD_MOTOR_SINGLE_CONTROL, wheel: " + String(wheel) + ", direction: " + String(direction) + ", speed: " + String(speed)); - // 单轮控制 - switch (wheel) - { - case 0x00: - motorControl('A', speed); - break; - case 0x01: - motorControl('B', speed); - break; - case 0x02: - motorControl('C', speed); - break; - case 0x03: - motorControl('D', speed); - break; - } - break; - - case CMD_DEMO_PID: - Serial.println("CMD_DEMO_PID"); - - // PID 参数 - Kp = 2.0; - Ki = 0.5; - Kd = 0.1; - - targetSpeed = 255; - currentSpeed = 0; - lastError = 0; - integral = 0; - lastTime = millis(); - - // 加速阶段 - PID控制 - while (millis() - lastTime < 2000) - { // 2秒加速过程 - float error = targetSpeed - currentSpeed; - integral += error; - float derivative = error - lastError; - - float output = Kp * error + Ki * integral + Kd * derivative; - output = constrain(output, 0, 255); // 限制输出范围 - - currentSpeed = output; - - // 应用到所有电机 - motorControl('A', output); - motorControl('B', output); - motorControl('C', output); - motorControl('D', output); - - lastError = error; - delay(10); // 控制周期 - } - - // 停止阶段 - PID控制 - lastTime = millis(); - targetSpeed = 0; // 目标速度设为0 - - while (millis() - lastTime < 1000) - { // 1秒减速过程 - float error = targetSpeed - currentSpeed; - integral += error; - float derivative = error - lastError; - - float output = Kp * error + Ki * integral + Kd * derivative; - output = constrain(output, 0, 255); - - currentSpeed = output; - - // 应用到所有电机 - motorControl('A', output); - motorControl('B', output); - motorControl('C', output); - motorControl('D', output); - - lastError = error; - delay(10); - } - - // 确保完全停止 - motorControl('A', 0); - motorControl('B', 0); - motorControl('C', 0); - motorControl('D', 0); - break; - - case CMD_DEMO_PATH: - Serial.println("CMD_DEMO_PATH"); - - // 第一段：全速前进1秒 - currentSpeed = 255; - startTime = millis(); - // 第一段：全速前进1秒 - while (millis() - startTime < 1000) - { - motorControl('A', currentSpeed); - motorControl('B', currentSpeed); - motorControl('C', currentSpeed); - motorControl('D', currentSpeed); - delay(10); - } - - // 第二段：左转90度 - startTime = millis(); - while (millis() - startTime < 500) - { // 假设500ms可以转90度 - // 左侧电机反转,右侧电机正转 - motorControl('A', 200); - motorControl('B', 200); - motorControl('C', 200); - motorControl('D', 200); - delay(10); - } - - // 第三段：全速前进1秒 - startTime = millis(); - while (millis() - startTime < 1000) - { - motorControl('A', currentSpeed); - motorControl('B', currentSpeed); - motorControl('C', currentSpeed); - motorControl('D', currentSpeed); - delay(10); - } - - // 最后停止所有电机 - motorControl('A', 0); - motorControl('B', 0); - motorControl('C', 0); - motorControl('D', 0); - - break; - - case CMD_MOTOR_XYR_CONTROL: - if (length >= 7) { // 确保数据包长度正确 - int8_t x = (int8_t)packet[3]; // X轴向速度 - int8_t y = (int8_t)packet[4]; // Y轴向速度 - int8_t r = (int8_t)packet[5]; // 旋转速度 - - Serial.printf("CMD_MOTOR_XYR_CONTROL: X=%d, Y=%d, R=%d\n", x, y, r); - motorXYRControl(x, y, r); - } - break; - - default: - break; - } -} - -void processSerialIncomingByte(uint8_t incomingByte, BLECharacteristic &characteristic) -{ - static uint8_t expectedLength = 0; - - packetIndex++; - - if (incomingByte == PACKET_R_HEAD && !isReceivingPacket) - { - isReceivingPacket = true; - packetIndex = 0; - packetBuffer[packetIndex] = incomingByte; - } - else if (isReceivingPacket) - { - if (packetIndex < PACKET_MAX_LENGTH) - { - packetBuffer[packetIndex] = incomingByte; - - // 第二个字节是包体长度 - if (packetIndex == 1) - { - expectedLength = incomingByte; - } - - // 到预期长度时检查包尾 - if (packetIndex == expectedLength - 1) - { - isReceivingPacket = false; - if (incomingByte == PACKET_R_TAIL) - { - handleSerialPacket(packetBuffer, packetIndex + 1, *pTxCharacteristic); - } - } - } - else - { - isReceivingPacket = false; - } - } -} - -void updateStatusLED() -{ - if (deviceConnected) - { - digitalWrite(STATUS_LED, HIGH); - } - else - { - unsigned long currentMillis = millis(); - if (currentMillis - lastLedToggle >= LED_FLASH_INTERVAL) - { - lastLedToggle = currentMillis; - ledState = !ledState; - digitalWrite(STATUS_LED, ledState); - } - } -} - -void sendMotorStatus() -{ - unsigned long currentMillis = millis(); - if (currentMillis - lastMotorStatusUpdate >= MOTOR_STATUS_INTERVAL) - { - lastMotorStatusUpdate = currentMillis; - - uint8_t buffer[PACKET_MAX_LENGTH]; - buffer[0] = PACKET_T_HEAD; - buffer[1] = 0x0C; - buffer[2] = CMD_STATUS_MOTOR; - buffer[3] = (motorA.in2 << 1) | motorA.in1; - buffer[4] = motorA.pwm; - buffer[5] = (motorB.in2 << 1) | motorB.in1; - buffer[6] = motorB.pwm; - buffer[7] = (motorC.in2 << 1) | motorC.in1; - buffer[8] = motorC.pwm; - buffer[9] = (motorD.in2 << 1) | motorD.in1; - buffer[10] = motorD.pwm; - buffer[11] = PACKET_T_TAIL; - pTxCharacteristic->setValue(buffer, 12); - pTxCharacteristic->notify(); - } } void loop() { - updateStatusLED(); - if (deviceConnected) + updateStatusLED(BLEManager::deviceConnected, STATUS_LED); + if (BLEManager::deviceConnected) { - sendMotorStatus(); + sendStatus(*BLEManager::pTxCharacteristic); } } \ No newline at end of file diff --git a/src/motor.cpp b/src/motor.cpp new file mode 100644 index 0000000..669ecef --- /dev/null +++ b/src/motor.cpp @@ -0,0 +1,193 @@ +#include "motor.h" + + +// 静态成员初始化 +MotorStatus MotorController::motorA = {{0, 0, 0}, false, false, 0}; +MotorStatus MotorController::motorB = {{0, 0, 0}, false, false, 0}; +MotorStatus MotorController::motorC = {{0, 0, 0}, false, false, 0}; +MotorStatus MotorController::motorD = {{0, 0, 0}, false, false, 0}; + +void MotorController::init( + MotorPin aPin, + MotorPin bPin, + MotorPin cPin, + MotorPin dPin) +{ + motorA.pin = aPin; + motorB.pin = bPin; + motorC.pin = cPin; + motorD.pin = dPin; + + pinMode(aPin.pwm, OUTPUT); + pinMode(aPin.in1, OUTPUT); + pinMode(aPin.in2, OUTPUT); + + pinMode(bPin.pwm, OUTPUT); + pinMode(bPin.in1, OUTPUT); + pinMode(bPin.in2, OUTPUT); + + pinMode(cPin.pwm, OUTPUT); + pinMode(cPin.in1, OUTPUT); + pinMode(cPin.in2, OUTPUT); + + pinMode(dPin.pwm, OUTPUT); + pinMode(dPin.in1, OUTPUT); + pinMode(dPin.in2, OUTPUT); +} + +void MotorController::pwmControl(char motor, unsigned char pwm) +{ + int pwmPin; + switch (motor) + { + case 'A': + pwmPin = motorA.pin.pwm; + motorA.pwm = pwm; + break; + case 'B': + pwmPin = motorB.pin.pwm; + motorB.pwm = pwm; + break; + case 'C': + pwmPin = motorC.pin.pwm; + motorC.pwm = pwm; + break; + case 'D': + pwmPin = motorD.pin.pwm; + motorD.pwm = pwm; + break; + } + analogWrite(pwmPin, pwm); +} + +// 电机控制函数 +void MotorController::motorControl(char motor, int speed) +{ + int in1Pin, in2Pin; + MotorStatus *status; + + switch (motor) + { + case 'A': + in1Pin = motorA.pin.in1; + in2Pin = motorA.pin.in2; + status = &motorA; + break; + case 'B': + in1Pin = motorB.pin.in2; + in2Pin = motorB.pin.in1; + status = &motorB; + break; + case 'C': + in1Pin = motorC.pin.in2; + in2Pin = motorC.pin.in1; + status = &motorC; + break; + case 'D': + in1Pin = motorD.pin.in1; + in2Pin = motorD.pin.in2; + status = &motorD; + break; + } + + if (speed > 0) + { + digitalWrite(in1Pin, HIGH); + digitalWrite(in2Pin, LOW); + if (motor == 'A' || motor == 'D') + { + status->in1 = true; + status->in2 = false; + } + else + { + status->in1 = false; + status->in2 = true; + } + } + else if (speed < 0) + { + digitalWrite(in1Pin, LOW); + digitalWrite(in2Pin, HIGH); + if (motor == 'A' || motor == 'D') + { + status->in1 = false; + status->in2 = true; + } + else + { + status->in1 = true; + status->in2 = false; + } + + speed = -speed; + } + else + { + digitalWrite(in1Pin, LOW); + digitalWrite(in2Pin, LOW); + status->in1 = false; + status->in2 = false; + } + pwmControl(motor, speed); +} + +void MotorController::motorXYRControl(int8_t x, int8_t y, int8_t r) +{ + // 将输入范围限制在 -100 到 100 + x = constrain(x, -100, 100); + y = constrain(y, -100, 100); + r = constrain(r, -100, 100); + + // 将 -100~100 映射到 -255~255 + int16_t mappedX = map(x, -100, 100, -255, 255); + int16_t mappedY = map(y, -100, 100, -255, 255); + int16_t mappedR = map(r, -100, 100, -255, 255); + + // 麦克纳姆轮运动学方程 + // 左前轮 = Y + X + R + // 右前轮 = Y - X - R + // 右后轮 = Y + X - R + // 左后轮 = Y - X + R + int16_t speedA = mappedY + mappedX + mappedR; // 左前轮 + int16_t speedB = mappedY - mappedX - mappedR; // 右前轮 + int16_t speedC = mappedY + mappedX - mappedR; // 右后轮 + int16_t speedD = mappedY - mappedX + mappedR; // 左后轮 + + // 找出最大速度的绝对值 + int16_t maxSpeed = max(max(abs(speedA), abs(speedB)), + max(abs(speedC), abs(speedD))); + + // 如果最大速度超过255，等比例缩放所有速度 + if (maxSpeed > 255) + { + float scale = 255.0f / maxSpeed; + speedA *= scale; + speedB *= scale; + speedC *= scale; + speedD *= scale; + } + + // 控制各个电机 + motorControl('A', speedA); + motorControl('B', speedB); + motorControl('C', speedC); + motorControl('D', speedD); +} + +MotorStatus MotorController::getMotorStatus(char motor) +{ + switch (motor) + { + case 'A': + return motorA; + case 'B': + return motorB; + case 'C': + return motorC; + case 'D': + return motorD; + default: + return {false, false, 0}; + } +} diff --git a/src/storage.cpp b/src/storage.cpp new file mode 100644 index 0000000..01ba4b1 --- /dev/null +++ b/src/storage.cpp @@ -0,0 +1,85 @@ +#include "storage.h" + +bool Storage::isMounted; + +void Storage::init() +{ + if (!SPIFFS.begin(true)) + { + Serial.println("SPIFFS Mount Failed"); + isMounted = false; + } + else + isMounted = true; +} + +unsigned int Storage::getSensitivity() +{ + if (!isMounted) + return 0xFF; + + File file = SPIFFS.open("/sensitivity.txt", "r"); + if (!file) + return 0xFF; + + String sensitivity = file.readStringUntil('\n'); + file.close(); + + return sensitivity.toInt(); +} + +void Storage::setSensitivity(unsigned int sensitivity) +{ + if (!isMounted) + return; + + File file = SPIFFS.open("/sensitivity.txt", "w"); + file.println(sensitivity); + file.close(); +} + +void Storage::setPID(float kp, float ki, float kd) +{ + if (!isMounted) + return; + + File file = SPIFFS.open("/pid.txt", "w"); + file.println(kp); + file.println(ki); + file.println(kd); + file.close(); +} + +void Storage::getPID(float &kp, float &ki, float &kd) +{ + if (!isMounted) + return; + + File file = SPIFFS.open("/pid.txt", "r"); + kp = file.readStringUntil('\n').toFloat(); + ki = file.readStringUntil('\n').toFloat(); + kd = file.readStringUntil('\n').toFloat(); + file.close(); +} + +void Storage::setName(String name) +{ + if (!isMounted) + return; + + File file = SPIFFS.open("/name.txt", "w"); + file.println(name); + file.close(); +} + +String Storage::getName() +{ + if (!isMounted) + return ""; + + File file = SPIFFS.open("/name.txt", "r"); + String name = file.readStringUntil('\n'); + file.close(); + + return name; +} diff --git a/src/transmit.cpp b/src/transmit.cpp new file mode 100644 index 0000000..4554324 --- /dev/null +++ b/src/transmit.cpp @@ -0,0 +1,125 @@ +#include "transmit.h" + +#define MOTOR_STATUS_INTERVAL 200 // 发送电机状态的时间间隔(ms) +unsigned long lastMotorStatusUpdate = 0; // 上次发送电机状态的时间 + +// 数据包缓冲区 +uint8_t packetBuffer[PACKET_MAX_LENGTH]; +int packetIndex = 0; +bool isReceivingPacket = false; + +void processSerialIncomingByte(uint8_t incomingByte, BLECharacteristic &characteristic) +{ + static uint8_t expectedLength = 0; + + packetIndex++; + + if (incomingByte == PACKET_R_HEAD && !isReceivingPacket) + { + isReceivingPacket = true; + packetIndex = 0; + packetBuffer[packetIndex] = incomingByte; + } + else if (isReceivingPacket) + { + if (packetIndex < PACKET_MAX_LENGTH) + { + packetBuffer[packetIndex] = incomingByte; + + // 第二个字节是包体长度 + if (packetIndex == 1) + { + expectedLength = incomingByte; + } + + // 到预期长度时检查包尾 + if (packetIndex == expectedLength - 1) + { + isReceivingPacket = false; + if (incomingByte == PACKET_R_TAIL) + { + handleSerialPacket(packetBuffer, packetIndex + 1, characteristic); + } + } + } + else + { + isReceivingPacket = false; + } + } +} + +void handleSerialPacket(uint8_t *packet, int length, BLECharacteristic &characteristic) +{ + if (length < 4) + return; + + uint8_t cmd = packet[2]; + + switch (cmd) + { + case CMD_GET_BT_STATUS: + Handlers::getBTStatus(characteristic); + break; + + case CMD_GET_SPIFFS_STATUS: + Handlers::getSPIFFSStatus(characteristic); + break; + + case CMD_GET_DISTANCE: + Handlers::getDistance(characteristic); + break; + + case CMD_MOTOR_MOVE_CONTROL: + Handlers::motorMoveControl(characteristic, packet); + break; + + case CMD_MOTOR_STEER_CONTROL: + Handlers::motorSteerControl(characteristic, packet); + break; + + case CMD_MOTOR_ROTATE_CONTROL: + Handlers::motorRotateControl(characteristic, packet); + break; + + case CMD_MOTOR_SINGLE_CONTROL: + Handlers::motorSingleControl(characteristic, packet); + break; + + case CMD_DEMO_PID: + Handlers::demoPID(characteristic, packet); + break; + + case CMD_MOTOR_XYR_CONTROL: + Handlers::motorXYRControl(characteristic, packet); + break; + + default: + break; + } +} + +void sendStatus(BLECharacteristic &characteristic) +{ + unsigned long currentMillis = millis(); + if (currentMillis - lastMotorStatusUpdate >= MOTOR_STATUS_INTERVAL) + { + lastMotorStatusUpdate = currentMillis; + + uint8_t buffer[PACKET_MAX_LENGTH]; + buffer[0] = PACKET_T_HEAD; + buffer[1] = 0x0C; + buffer[2] = CMD_STATUS_MOTOR; + buffer[3] = (MotorController::getMotorStatus('A').in2 << 1) | MotorController::getMotorStatus('A').in1; + buffer[4] = MotorController::getMotorStatus('A').pwm; + buffer[5] = (MotorController::getMotorStatus('B').in2 << 1) | MotorController::getMotorStatus('B').in1; + buffer[6] = MotorController::getMotorStatus('B').pwm; + buffer[7] = (MotorController::getMotorStatus('C').in2 << 1) | MotorController::getMotorStatus('C').in1; + buffer[8] = MotorController::getMotorStatus('C').pwm; + buffer[9] = (MotorController::getMotorStatus('D').in2 << 1) | MotorController::getMotorStatus('D').in1; + buffer[10] = MotorController::getMotorStatus('D').pwm; + buffer[11] = PACKET_T_TAIL; + characteristic.setValue(buffer, 12); + characteristic.notify(); + } +} \ No newline at end of file diff --git a/src/ultrasound.cpp b/src/ultrasound.cpp new file mode 100644 index 0000000..b9a8a1a --- /dev/null +++ b/src/ultrasound.cpp @@ -0,0 +1,20 @@ +#include "ultrasound.h" + +UltrasonicPin Ultrasonic::pin = {0, 0}; + +void Ultrasonic::init(UltrasonicPin pin) +{ + Ultrasonic::pin = pin; + pinMode(pin.trig, OUTPUT); + pinMode(pin.echo, INPUT); +} + +float Ultrasonic::getDistance() +{ + 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; +} \ No newline at end of file diff --git a/src/utils.cpp b/src/utils.cpp new file mode 100644 index 0000000..4ddc65b --- /dev/null +++ b/src/utils.cpp @@ -0,0 +1,39 @@ +#include "utils.h" + +#define LED_FLASH_INTERVAL 1000 // LED闪烁间隔(ms) +unsigned long lastLedToggle = 0; // 上次LED切换状态的时间 +bool ledState = false; // LED当前状态 + +void floatToBytes(float val, uint8_t *bytes) +{ + union + { + float f; + uint8_t bytes[4]; + } u; + u.f = val; + + // 考虑大小端问题 + for (int i = 0; i < 4; i++) + { + bytes[i] = u.bytes[i]; + } +} + +void updateStatusLED(bool deviceConnected, int ledPin) +{ + if (deviceConnected) + { + digitalWrite(ledPin, HIGH); + } + else + { + unsigned long currentMillis = millis(); + if (currentMillis - lastLedToggle >= LED_FLASH_INTERVAL) + { + lastLedToggle = currentMillis; + ledState = !ledState; + digitalWrite(ledPin, ledState); + } + } +} \ No newline at end of file