肥仔教程网

验证出平衡小车直立时，angleY为5左右,轮子死区PWM35左右。编写程序，大于5，轮子正转，小于5轮子反转，轮子起始PWM为40。

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

MPU6050 mpu;

// 控制参数
float target_angle = 5.0; // 目标平衡角度
int base_pwm = 40;        // 起始PWM值
const int motor_deadzone = 35; // 轮子死区

// MPU变量
float angleY = 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;

// MPU6050连接状态
bool mpu_connected = false;

void initializeMPU6050();
void readMPU6050();
void simpleControl();
void setMotorSpeed(int pwm, bool forward);
void displayStatus();
void serialEvent();

void setup() {
  Serial.begin(115200);
  delay(1000);
  
  Serial.println("自平衡小车简单控制程序启动中...");
  
  // 初始化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);
  
  // 停止电机
  digitalWrite(motor1_pwm, LOW);
  digitalWrite(motor2_pwm, LOW);
  
  Serial.println("\n=== 自平衡小车简单控制程序 ===");
  Serial.println("控制策略: angleY > 5 → 正转, angleY < 5 → 反转");
  Serial.println("起始PWM: " + String(base_pwm));
  Serial.println("轮子死区: " + String(motor_deadzone));
  Serial.println("----------------------------");
}

void initializeMPU6050() {
  Serial.println("正在初始化MPU6050...");
  
  mpu.initialize();
  delay(100);
  
  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);
    
    Serial.println("MPU6050配置完成");
  } else {
    Serial.println("MPU6050连接失败! 请检查连接:");
    Serial.println("SDA:41, SCL:42, 电源:3.3V");
  }
}

void loop() {
  // 读取MPU6050数据
  if (mpu_connected) {
    readMPU6050();
    
    // 简单控制逻辑
    simpleControl();
    
    // 显示状态
    static unsigned long last_display = 0;
    if (millis() - last_display > 500) {
      displayStatus();
      last_display = millis();
    }
  } else {
    // 如果MPU未连接，尝试重新初始化
    static unsigned long last_reconnect = 0;
    if (millis() - last_reconnect > 2000) {
      initializeMPU6050();
      last_reconnect = 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;
}

void simpleControl() {
  int motor_pwm = 0;
  
  // 简单控制逻辑
  if (angleY > target_angle) {
    // 角度大于5度，轮子正转
    motor_pwm = base_pwm;
    setMotorSpeed(motor_pwm, true); // 正转
  } else if (angleY < target_angle) {
    // 角度小于5度，轮子反转
    motor_pwm = base_pwm;
    setMotorSpeed(motor_pwm, false); // 反转
  } else {
    // 角度等于5度，停止
    setMotorSpeed(0, true);
  }
}

void setMotorSpeed(int pwm, bool forward) {
  // 处理死区
  if (abs(pwm) < motor_deadzone) {
    pwm = 0;
  }
  
  // 限制PWM值
  pwm = constrain(pwm, 0, 255);
  
  // 控制电机
  if (pwm > 0) {
    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);
  } else {
    // 停止
    analogWrite(motor1_pwm, 0);
    analogWrite(motor2_pwm, 0);
  }
}

void displayStatus() {
  Serial.print("角度: ");
  Serial.print(angleY, 2);
  Serial.print("° | 目标: ");
  Serial.print(target_angle, 1);
  Serial.print("° | 动作: ");
  
  if (angleY > target_angle) {
    Serial.print("正转");
  } else if (angleY < target_angle) {
    Serial.print("反转");
  } else {
    Serial.print("停止");
  }
  
  Serial.print(" | PWM: ");
  Serial.println(base_pwm);
}

void serialEvent(){
  while (Serial.available()) {
    char inChar = (char)Serial.read();
    
    switch(inChar) {
      case '+': // 增加PWM
        base_pwm += 5;
        if (base_pwm > 255) base_pwm = 255;
        Serial.println("PWM增加到: " + String(base_pwm));
        break;
        
      case '-': // 减少PWM
        base_pwm -= 5;
        if (base_pwm < motor_deadzone) base_pwm = motor_deadzone;
        Serial.println("PWM减少到: " + String(base_pwm));
        break;
        
      case 's': // 显示状态
        displayStatus();
        break;
        
      case ' ': // 停止电机
        setMotorSpeed(0, true);
        Serial.println("电机已停止");
        break;
        
      case 't': // 调整目标角度
        target_angle += 1.0;
        if (target_angle > 15.0) target_angle = 0.0;
        Serial.println("目标角度设置为: " + String(target_angle, 1) + "°");
        break;
        
      case '\n':
        break;
        
      default:
        Serial.println("可用命令:");
        Serial.println("+ : 增加PWM");
        Serial.println("- : 减少PWM");
        Serial.println("s : 显示状态");
        Serial.println("空格 : 停止电机");
        Serial.println("t : 切换目标角度");
        break;
    }
  }
}