refactor: 规整格式化

.gitignore

@@ -3,6 +3,8 @@
 .vscode/c_cpp_properties.json
 .vscode/launch.json
 .vscode/ipch
-src/consts.h
 .DS_Store
+src/consts.h
 src/consts-*.h
+include/consts.h
+include/consts-*.h

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

include/ble.h

@@ -0,0 +1,22 @@
+#ifndef BLE_H
+#define BLE_H
+
+#include <Arduino.h>
+#include <BLEDevice.h>
+#include <BLEServer.h>
+#include <BLEUtils.h>
+#include <BLE2902.h>
+#include <transmit.h>
+#include "consts.h"
+
+class BLEManager
+{
+public:
+  static bool deviceConnected;
+  static BLEServer *pServer;
+  static BLECharacteristic *pTxCharacteristic;
+
+  static void init(String deviceName);
+};
+
+#endif

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

include/motor.h

@@ -0,0 +1,40 @@
+#ifndef MOTOR_H
+#define MOTOR_H
+
+#include <Arduino.h>
+
+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

include/storage.h

@@ -0,0 +1,20 @@
+#ifndef STORAGE_H
+#define STORAGE_H
+
+#include <Arduino.h>
+#include <SPIFFS.h>
+
+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

include/transmit.h

@@ -0,0 +1,19 @@
+#ifndef TRANSMIT_H
+#define TRANSMIT_H
+
+#include <Arduino.h>
+#include <BLEDevice.h>
+#include <BLECharacteristic.h>
+#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

include/ultrasound.h

@@ -0,0 +1,19 @@
+#ifndef ULTRASOUND_H
+#define ULTRASOUND_H
+
+#include <Arduino.h>
+
+struct UltrasonicPin
+{
+  int trig;
+  int echo;
+};
+
+struct Ultrasonic
+{
+  static UltrasonicPin pin;
+  static void init(UltrasonicPin pin);
+  static float getDistance();
+};
+
+#endif

include/utils.h

@@ -0,0 +1,8 @@
+#ifndef UTILS_H
+#define UTILS_H
+
+#include <Arduino.h>
+
+void floatToBytes(float val, uint8_t *bytes);
+void updateStatusLED(bool deviceConnected, int ledPin);
+#endif

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

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();
+}

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);
+}

src/main.cpp

@@ -1,736 +1,44 @@
 #include <Arduino.h>
-#include <BLEDevice.h>
+#include <motor.h>
-#include <BLEServer.h>
+#include <utils.h>
-#include <BLEUtils.h>
+#include <storage.h>
-#include <BLE2902.h>
+#include <ble.h>
-#include <SPIFFS.h>
 #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);
+  BLEManager::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();
 
   // 初始化 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();
+  updateStatusLED(BLEManager::deviceConnected, STATUS_LED);
-  if (deviceConnected)
+  if (BLEManager::deviceConnected)
   {
-    sendMotorStatus();
+    sendStatus(*BLEManager::pTxCharacteristic);
   }
 }

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};
+  }
+}

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;
+}

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();
+  }
+}

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;
+}

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);
+    }
+  }
+}