esp32-s3-supermini使用arduio IDE进行mpu6050的数据读取

本代码是读取mpu6050的数据，然后根据数据输出1,2,3,4,5,6，让mpu6050当色子。需要安装库

在工具-库管理输入mpu6050

usb-cdc选enable

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

// MPU6050对象
MPU6050 mpu;

// 定义I2C引脚
#define I2C_SDA 8
#define I2C_SCL 9

// 姿态角度阈值
const int MIN_ANGLE = 35;  // 最小角度阈值
const int MAX_ANGLE = 90;  // 最大角度阈值

// 共享变量（使用volatile确保多任务间的可见性）
volatile float pitch = 0, roll = 0, accel_z = 0;
volatile bool newDataAvailable = false;
portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;

// 状态变量
int lastState = 0;
unsigned long lastStateTime = 0;
const int STABLE_TIME = 300;  // 稳定时间

// 稳定性检测参数
const float ANGLE_CHANGE_THRESHOLD = 3.0;  // 角度变化阈值(度)
const int STABILITY_COUNT_THRESHOLD = 3;   // 稳定计数阈值
int stabilityCount = 0;
float lastPitch = 0, lastRoll = 0, lastAccelZ = 0;

// 滤波参数
const float ALPHA = 0.6;  // 滤波系数

// 任务句柄
TaskHandle_t mpuTaskHandle = NULL;
TaskHandle_t diceTaskHandle = NULL;

void setup() {
  Serial.begin(115200);
  delay(1000);
  
  // 初始化I2C
  Wire.begin(I2C_SDA, I2C_SCL);
  Wire.setClock(400000);
  
  // 尝试连接MPU6050
  bool connected = false;
  uint8_t addresses[] = {0x68, 0x69};
  
  for (int i = 0; i < 2; i++) {
    mpu = MPU6050(addresses[i]);
    mpu.initialize();
    
    if (mpu.testConnection()) {
      connected = true;
      break;
    }
    delay(100);
  }
  
  if (!connected) {
    Serial.println("MPU6050连接失败");
    while(1) delay(1000);
  }
  
  // 设置MPU6050参数
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
  
  // 创建任务
  xTaskCreatePinnedToCore(
    mpu6050Task,    // 任务函数
    "MPU6050",      // 任务名称
    10000,          // 堆栈大小
    NULL,           // 参数
    3,              // 优先级
    &mpuTaskHandle, // 任务句柄
    1               // 核心1
  );
  
  xTaskCreatePinnedToCore(
    diceTask,       // 任务函数
    "Dice",         // 任务名称
    10000,          // 堆栈大小
    NULL,           // 参数
    2,              // 优先级略低于MPU任务
    &diceTaskHandle,// 任务句柄
    1               // 核心1
  );
  
  // 删除默认的loop任务
  vTaskDelete(NULL);
}

void loop() {
  // 任务已删除，这里不会执行
}

// MPU6050数据读取任务（50ms间隔）
void mpu6050Task(void *pvParameters) {
  TickType_t xLastWakeTime = xTaskGetTickCount();
  const TickType_t xFrequency = 50 / portTICK_PERIOD_MS;  // 50ms
  
  // 局部滤波变量
  float localPitch = 0, localRoll = 0;
  
  for(;;) {
    // 读取MPU6050数据
    int16_t ax, ay, az, gx, gy, gz;
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    
    // 转换为重力加速度单位
    float accel_x = ax / 16384.0;
    float accel_y = ay / 16384.0;
    float current_accel_z = az / 16384.0;
    
    // 计算角度
    float accelPitch = atan2(-accel_x, sqrt(accel_y*accel_y + current_accel_z*current_accel_z)) * 180/PI;
    float accelRoll = atan2(accel_y, current_accel_z) * 180/PI;
    
    // 互补滤波
    localPitch = ALPHA * localPitch + (1-ALPHA) * accelPitch;
    localRoll = ALPHA * localRoll + (1-ALPHA) * accelRoll;
    
    // 保护共享变量
    portENTER_CRITICAL(&mux);
    pitch = localPitch;
    roll = localRoll;
    accel_z = current_accel_z;
    newDataAvailable = true;
    portEXIT_CRITICAL(&mux);
    
    // 精确延时50ms
    vTaskDelayUntil(&xLastWakeTime, xFrequency);
  }
}

// 骰子状态判断任务
void diceTask(void *pvParameters) {
  for(;;) {
    // 检查是否有新数据
    bool newData = false;
    float currentPitch, currentRoll, currentAccelZ;
    
    portENTER_CRITICAL(&mux);
    if (newDataAvailable) {
      currentPitch = pitch;
      currentRoll = roll;
      currentAccelZ = accel_z;
      newDataAvailable = false;
      newData = true;
    }
    portEXIT_CRITICAL(&mux);
    
    if (newData) {
      // 判断骰子点数
      int currentState = getDiceState(currentPitch, currentRoll, currentAccelZ);
      
      // 检查角度变化是否稳定
      bool isStable = checkStability(currentPitch, currentRoll, currentAccelZ);
      
      // 状态稳定检测
      if (currentState == lastState) {
        if (millis() - lastStateTime > STABLE_TIME && currentState != 0 && isStable) {
          // 只输出骰子点数
          Serial.println(currentState);
          lastStateTime = millis() + 500; // 0.5秒内不重复输出
          
          // 重置稳定性计数器
          stabilityCount = 0;
        }
      } else {
        lastState = currentState;
        lastStateTime = millis();
        
        // 状态变化时重置稳定性计数器
        stabilityCount = 0;
        lastPitch = currentPitch;
        lastRoll = currentRoll;
        lastAccelZ = currentAccelZ;
      }
    }
    
    // 短暂延时，让出CPU
    vTaskDelay(2 / portTICK_PERIOD_MS);
  }
}

// 检查角度稳定性
bool checkStability(float currentPitch, float currentRoll, float currentAccelZ) {
  // 计算角度变化量
  float pitchChange = abs(currentPitch - lastPitch);
  float rollChange = abs(currentRoll - lastRoll);
  float accelZChange = abs(currentAccelZ - lastAccelZ);
  
  // 检查是否稳定（角度变化小于阈值）
  if (pitchChange < ANGLE_CHANGE_THRESHOLD && 
      rollChange < ANGLE_CHANGE_THRESHOLD && 
      accelZChange < 0.1) {
    stabilityCount++;
  } else {
    stabilityCount = 0; // 不稳定时重置计数器
  }
  
  // 更新上一次的角度值
  lastPitch = currentPitch;
  lastRoll = currentRoll;
  lastAccelZ = currentAccelZ;
  
  // 如果连续多次稳定，则认为姿态已稳定
  return stabilityCount >= STABILITY_COUNT_THRESHOLD;
}

// 根据姿态判断骰子点数
int getDiceState(float pitch, float roll, float accel_z) {
  // 首先检查是否为倒置状态（6点）- 优先级最高
  if (accel_z < -0.6) {
    return 6;  // 倒置状态，显示6点
  }
  
  // 然后检查是否为正面朝上（1点）
  if (accel_z > 0.6 && abs(pitch) < 45 && abs(roll) < 45) {
    return 1;  // 正面朝上，显示1点
  }
  
  // 检查倾斜状态
  // 2点：向前倾斜
  if (pitch > MIN_ANGLE && pitch <= MAX_ANGLE && abs(roll) < 50) {
    return 2;
  }
  
  // 5点：向后倾斜
  if (pitch < -MIN_ANGLE && pitch >= -MAX_ANGLE && abs(roll) < 50) {
    return 5;
  }
  
  // 3点：向左倾斜
  if (roll > MIN_ANGLE && roll <= MAX_ANGLE && abs(pitch) < 50) {
    return 3;
  }
  
  // 4点：向右倾斜
  if (roll < -MIN_ANGLE && roll >= -MAX_ANGLE && abs(pitch) < 50) {
    return 4;
  }
  
  return 0; // 未识别状态
}