肥仔教程网

通过串口调整KP,KI,KD，初步实现直立平衡

#include <Wire.h>
#include <MPU6050.h>


MPU6050 mpu;

// PID参数
float kp = 14.0;   // 比例系数
float ki = 0.6;    // 积分系数
float kd = 0.8;    // 微分系数
float ki_limit = 50.0; // 积分限幅

// 控制参数
float target_angle = -5.0;
const int motor_deadzone = 30;
const int motor_max = 255;
const float max_angle = 40.0;   // 最大允许角度
const float min_angle = -40.0;  // 最小允许角度

// PID计算变量
float error = 0, last_error = 0;
float integral = 0, derivative = 0;
float output = 0;

// 控制模式
enum ControlMode { MANUAL, AUTOMATIC };
ControlMode control_mode = AUTOMATIC;

// MPU变量
float angleY = 0;
float gyroY = 0;
unsigned long last_time = 0;

// 电机引脚定义
const int motor1_pwm = 15;
const int motor1_dir = 16;
const int motor2_pwm =17;
const int motor2_dir = 18;

// I2C引脚定义
const int i2c_sda = 41;
const int i2c_scl = 42;

bool mpu_connected = false;

// 串口参数调节相关变量
bool param_adjust_mode = false;
char adjust_param_type = ' ';
String inputString = "";

void initializeMPU6050() ;
void automaticControl();
void readMPU6050();
void stopMotors();
void setMotorSpeed(int pwm, bool forward);
void serialEvent() ;
void displayStatus();
void calculatePID();
void showHelp();


void setup() {
  Serial.begin(115200);
  delay(1000);
  
  Serial.println("PID自平衡小车控制程序");
  
  // 初始化I2C
  Wire.begin(i2c_sda, i2c_scl, 400000);
  delay(100);
  
  // 初始化MPU6050
  initializeMPU6050();
  
  // 设置电机引脚
  pinMode(motor1_pwm, OUTPUT);
  pinMode(motor1_dir, OUTPUT);
  pinMode(motor2_pwm, OUTPUT);
  pinMode(motor2_dir, OUTPUT);
  
  // 初始停止电机
  stopMotors();
  
  last_time = micros();
  
  Serial.println("\n=== PID自平衡小车控制程序 ===");
  Serial.println("角度范围限制: -40° 到 +40°");
  Serial.println("目标角度: " + String(target_angle) + "°");
  Serial.println("当前PID参数: Kp=" + String(kp, 2) + " Ki=" + String(ki, 3) + " Kd=" + String(kd, 2));
  Serial.println("控制模式:");
  Serial.println("  a - 自动平衡模式");
  Serial.println("  m - 手动控制模式");
  Serial.println("手动控制命令:");
  Serial.println("  f - 正转");
  Serial.println("  b - 反转");
  Serial.println("  s - 停止");
  Serial.println("PID参数调节:");
  Serial.println("  p - 调节Kp (或直接输入 p数值，如 p14.0)");
  Serial.println("  i - 调节Ki (或直接输入 i数值，如 i0.1)");
  Serial.println("  d - 调节Kd (或直接输入 d数值，如 d0.8)");
  Serial.println("  r - 重置PID积分项");
  Serial.println("  t - 调整目标角度 (或直接输入 t数值，如 t6.0)");
  Serial.println("  ? - 显示帮助信息");
  Serial.println("当前模式: 自动平衡");
  Serial.println("----------------------------");
}

void initializeMPU6050() {
  mpu.initialize();
  mpu_connected = mpu.testConnection();
  if (mpu_connected) {
    Serial.println("MPU6050连接成功");
    
    // 配置MPU6050
    mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
    mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
    mpu.setDLPFMode(MPU6050_DLPF_BW_10);
  } else {
    Serial.println("MPU6050连接失败");
  }
}

void loop() {
  // 读取MPU6050数据（如果连接）
  if (mpu_connected) {
    readMPU6050();
  }
  
  // 根据当前模式执行相应控制
  if (control_mode == AUTOMATIC) {
    // 自动平衡模式 - 使用PID控制
    calculatePID();
    automaticControl();
  }
  
  // 显示状态
  static unsigned long last_display = 0;
  if (millis() - last_display > 500) {
    displayStatus();
    last_display = millis();
  }
  
  serialEvent();
  delay(10);
}

void readMPU6050() {
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  
  // 计算角度
  angleY = atan2(ax, az) * 180.0 / PI;
  
  // 计算陀螺仪角速度 (度/秒)
  gyroY = gy / 131.0; // ±250°/s 量程，灵敏度 131 LSB/(°/s)
  
  // 互补滤波：融合加速度计和陀螺仪数据
  unsigned long current_time = micros();
  float dt = (current_time - last_time) / 1000000.0;
  last_time = current_time;
  
  float alpha = 0.96; // 互补滤波系数
  static float angle_filtered = 0;
  angle_filtered = alpha * (angle_filtered + gyroY * dt) + (1 - alpha) * angleY;
  angleY = angle_filtered;
}

void calculatePID() {
  // 计算误差
  error = target_angle - angleY;
  
  // 积分项 (防饱和)
  integral += error;
  if (integral > ki_limit) integral = ki_limit;
  if (integral < -ki_limit) integral = -ki_limit;
  
  // 微分项 (使用陀螺仪数据作为微分项)
  derivative = -gyroY; // 负号是因为方向
  
  // PID输出
  output = kp * error + ki * integral + kd * derivative;
  
  // 限制输出范围
  output = constrain(output, -motor_max, motor_max);
}

void automaticControl() {
  // 检查角度是否超出安全范围
  if (angleY > max_angle || angleY < min_angle) {
    // 角度超出安全范围，停止电机
    stopMotors();
    integral = 0; // 重置积分项
    return;
  }
  
  // 使用PID输出控制电机
  int motor_pwm = abs(output);
  
  // 处理死区
  if (motor_pwm < motor_deadzone) {
    motor_pwm = 0;
  }
  
  // 根据PID输出方向控制电机
  if (output > 0) {
    setMotorSpeed(motor_pwm, true); // 正转
  } else if (output < 0) {
    setMotorSpeed(motor_pwm, false); // 反转
  } else {
    stopMotors(); // 停止
  }
}

void stopMotors() {
  // 设置PWM为0
  analogWrite(motor1_pwm, 0);
  analogWrite(motor2_pwm, 0);
  
  // 设置方向引脚为确定状态
  digitalWrite(motor1_dir, LOW);
  digitalWrite(motor2_dir, LOW);
}

void setMotorSpeed(int pwm, bool forward) {
  // 如果PWM为0，停止电机
  if (pwm == 0) {
    stopMotors();
    return;
  }
  
  // 处理死区
  if (pwm < motor_deadzone) {
    stopMotors();
    return;
  }
  
  // 限制PWM值
  pwm = constrain(pwm, motor_deadzone, motor_max);
  
  // 控制电机
  if (forward) {
    digitalWrite(motor1_dir, HIGH);
    digitalWrite(motor2_dir, HIGH);
  } else {
    digitalWrite(motor1_dir, LOW);
    digitalWrite(motor2_dir, LOW);
  }
  
  analogWrite(motor1_pwm, pwm);
  analogWrite(motor2_pwm, pwm);
}

void displayStatus() {
  Serial.print("角度: ");
  Serial.print(angleY, 1);
  Serial.print("° | 目标: ");
  Serial.print(target_angle, 1);
  Serial.print("° | 误差: ");
  Serial.print(error, 2);
  Serial.print(" | PID输出: ");
  Serial.print(output, 2);
  
  Serial.print(" | 积分: ");
  Serial.print(integral, 2);
  Serial.print(" | 微分: ");
  Serial.print(derivative, 2);
  
  Serial.print(" | PID参数: Kp=");
  Serial.print(kp, 2);
  Serial.print(" Ki=");
  Serial.print(ki, 3);
  Serial.print(" Kd=");
  Serial.print(kd, 2);
  
  Serial.print(" | 模式: ");
  Serial.print(control_mode == AUTOMATIC ? "自动" : "手动");
  
  // 显示角度状态
  if (angleY > max_angle || angleY < min_angle) {
    Serial.print(" | 状态: 超出安全范围!");
  } else if (output > 0) {
    Serial.print(" | 状态: 正转");
  } else if (output < 0) {
    Serial.print(" | 状态: 反转");
  } else {
    Serial.print(" | 状态: 平衡");
  }
  
  Serial.println();
}

// 处理直接参数设置命令 (如 "p14.0")
bool processDirectCommand(String command) {
  if (command.length() < 2) return false;
  
  char param_type = command.charAt(0);
  String value_str = command.substring(1);
  float new_value = value_str.toFloat();
  
  switch(param_type) {
    case 'p': // kp
      kp = new_value;
      Serial.print("Kp设置为: ");
      break;
    case 'i': // ki  
      ki = new_value;
      Serial.print("Ki设置为: ");
      break;
    case 'd': // kd
      kd = new_value;
      Serial.print("Kd设置为: ");
      break;
    case 't': // 目标角度
      target_angle = new_value;
      Serial.print("目标角度设置为: ");
      break;
    default:
      return false; // 未知命令
  }
  
  Serial.println(new_value, 4);
  displayStatus();
  return true;
}

void serialEvent() {
  while (Serial.available()) {
    char cmd = Serial.read();
    
    if (param_adjust_mode) {
      // 参数调节模式 (两步输入)
      if (cmd == '\n') {
        float new_value = inputString.toFloat();
        
        switch(adjust_param_type) {
          case 'p': // kp
            kp = new_value;
            Serial.print("Kp设置为: ");
            break;
          case 'i': // ki  
            ki = new_value;
            Serial.print("Ki设置为: ");
            break;
          case 'd': // kd
            kd = new_value;
            Serial.print("Kd设置为: ");
            break;
          case 't': // 目标角度
            target_angle = new_value;
            Serial.print("目标角度设置为: ");
            break;
        }
        
        Serial.println(new_value, 4);
        displayStatus();
        
        // 退出参数调节模式
        param_adjust_mode = false;
        adjust_param_type = ' ';
        inputString = "";
        
        Serial.println("输入指令继续调节 (p:Kp, i:Ki, d:Kd, t:目标角度, ?:帮助)");
      } else {
        inputString += cmd;
      }
    } else {
      // 命令模式
      switch(cmd) {
        case 'm': // 切换到手动模式
          control_mode = MANUAL;
          stopMotors();
          integral = 0; // 重置积分项
          Serial.println("切换到手动控制模式");
          break;
          
        case 'a': // 切换到自动模式
          control_mode = AUTOMATIC;
          integral = 0; // 重置积分项
          Serial.println("切换到自动平衡模式");
          break;
          
        case 'f': // 正转 (仅在手动模式下有效)
          if (control_mode == MANUAL) {
            setMotorSpeed(100, true);
            Serial.println("正转");
          } else {
            Serial.println("请在手动模式下使用此命令");
          }
          break;
          
        case 'b': // 反转 (仅在手动模式下有效)
          if (control_mode == MANUAL) {
            setMotorSpeed(100, false);
            Serial.println("反转");
          } else {
            Serial.println("请在手动模式下使用此命令");
          }
          break;
          
        case 's': // 停止 (在两种模式下都有效)
          stopMotors();
          integral = 0; // 重置积分项
          Serial.println("停止");
          break;
          
        case 'p': // 调节Kp (两步输入)
          adjust_param_type = 'p';
          param_adjust_mode = true;
          inputString = "";
          Serial.println("请输入Kp值 (当前值: " + String(kp, 2) + "): ");
          break;
          
        case 'i': // 调节Ki (两步输入)
          adjust_param_type = 'i';
          param_adjust_mode = true;
          inputString = "";
          Serial.println("请输入Ki值 (当前值: " + String(ki, 3) + "): ");
          break;
          
        case 'd': // 调节Kd (两步输入)
          adjust_param_type = 'd';
          param_adjust_mode = true;
          inputString = "";
          Serial.println("请输入Kd值 (当前值: " + String(kd, 2) + "): ");
          break;
          
        case 't': // 调节目标角度 (两步输入)
          adjust_param_type = 't';
          param_adjust_mode = true;
          inputString = "";
          Serial.println("请输入目标角度值 (当前值: " + String(target_angle, 1) + "): ");
          break;
          
        case 'r': // 重置积分项
          integral = 0;
          Serial.println("PID积分项已重置");
          break;
          
        case '?': // 显示帮助
          showHelp();
          break;
          
        case '\n': // 忽略换行符
          break;
          
        default:
          // 尝试处理直接命令 (如 "p14.0")
          String fullCommand = String(cmd);
          while (Serial.available()) {
            char nextChar = Serial.read();
            if (nextChar == '\n') break;
            fullCommand += nextChar;
          }
          
          if (!processDirectCommand(fullCommand)) {
            Serial.println("未知命令，输入 ? 查看帮助");
          }
          break;
      }
    }
  }
}

void showHelp() {
  Serial.println("\n=== 自平衡小车控制命令 ===");
  Serial.println("模式控制:");
  Serial.println("  a - 切换到自动平衡模式");
  Serial.println("  m - 切换到手动控制模式");
  Serial.println("手动控制:");
  Serial.println("  f - 正转");
  Serial.println("  b - 反转");
  Serial.println("  s - 停止");
  Serial.println("PID参数调节 (两种方式):");
  Serial.println("  方式1 - 分两步输入:");
  Serial.println("    p - 调节比例系数 Kp");
  Serial.println("    i - 调节积分系数 Ki");
  Serial.println("    d - 调节微分系数 Kd");
  Serial.println("    t - 调整目标角度");
  Serial.println("  方式2 - 直接输入:");
  Serial.println("    p14.0 - 设置Kp为14.0");
  Serial.println("    i0.1 - 设置Ki为0.1");
  Serial.println("    d0.8 - 设置Kd为0.8");
  Serial.println("    t6.0 - 设置目标角度为6.0°");
  Serial.println("其他:");
  Serial.println("  r - 重置PID积分项");
  Serial.println("  ? - 显示此帮助信息");
  Serial.println("----------------------------");
  Serial.println("当前PID参数: Kp=" + String(kp, 2) + " Ki=" + String(ki, 3) + " Kd=" + String(kd, 2));
  Serial.println("目标角度: " + String(target_angle) + "°");
  Serial.println("----------------------------");
}