锂电池正极→MX1919 的 VM 引脚；

锂电池负极→系统地（GND）；

DC/DC 降压模块输入→锂电池正负极；

DC/DC 5V 输出→ESP32 的 5V 引脚、超声波模块 VCC、测速模块 VCC；

DC/DC 3.3V 输出→ESP32 的 3.3V 引脚。

ESP32 GPIO2→MX1919 IN1；

ESP32 GPIO3→MX1919 IN2；

ESP32 GPIO10→MX1919 IN3；

ESP32 GPIO6→MX1919 IN4；

左测速模块 OUT→ESP32 GPIO0；

右测速模块 OUT→ESP32 GPIO1；

超声波 Trig→ESP32 GPIO18；

超声波 Echo→ESP32 GPIO19。

电机连接

左电机→MX1919 A+、A-；

右电机→MX1919 B+、B-。

void initGPIO() {

// 电机驱动引脚（IN1-IN4）

pinMode(IN1, OUTPUT); // GPIO2

pinMode(IN2, OUTPUT); // GPIO3

pinMode(IN3, OUTPUT); // GPIO10

pinMode(IN4, OUTPUT); // GPIO6

// 超声波模块引脚

pinMode(TRIG, OUTPUT); // GPIO18

pinMode(ECHO, INPUT); // GPIO19

// 测速模块引脚（外部中断）

pinMode(LEFT_SPEED, INPUT_PULLUP); // GPIO0

pinMode(RIGHT_SPEED, INPUT_PULLUP); // GPIO1

// 初始化电机停止状态

digitalWrite(IN1, LOW);

digitalWrite(IN2, LOW);

digitalWrite(IN3, LOW);

digitalWrite(IN4, LOW);

}

void initPWM() {

// 左电机PWM（IN1为PWM输出）

ledcSetup(LEFT_PWM_CHANNEL, 25000, 10); // 频率25kHz，分辨率10位（0-1023）

ledcAttachPin(IN1, LEFT_PWM_CHANNEL); // 绑定IN1（GPIO2）到PWM通道0

ledcAttachPin(IN2, LEFT_PWM_CHANNEL); // 绑定IN2（GPIO3）到PWM通道0

// 右电机PWM（IN3为PWM输出）

ledcSetup(RIGHT_PWM_CHANNEL, 25000, 10); // 频率25kHz，分辨率10位

ledcAttachPin(IN3, RIGHT_PWM_CHANNEL); // 绑定IN3（GPIO10）到PWM通道1

ledcAttachPin(IN4, RIGHT_PWM_CHANNEL); // 绑定IN4（GPIO6）到PWM通道1

// 初始占空比0（停止）

ledcWrite(LEFT_PWM_CHANNEL, 0);

ledcWrite(RIGHT_PWM_CHANNEL, 0);

}

void initInterrupt() {

// 左测速模块：上升沿触发中断

attachInterrupt(digitalPinToInterrupt(LEFT_SPEED), leftSpeedISR, RISING);

// 右测速模块：上升沿触发中断

attachInterrupt(digitalPinToInterrupt(RIGHT_SPEED), rightSpeedISR, RISING);

}

// 左测速中断服务程序（计数脉冲）

void leftSpeedISR() {

leftPulseCount++;

}

// 右测速中断服务程序（计数脉冲）

void rightSpeedISR() {

rightPulseCount++;

}

void initBLE() {

BLEDevice::init("ESP32-Car"); // 设备名称

BLEServer \*pServer = BLEDevice::createServer(); // 创建BLE服务器

BLEService \*pService = pServer-\>createService(SERVICE_UUID); // 创建服务

// 创建特征值（用于收发数据）

pTxCharacteristic = pService-\>createCharacteristic(

CHARACTERISTIC_UUID_TX,

BLECharacteristic::PROPERTY_READ \| BLECharacteristic::PROPERTY_NOTIFY

);

pRxCharacteristic = pService-\>createCharacteristic(

CHARACTERISTIC_UUID_RX,

BLECharacteristic::PROPERTY_WRITE

);

// 设置接收回调函数

pRxCharacteristic-\>setCallbacks(new MyCallbacks());

pService-\>start(); // 启动服务

pServer-\>getAdvertising()-\>start(); // 开始广播

Serial.println("BLE初始化完成，等待连接...");

}

void initTimer() {

// 定时器0：10ms触发一次（用于PID计算）

timer = timerBegin(0, 80, true); // 预分频80（时钟80MHz/80=1MHz）

timerAttachInterrupt(timer, &timerISR, true); // 绑定中断函数

timerAlarmWrite(timer, 10000, true); // 10000us = 10ms

timerAlarmEnable(timer); // 使能定时器

}

// 定时器中断服务程序（执行PID计算）

void IRAM_ATTR timerISR() {

pidFlag = true; // 置位PID计算标志

}

void calculateSpeed() {

// 读取脉冲数（关闭中断防止计数错误）

noInterrupts();

leftPulse = leftPulseCount;

rightPulse = rightPulseCount;

leftPulseCount = 0;

rightPulseCount = 0;

interrupts();

// 计算转速（RPM）

leftSpeed = leftPulse \* 30;

rightSpeed = rightPulse \* 30;

}

float measureDistance() {

digitalWrite(TRIG, LOW);

delayMicroseconds(2);

digitalWrite(TRIG, HIGH);

delayMicroseconds(10);

digitalWrite(TRIG, LOW);

long duration = pulseIn(ECHO, HIGH); // 读取回声时间（μs）

float distance = duration \* 0.034 / 2; // 计算距离（cm）

// 中位数滤波（5次采样）

static float distBuffer\[5\];

static int bufIndex = 0;

distBuffer\[bufIndex++\] = distance;

if (bufIndex \>= 5) bufIndex = 0;

// 排序取中位数

float sorted\[5\];

memcpy(sorted, distBuffer, sizeof(distBuffer));

for (int i = 0; i \< 4; i++) {

for (int j = 0; j \< 4 - i; j++) {

if (sorted\[j\] \> sorted\[j + 1\]) {

float temp = sorted\[j\];

sorted\[j\] = sorted\[j + 1\];

sorted\[j + 1\] = temp;

}

}

}

return sorted\[2\]; // 返回中位数

}

void setMotorForward(int speed) {

// 左电机正转：IN1=H，IN2=L

digitalWrite(IN1, HIGH);

digitalWrite(IN2, LOW);

// 右电机正转：IN3=H，IN4=L

digitalWrite(IN3, HIGH);

digitalWrite(IN4, LOW);

// 设置PWM占空比（转速）

setMotorSpeed(speed, speed);

}

void setMotorBackward(int speed) {

// 左电机反转：IN1=L，IN2=H

digitalWrite(IN1, LOW);

digitalWrite(IN2, HIGH);

// 右电机反转：IN3=L，IN4=H

digitalWrite(IN3, LOW);

digitalWrite(IN4, HIGH);

// 设置PWM占空比（转速）

setMotorSpeed(speed, speed);

}

void setMotorStop() {

// 所有控制引脚置低

digitalWrite(IN1, LOW);

digitalWrite(IN2, LOW);

digitalWrite(IN3, LOW);

digitalWrite(IN4, LOW);

// PWM占空比0

setMotorSpeed(0, 0);

}

void setMotorLeft(int speed) {

digitalWrite(IN1, HIGH);

digitalWrite(IN2, LOW);

digitalWrite(IN3, HIGH);

digitalWrite(IN4, LOW);

// 左电机转速为右电机的50%（差速转向）

setMotorSpeed(speed \* 0.5, speed);

}

void setMotorRight(int speed) {

digitalWrite(IN1, HIGH);

digitalWrite(IN2, LOW);

digitalWrite(IN3, HIGH);

digitalWrite(IN4, LOW);

// 右电机转速为左电机的50%（差速转向）

setMotorSpeed(speed, speed \* 0.5);

}

void setMotorSpeed(int leftSpeed, int rightSpeed) {

// 限制转速范围0-350RPM（电机最大转速）

leftSpeed = constrain(leftSpeed, 0, 350);

rightSpeed = constrain(rightSpeed, 0, 350);

// 转速→占空比映射（350RPM对应100%占空比）

int leftDuty = map(leftSpeed, 0, 350, 0, 1023);

int rightDuty = map(rightSpeed, 0, 350, 0, 1023);

// 输出PWM

ledcWrite(LEFT_PWM_CHANNEL, leftDuty);

ledcWrite(RIGHT_PWM_CHANNEL, rightDuty);

}

// PID参数结构体

typedef struct {

float kp; // 比例系数

float ki; // 积分系数

float kd; // 微分系数

float target; // 目标值

float actual; // 实际值

float error; // 当前误差

float lastError;// 上一次误差

float integral; // 积分项

float derivative;// 微分项

float output; // 输出

float maxOut; // 输出最大值

float minOut; // 输出最小值

} PID_t;

// 左、右电机PID实例

PID_t leftPID = {1.2, 0.05, 0.1, 0, 0, 0, 0, 0, 0, 0, 1023, 0};

PID_t rightPID = {1.2, 0.05, 0.1, 0, 0, 0, 0, 0, 0, 0, 1023, 0};

void pidCalculate(PID_t \*pid) {

// 计算当前误差

pid-\>error = pid-\>target - pid-\>actual;

// 比例项

pid-\>output = pid-\>kp \* pid-\>error;

// 积分项（抗积分饱和：输出未达限幅时才累加）

if (pid-\>output \< pid-\>maxOut && pid-\>output \> pid-\>minOut) {

pid-\>integral += pid-\>ki \* pid-\>error \* 0.01; // 采样周期0.01s

// 积分限幅（防止积分饱和）

pid-\>integral = constrain(pid-\>integral, -pid-\>maxOut, pid-\>maxOut);

pid-\>output += pid-\>integral;

}

// 微分项（抑制高频噪声）

pid-\>derivative = pid-\>kd \* (pid-\>error - pid-\>lastError) / 0.01;

pid-\>output += pid-\>derivative;

// 输出限幅

pid-\>output = constrain(pid-\>output, pid-\>minOut, pid-\>maxOut);

// 保存当前误差

pid-\>lastError = pid-\>error;

}

void pidControlLoop() {

// 更新实际转速

leftPID.actual = leftSpeed;

rightPID.actual = rightSpeed;

// 计算PID输出

pidCalculate(&leftPID);

pidCalculate(&rightPID);

// 输出PWM

ledcWrite(LEFT_PWM_CHANNEL, (int)leftPID.output);

ledcWrite(RIGHT_PWM_CHANNEL, (int)rightPID.output);

}

void obstacleAvoidance() {

float distance = measureDistance(); // 获取距离

if (distance \>= 30) {

// 距离充足，正常前进

setMotorForward(leftPID.target);

} else if (distance \>= 20 && distance \< 30) {

// 距离较近，减速前进

setMotorForward(leftPID.target \* 0.5);

} else {

// 距离过近，避障

setMotorStop();

delay(500); // 停止0.5s

// 随机左转或右转

if (random(2) == 0) {

setMotorLeft(leftPID.target \* 0.7);

} else {

setMotorRight(leftPID.target \* 0.7);

}

delay(1000); // 转向1s

}

}

typedef enum {

STATE_FORWARD, // 前进

STATE_DECELERATE,// 减速

STATE_STOP, // 停止

STATE_TURN_LEFT, // 左转

STATE_TURN_RIGHT // 右转

} ObstacleState;

ObstacleState currentState = STATE_FORWARD;

void obstacleStateMachine() {

float distance = measureDistance();

switch (currentState) {

case STATE_FORWARD:

if (distance \< 20) currentState = STATE_STOP;

else if (distance \< 30) currentState = STATE_DECELERATE;

break;

case STATE_DECELERATE:

if (distance \>= 30) currentState = STATE_FORWARD;

else if (distance \< 20) currentState = STATE_STOP;

break;

case STATE_STOP:

currentState = (random(2) == 0) ? STATE_TURN_LEFT : STATE_TURN_RIGHT;

break;

case STATE_TURN_LEFT:

case STATE_TURN_RIGHT:

currentState = STATE_FORWARD; // 转向后恢复前进

break;

}

// 执行状态动作

switch (currentState) {

case STATE_FORWARD: setMotorForward(leftPID.target); break;

case STATE_DECELERATE: setMotorForward(leftPID.target \* 0.5); break;

case STATE_STOP: setMotorStop(); break;

case STATE_TURN_LEFT: setMotorLeft(leftPID.target \* 0.7); break;

case STATE_TURN_RIGHT: setMotorRight(leftPID.target \* 0.7); break;

}

}

#include <Arduino.h>
#include <BLE2902.h>
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
// PID 控制参数
struct PIDConfig {
    float Kp = 15.0;          // 比例系数
    float Ki = 0.0;           // 积分系数
    float Kd = 0.0;           // 微分系数
    float targetRPM = 0.0;    // 目标转速 (RPM)
    float currentRPM = 0.0;   // 当前转速 (RPM)
    float currentPWM = 0.0;   // 当前PWM值
    float lastSetRPM = 100.0; // 保存最后设置的RPM值，默认100
    float outputMin = 0.0;
    float outputMax = 1023.0; // 最大PWM值为1023
    float errorPrev = 0.0;
    float integral = 0.0;
    bool isFirstMove = true;    // 用于跟踪是否是初始移动
    const float maxRPM = 350.0; // 最大转速为350 RPM
    // 打印当前PID参数
    void printPIDParams() {
        Serial.println("\n当前PID参数：");
        Serial.print("Kp = ");
        Serial.println(Kp, 3);
        Serial.print("Ki = ");
        Serial.println(Ki, 3);
        Serial.print("Kd = ");
        Serial.println(Kd, 3);
        Serial.println("------------------------");
    }
};
// 创建全局PIDConfig实例
PIDConfig pidConfig;
// 串口绘图开关
bool enablePlotter = false;
BLEServer *pServer = NULL;
BLECharacteristic *pTxCharacteristic;
bool deviceConnected = false;
int PWM_Left;
int PWM_Right;
int rcv;
int IN1 = 2;
int IN2 = 3;
int IN3 = 10;
int IN4 = 6;
int ControlMode;
const int freq = 25000;    // 设置频率
const int resolution = 10; // 计数位数，取值 0 ~ 20
int max_PWMValue = pow(2, resolution) - 1;
float dutyCycle = 0.8;
double SpeedLeft;
double SpeedRight;
String String_Received;
unsigned long lastMsg;
// 超声波传感器引脚定义
const int trigPin = 18;
const int echoPin = 19;
// 避障距离阈值(厘米)
const int SLOW_DISTANCE = 30; // 减速距离阈值
const int STOP_DISTANCE = 20; // 停止距离阈值
// 避障状态标志
bool isAvoidingObstacle = false;
// 避障持续时间(毫秒)
unsigned long obstacleAvoidanceStartTime = 0;
const unsigned long obstacleAvoidanceDuration = 2000; // 避障持续2秒
// UUID 定义
#define SERVICE_UUID "6E400001-B5A3-F393-E0A9-E50E24DCCA9E"
#define CHARACTERISTIC_UUID_RX "6E400002-B5A3-F393-E0A9-E50E24DCCA9E"
#define CHARACTERISTIC_UUID_TX "6E400003-B5A3-F393-E0A9-E50E24DCCA9E"
// 存储避障前的状态
struct AvoidanceMemory {
    float originalPWM;
    int originalDirection;
    float originalRPM;
} avoidanceMemory;
// 将0-1023的PWM值映射到0-255范围
int mapPWM(int value) { return map(value, 0, 1023, 0, 255); }
// 提前声明所有控制函数
void stop() {
    analogWrite(IN1, 0);
    analogWrite(IN2, 0);
    analogWrite(IN3, 0);
    analogWrite(IN4, 0);
    Serial.println("Stop");
}
void Forward();    // 前向声明
void Backward();   // 前向声明
void turn_left();  // 前向声明
void turn_right(); // 前向声明
class MyServerCallbacks : public BLEServerCallbacks {
    void onConnect(BLEServer *pServer) {
        Serial.println("蓝牙已连接");
        deviceConnected = true;
    };
    void onDisconnect(BLEServer *pServer) {
        Serial.println("蓝牙已断开");
        deviceConnected = false;
        delay(500);
        pServer->startAdvertising();
    }
};
// 数据接收回调类
class MyCallbacks : public BLECharacteristicCallbacks {
    void onWrite(BLECharacteristic *pCharacteristic) {
        String rxValue(pCharacteristic->getValue().c_str());
        if (rxValue.length() > 0) {
            Serial.println("*********");
            Serial.print("收到新信息: ");
            String_Received = "";
            for (int i = 0; i < rxValue.length(); i++) {
                String_Received += (char)rxValue[i];
                Serial.print(rxValue[i]);
            }
            Serial.println("*********");
            // 检查是否是RPM设置命令
            if (String_Received.endsWith("RPM")) {
                String TempString = String_Received.substring(0, String_Received.length() - 3);
                // 直接使用输入值作为RPM，范围0-255
                float newRPM = constrain(TempString.toFloat(), 0, pidConfig.maxRPM);
                // 设置目标RPM和保存的RPM
                pidConfig.targetRPM = newRPM;
                pidConfig.lastSetRPM = newRPM; // 保存设置的RPM值
                // 初始PWM估计值 (非线性映射，考虑电机特性)
                float initialPWM;
                if (newRPM < 70) { // 低速区域
                    initialPWM = map(newRPM, 0, 70, 0, 400);
                } else if (newRPM < 200) { // 中速区域
                    initialPWM = map(newRPM, 70, 200, 400, 700);
                } else { // 高速区域
                    initialPWM = map(newRPM, 200, 350, 700, pidConfig.outputMax);
                }
                // 设置PWM值
                pidConfig.currentPWM = initialPWM;
                PWM_Left = initialPWM;
                PWM_Right = initialPWM;
                Serial.print("设置目标RPM为: ");
                Serial.print(newRPM, 2);
                Serial.print(" 初始PWM值: ");
                Serial.println(initialPWM);
            } else {
                rcv = String_Received.toInt();
                if (rcv == 5) { // 停止命令
                    pidConfig.targetRPM = 0;
                    pidConfig.currentPWM = 0;
                    PWM_Left = 0;
                    PWM_Right = 0;
                    stop();
                } else if (rcv >= 1 && rcv <= 4) {
                    // 使用保存的RPM值
                    pidConfig.targetRPM = pidConfig.lastSetRPM;
                    // 根据保存的RPM值计算初始PWM
                    float initialPWM;
                    if (pidConfig.lastSetRPM < 70) {
                        initialPWM = map(pidConfig.lastSetRPM, 0, 70, 0, 400);
                    } else if (pidConfig.lastSetRPM < 200) {
                        initialPWM = map(pidConfig.lastSetRPM, 70, 200, 400, 700);
                    } else {
                        initialPWM = map(pidConfig.lastSetRPM, 200, 350, 700, pidConfig.outputMax);
                    }
                    pidConfig.currentPWM = initialPWM;
                    PWM_Left = initialPWM;
                    PWM_Right = initialPWM;
                    Serial.print("使用保存的RPM值: ");
                    Serial.print(pidConfig.lastSetRPM);
                    Serial.print(" PWM值: ");
                    Serial.println(initialPWM);
                }
                Serial.print("rcv: ");
                Serial.println(rcv);
            }
        }
    }
};
// 实现其他控制函数
void Forward() {
    // 同步更新两个轮子的PWM值
    int mappedLeft = (PWM_Left < 50) ? 0 : mapPWM(PWM_Left);
    int mappedRight = (PWM_Right < 50) ? 0 : mapPWM(PWM_Right);
    // 同时设置所有PWM值 - 两个电机都向前
    analogWrite(IN1, 0);
    analogWrite(IN2, mappedRight);
    analogWrite(IN3, mappedLeft);
    analogWrite(IN4, 0);
    Serial.print("Forward - PWM Left: ");
    Serial.print(PWM_Left);
    Serial.print(" PWM Right: ");
    Serial.println(PWM_Right);
}
void Backward() {
    // 同步更新两个轮子的PWM值
    int mappedLeft = (PWM_Left < 50) ? 0 : mapPWM(PWM_Left);
    int mappedRight = (PWM_Right < 50) ? 0 : mapPWM(PWM_Right);
    // 同时设置所有PWM值 - 两个电机都向后
    analogWrite(IN1, mappedRight);
    analogWrite(IN2, 0);
    analogWrite(IN3, 0);
    analogWrite(IN4, mappedLeft);
    Serial.print("Backward - PWM Left: ");
    Serial.print(PWM_Left);
    Serial.print(" PWM Right: ");
    Serial.println(PWM_Right);
}
void turn_left() {
    // 左转时保持两个轮子的独立控制
    int mappedLeft = (PWM_Left < 50) ? 0 : mapPWM(PWM_Left);
    int mappedRight = (PWM_Right < 50) ? 0 : mapPWM(PWM_Right);
    // 左轮后退，右轮前进
    analogWrite(IN1, 0);
    analogWrite(IN2, mappedRight);
    analogWrite(IN3, 0);
    analogWrite(IN4, mappedLeft);
    Serial.print("Turn Left - PWM Left: ");
    Serial.print(PWM_Left);
    Serial.print(" PWM Right: ");
    Serial.println(PWM_Right);
}
void turn_right() {
    // 右转时保持两个轮子的独立控制
    int mappedLeft = (PWM_Left < 50) ? 0 : mapPWM(PWM_Left);
    int mappedRight = (PWM_Right < 50) ? 0 : mapPWM(PWM_Right);
    // 左轮前进，右轮后退
    analogWrite(IN1, mappedRight);
    analogWrite(IN2, 0);
    analogWrite(IN3, mappedLeft);
    analogWrite(IN4, 0);
    Serial.print("Turn Right - PWM Left: ");
    Serial.print(PWM_Left);
    Serial.print(" PWM Right: ");
    Serial.println(PWM_Right);
}
struct SystemConfig {
    const int interruptPinLeft = 0;
    const int interruptPinRight = 1;
    const int pulsesPerRevolution = 40;
    const float wheelCircumference = 20;
    const long interval = 100;
    const int serialBaudRate = 115200;
};
struct SensorData {
    volatile int counter_left = 0;
    volatile int counter_right = 0;
    unsigned long previousMillis = 0;
};
struct SpeedData {
    float leftRPM = 0.0;
    float rightRPM = 0.0;
    float leftSpeed = 0.0;
    float rightSpeed = 0.0;
    int leftPulses = 0;
    int rightPulses = 0;
};
SystemConfig config;
SensorData sensorData;
SpeedData speedData;
void IRAM_ATTR handleInterruptLeft() { sensorData.counter_left++; }
void IRAM_ATTR handleInterruptRight() { sensorData.counter_right++; }
void setupSystem() {
    Serial.begin(config.serialBaudRate);
    while (!Serial) {
        ;
    }
    Serial.println("ESP32-C3 智能小车测速系统启动");
    pinMode(config.interruptPinLeft, INPUT_PULLDOWN);
    pinMode(config.interruptPinRight, INPUT_PULLDOWN);
    attachInterrupt(digitalPinToInterrupt(config.interruptPinLeft), handleInterruptLeft, CHANGE);
    attachInterrupt(digitalPinToInterrupt(config.interruptPinRight), handleInterruptRight, CHANGE);
    // 超声波传感器初始化
    pinMode(trigPin, OUTPUT);
    pinMode(echoPin, INPUT);
    Serial.println("\n系统配置:");
    Serial.print("脉冲/圈: ");
    Serial.println(config.pulsesPerRevolution);
    Serial.print("轮子周长(米): ");
    Serial.println(config.wheelCircumference);
    Serial.print("测速间隔(毫秒): ");
    Serial.println(config.interval);
    Serial.print("超声波避障阈值(厘米): ");
    Serial.println(SLOW_DISTANCE);
    Serial.println("------------------------");
}
// PID 控制器函数
float computePID(float targetRPM, float currentRPM, float /*currentPWM unused now*/) {
    // 如果目标转速为 0，直接返回 0 并重置状态
    if (targetRPM == 0) {
        pidConfig.integral = 0;
        pidConfig.errorPrev = 0;
        return 0;
    }
    const float dt = 0.1f;                // 采样周期 (s)
    float error = targetRPM - currentRPM; // 误差 (RPM)
    // ---------- P ----------
    float pTerm = pidConfig.Kp * error;
    // ---------- I ---------- (带防积分饱和)
    pidConfig.integral += error * dt;
    pidConfig.integral = constrain(pidConfig.integral, -2000.0f, 2000.0f);
    float iTerm = pidConfig.Ki * pidConfig.integral;
    // ---------- D ----------
    float derivative = (error - pidConfig.errorPrev) / dt;
    float dTerm = pidConfig.Kd * derivative;
    pidConfig.errorPrev = error;
    // PID 输出 (绝对 PWM)
    float output = pTerm + iTerm + dTerm;
    float newPWM = constrain(output, pidConfig.outputMin, pidConfig.outputMax);
    // 抗积分饱和：如果输出饱和，扣除本次积分增量
    if (newPWM == pidConfig.outputMax || newPWM == pidConfig.outputMin) {
        pidConfig.integral -= error * dt; // 撤销本次积分
    }
    // 绘图输出 (目标RPM,当前RPM,误差,P,I,D,PWM)
    if (enablePlotter) {
        Serial.print(targetRPM);
        Serial.print(",");
        Serial.print(currentRPM);
        Serial.print(",");
        Serial.print(error);
        Serial.print(",");
        Serial.print(pTerm);
        Serial.print(",");
        Serial.print(iTerm);
        Serial.print(",");
        Serial.print(dTerm);
        Serial.print(",");
        Serial.println(newPWM);
    }
    return newPWM;
}
// 计算速度
void calculateSpeed() {
    noInterrupts();
    speedData.leftPulses = sensorData.counter_left;
    speedData.rightPulses = sensorData.counter_right;
    sensorData.counter_left = 0;
    sensorData.counter_right = 0;
    interrupts();
    // 计算每秒转数
    float timeSeconds = config.interval / 1000.0;
    float leftRevolutionsPerSecond = (speedData.leftPulses / (float)config.pulsesPerRevolution) / timeSeconds;
    float rightRevolutionsPerSecond = (speedData.rightPulses / (float)config.pulsesPerRevolution) / timeSeconds;
    // 计算RPM并应用低通滤波
    const float alpha = 0.3; // 滤波系数 (0-1)
    float newLeftRPM = leftRevolutionsPerSecond * 60.0;
    float newRightRPM = rightRevolutionsPerSecond * 60.0;
    speedData.leftRPM = alpha * newLeftRPM + (1 - alpha) * speedData.leftRPM;
    speedData.rightRPM = alpha * newRightRPM + (1 - alpha) * speedData.rightRPM;
    // 计算线速度 (cm/s)
    speedData.leftSpeed = leftRevolutionsPerSecond * config.wheelCircumference;
    speedData.rightSpeed = rightRevolutionsPerSecond * config.wheelCircumference;
    if (enablePlotter) {
        Serial.print("Raw RPM: ");
        Serial.print(newLeftRPM);
        Serial.print(" Filtered RPM: ");
        Serial.println(speedData.leftRPM);
    }
}
// 打印速度数据
void printSpeedData() {
    Serial.print("左轮: 脉冲=");
    Serial.print(speedData.leftPulses);
    Serial.print(" | 转速=");
    Serial.print(speedData.leftRPM, 2);
    Serial.print(" RPM | 线速度=");
    Serial.print(speedData.leftSpeed, 3);
    Serial.print(" cm/s | ");
    Serial.print("右轮: 脉冲=");
    Serial.print(speedData.rightPulses);
    Serial.print(" | 转速=");
    Serial.print(speedData.rightRPM, 2);
    Serial.print(" RPM | 线速度=");
    Serial.print(speedData.rightSpeed, 3);
    Serial.println(" cm/s");
}
// 检测前方障碍物距离
float getObstacleDistance() {
    // 发送触发信号
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    // 读取回声信号
    long duration = pulseIn(echoPin, HIGH);
    // 计算距离(声速 340m/s，来回除以 2)
    float distance = duration * 0.034 / 2;
    return distance;
}
// 执行避障动作
void performObstacleAvoidance() {
    Serial.println("检测到障碍物，执行避障");
    isAvoidingObstacle = true;
    obstacleAvoidanceStartTime = millis();
    // 保存当前状态
    avoidanceMemory.originalPWM = pidConfig.currentPWM;
    avoidanceMemory.originalDirection = rcv;
    avoidanceMemory.originalRPM = pidConfig.targetRPM;
    // 先停止
    stop();
    delay(200); // 短暂停顿
    // 后退 - 使用较低的速度
    rcv = 2;                   // 设置为后退模式
    pidConfig.targetRPM = 100; // 使用较低的避障速度
    float backupPWM = map(100, 0, 255, 0, pidConfig.outputMax);
    pidConfig.currentPWM = backupPWM;
    PWM_Left = backupPWM;
    PWM_Right = backupPWM;
    Backward();  // 使用Backward来后退
    delay(1000); // 后退1秒
    // 随机选择转向和转向时间
    int turnTime = random(400, 1000); // 随机生成400-1000毫秒的转向时间
    if (random(2) == 0) {
        Serial.print("避障：向左转 ");
        Serial.print(turnTime);
        Serial.println("毫秒");
        rcv = 4;
        turn_left();
    } else {
        Serial.print("避障：向右转 ");
        Serial.print(turnTime);
        Serial.println("毫秒");
        rcv = 3;
        turn_right();
    }
    delay(turnTime); // 使用随机时间进行转向
    // 恢复原来的方向和速度
    rcv = avoidanceMemory.originalDirection;
    pidConfig.targetRPM = avoidanceMemory.originalRPM;
    pidConfig.currentPWM = avoidanceMemory.originalPWM;
    PWM_Left = avoidanceMemory.originalPWM;
    PWM_Right = avoidanceMemory.originalPWM;
    isAvoidingObstacle = false;
    Serial.println("避障完成，恢复原始状态");
    Serial.print("恢复方向: ");
    switch (rcv) {
    case 1:
        Serial.println("前进");
        Forward();
        break;
    case 2:
        Serial.println("后退");
        Backward();
        break;
    case 3:
        Serial.println("右转");
        turn_right();
        break;
    case 4:
        Serial.println("左转");
        turn_left();
        break;
    default:
        Serial.println("停止");
        stop();
        break;
    }
    Serial.print("恢复速度 RPM: ");
    Serial.print(pidConfig.targetRPM);
    Serial.print(" PWM: ");
    Serial.println(pidConfig.currentPWM);
}
// 根据距离调整速度
void adjustSpeedForObstacle(float distance) {
    if (distance < STOP_DISTANCE && rcv == 1) { // 只在前进时执行避障
        // 距离小于20cm，执行避障
        performObstacleAvoidance();
    } else if (distance < SLOW_DISTANCE && rcv == 1) { // 只在前进时执行减速
        // 距离小于30cm，减速
        float speedRatio = (distance - STOP_DISTANCE) / (SLOW_DISTANCE - STOP_DISTANCE);
        float targetPWM = avoidanceMemory.originalPWM * speedRatio;
        // 逐渐减速
        pidConfig.currentPWM = targetPWM;
        PWM_Left = targetPWM;
        PWM_Right = targetPWM;
        Serial.print("减速运行 - 距离: ");
        Serial.print(distance);
        Serial.print("cm, 速度比例: ");
        Serial.println(speedRatio);
    }
}
// 处理串口命令
void processSerialCommand() {
    if (Serial.available() > 0) {
        String command = Serial.readStringUntil('\n');
        command.trim(); // 移除多余的空格和换行符
        // 解析命令
        if (command.startsWith("PID")) {
            // 格式：PID p i d
            // 示例：PID 15.0 0.0 0.0
            command = command.substring(4); // 跳过"PID "
            float p = command.substring(0, command.indexOf(' ')).toFloat();
            command = command.substring(command.indexOf(' ') + 1);
            float i = command.substring(0, command.indexOf(' ')).toFloat();
            command = command.substring(command.indexOf(' ') + 1);
            float d = command.toFloat();
            // 更新PID参数
            pidConfig.Kp = p;
            pidConfig.Ki = i;
            pidConfig.Kd = d;
            // 打印更新后的参数
            if (!enablePlotter) {
                pidConfig.printPIDParams();
            }
        } else if (command == "GET_PID") {
            // 打印当前PID参数
            if (!enablePlotter) {
                pidConfig.printPIDParams();
            }
        } else if (command == "PLOT_ON") {
            enablePlotter = true;
            Serial.println("目标RPM,当前RPM,PWM值,误差");
        } else if (command == "PLOT_OFF") {
            enablePlotter = false;
            Serial.println("串口绘图已关闭");
        } else if (command == "HELP") {
            // 打印帮助信息
            if (!enablePlotter) {
                Serial.println("\nPID参数调整命令说明：");
                Serial.println("1. 设置PID参数：PID p i d");
                Serial.println(" 示例：PID 15.0 0.0 0.0");
                Serial.println("2. 获取当前PID参数：GET_PID");
                Serial.println("3. 开启串口绘图：PLOT_ON");
                Serial.println("4. 关闭串口绘图：PLOT_OFF");
                Serial.println("5. 显示帮助信息：HELP");
                Serial.println("------------------------");
            }
        }
    }
}
// 输出绘图数据
void outputPlotData() {
    if (enablePlotter) {
        float error = pidConfig.targetRPM - pidConfig.currentRPM;
        // 计算各个控制项的值
        float pTerm = pidConfig.Kp * error;
        float iTerm = pidConfig.Ki * pidConfig.integral;
        float dTerm = pidConfig.Kd * (error - pidConfig.errorPrev) / 0.1;
        Serial.print("目标RPM:");
        Serial.print(pidConfig.targetRPM);
        Serial.print(",当前RPM:");
        Serial.print(pidConfig.currentRPM);
        Serial.print(",PWM:");
        Serial.print(pidConfig.currentPWM);
        Serial.print(",P项:");
        Serial.print(pTerm);
        Serial.print(",I项:");
        Serial.print(iTerm);
        Serial.print(",D项:");
        Serial.println(dTerm);
    }
}
void setup() {
    setupSystem();
    Serial.begin(115200);
    lastMsg = 0;
    BLEBegin();
    ControlMode = 1;
    // 设置引脚模式
    pinMode(IN1, OUTPUT);
    pinMode(IN2, OUTPUT);
    pinMode(IN3, OUTPUT);
    pinMode(IN4, OUTPUT);
    // 初始化为停止状态
    stop();
    rcv = 5;
}
void loop() {
    unsigned long currentMillis = millis();
    // 处理串口命令
    processSerialCommand();
    // 距离检测与避障逻辑
    static unsigned long lastObstacleCheck = 0;
    if (currentMillis - lastObstacleCheck >= 100) { // 提高检测频率到100ms
        if (!isAvoidingObstacle && rcv != 5) {      // 只在非停止状态检测障碍物
            float distance = getObstacleDistance();
            // 打印距离信息（降低打印频率）
            static unsigned long lastDistancePrint = 0;
            if (currentMillis - lastDistancePrint >= 500 && !enablePlotter) {
                Serial.print("前方距离: ");
                Serial.print(distance);
                Serial.println(" cm");
                lastDistancePrint = currentMillis;
            }
            // 根据距离调整速度或执行避障
            adjustSpeedForObstacle(distance);
        }
        lastObstacleCheck = currentMillis;
    }
    // 更新速度数据
    if (currentMillis - sensorData.previousMillis >= 100) {
        sensorData.previousMillis = currentMillis;
        // 计算速度
        calculateSpeed();
        // 更新当前转速 (使用左轮速度作为参考)
        pidConfig.currentRPM = speedData.leftRPM;
        // 仅在非停止状态下应用PID控制
        if (rcv != 5) {
            // 计算新的PWM值
            float newPWM = computePID(pidConfig.targetRPM, pidConfig.currentRPM, pidConfig.currentPWM);
            pidConfig.currentPWM = newPWM;
            // 更新两个轮子的PWM值
            PWM_Left = (int)newPWM;
            PWM_Right = (int)newPWM;
            // 根据当前控制模式执行相应动作
            switch (rcv) {
            case 1:
                Forward();
                break;
            case 2:
                Backward();
                break;
            case 3:
                turn_right();
                break;
            case 4:
                turn_left();
                break;
            default:
                if (PWM_Left > 0) {
                    Forward(); // 默认前进
                }
                break;
            }
        }
        // 输出绘图数据
        outputPlotData();
        // 打印调试信息（降低打印频率，且只在非绘图模式下打印）
        static unsigned long lastPrint = 0;
        if (currentMillis - lastPrint >= 500 && !enablePlotter) {
            float errorRPM = pidConfig.targetRPM - pidConfig.currentRPM;
            float errorPercent = (pidConfig.targetRPM != 0) ? (errorRPM / pidConfig.targetRPM) * 100 : 0;
            Serial.println("\n-------- 控制状态信息 --------");
            Serial.print("控制模式: ");
            switch (rcv) {
            case 1:
                Serial.println("前进");
                break;
            case 2:
                Serial.println("后退");
                break;
            case 3:
                Serial.println("右转");
                break;
            case 4:
                Serial.println("左转");
                break;
            case 5:
                Serial.println("停止");
                break;
            default:
                Serial.println("未知");
                break;
            }
            Serial.println("\n速度数据:");
            Serial.print("目标RPM: ");
            Serial.print(pidConfig.targetRPM);
            Serial.print(" | 当前RPM: ");
            Serial.print(pidConfig.currentRPM);
            Serial.print(" | 误差RPM: ");
            Serial.print(errorRPM);
            Serial.print(" (");
            Serial.print(errorPercent, 1);
            Serial.print("%)");
            Serial.print(" | PWM值: ");
            Serial.println(pidConfig.currentPWM);
            Serial.println("\n------------------------\n");
            lastPrint = currentMillis;
        }
    }
    // 蓝牙通信部分
    if (deviceConnected) {
        unsigned long now = millis();
        if (now - lastMsg > 1000) {
            String stra = String(PWM_Left);
            String strb = ",";
            String strc = String(PWM_Right);
            String str_Send = stra + strb + strc;
            pTxCharacteristic->setValue((char *)str_Send.c_str());
            pTxCharacteristic->notify();
            lastMsg = now;
        }
    }
}
void BLEBegin() {
    BLEDevice::init("C3");
    pServer = BLEDevice::createServer();
    pServer->setCallbacks(new MyServerCallbacks());
    BLEService *pService = pServer->createService(SERVICE_UUID);
    pTxCharacteristic = pService->createCharacteristic(CHARACTERISTIC_UUID_TX, BLECharacteristic::PROPERTY_NOTIFY);
    pTxCharacteristic->addDescriptor(new BLE2902());
    BLECharacteristic *pRxCharacteristic =
        pService->createCharacteristic(CHARACTERISTIC_UUID_RX, BLECharacteristic::PROPERTY_WRITE);
    pRxCharacteristic->setCallbacks(new MyCallbacks());
    pService->start();
    pServer->getAdvertising()->start();
    Serial.println("Waiting a client connection to notify...");
}