شرح استخدام مستشعر MPU6050 لقراءة الزوايا مع الأردوينو

نسخ الكود

#include <Wire.h>

// MPU6050 I2C address
const int MPU = 0x68;

// Variables for gyroscope and accelerometer data
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
float roll, pitch, yaw;
float elapsedTime, currentTime, previousTime;

// Calibration offsets
float AccX_offset = 0, AccY_offset = 0, AccZ_offset = 0;
float GyroX_offset = 0, GyroY_offset = 0, GyroZ_offset = 0;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  
  // Initialize MPU6050
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // Set to zero (wakes up the MPU6050)
  Wire.endTransmission(true);
  
  // Configure Accelerometer Sensitivity - Full Scale Range (default +/- 2g)
  Wire.beginTransmission(MPU);
  Wire.write(0x1C);
  Wire.write(0x00);
  Wire.endTransmission(true);
  
  // Configure Gyroscope Sensitivity - Full Scale Range (default +/- 250deg/s)
  Wire.beginTransmission(MPU);
  Wire.write(0x1B);
  Wire.write(0x00);
  Wire.endTransmission(true);
  
  delay(20);
  
  // Calibration
  Serial.println("Starting calibration...");
  Serial.println("Keep the sensor flat and still!");
  delay(2000);
  
  calibrateSensor();
  
  Serial.println("Calibration complete!");
  Serial.println("Starting angle measurements...");
  delay(1000);
}

void calibrateSensor() {
  int numReadings = 2000;
  float sumAccX = 0, sumAccY = 0, sumAccZ = 0;
  float sumGyroX = 0, sumGyroY = 0, sumGyroZ = 0;
  
  for (int i = 0; i < numReadings; i++) {
    // Read accelerometer data
    Wire.beginTransmission(MPU);
    Wire.write(0x3B);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    
    float ax = (Wire.read() << 8 | Wire.read()) / 16384.0;
    float ay = (Wire.read() << 8 | Wire.read()) / 16384.0;
    float az = (Wire.read() << 8 | Wire.read()) / 16384.0;
    
    // Read gyroscope data
    Wire.beginTransmission(MPU);
    Wire.write(0x43);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    
    float gx = (Wire.read() << 8 | Wire.read()) / 131.0;
    float gy = (Wire.read() << 8 | Wire.read()) / 131.0;
    float gz = (Wire.read() << 8 | Wire.read()) / 131.0;
    
    sumAccX += ax;
    sumAccY += ay;
    sumAccZ += az;
    sumGyroX += gx;
    sumGyroY += gy;
    sumGyroZ += gz;
    
    // Progress indicator
    if (i % 200 == 0) {
      Serial.print(".");
    }
    
    delay(3);
  }
  
  Serial.println();
  
  // Calculate average offsets
  AccX_offset = sumAccX / numReadings;
  AccY_offset = sumAccY / numReadings;
  AccZ_offset = (sumAccZ / numReadings) - 1.0; // Z should be 1g when flat
  GyroX_offset = sumGyroX / numReadings;
  GyroY_offset = sumGyroY / numReadings;
  GyroZ_offset = sumGyroZ / numReadings;
  
  // Display calibration values
  Serial.println("Calibration offsets:");
  Serial.print("AccX: "); Serial.print(AccX_offset);
  Serial.print(" | AccY: "); Serial.print(AccY_offset);
  Serial.print(" | AccZ: "); Serial.println(AccZ_offset);
  Serial.print("GyroX: "); Serial.print(GyroX_offset);
  Serial.print(" | GyroY: "); Serial.print(GyroY_offset);
  Serial.print(" | GyroZ: "); Serial.println(GyroZ_offset);
}

void loop() {
  // Read accelerometer data
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); // Start with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true);
  
  AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 - AccX_offset;
  AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 - AccY_offset;
  AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 - AccZ_offset;
  
  // Calculate pitch and roll from accelerometer
  accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI);
  accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI);
  
  // Read gyroscope data
  previousTime = currentTime;
  currentTime = millis();
  elapsedTime = (currentTime - previousTime) / 1000; // Convert to seconds
  
  Wire.beginTransmission(MPU);
  Wire.write(0x43); // Gyro data first register address 0x43
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true);
  
  GyroX = (Wire.read() << 8 | Wire.read()) / 131.0 - GyroX_offset;
  GyroY = (Wire.read() << 8 | Wire.read()) / 131.0 - GyroY_offset;
  GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0 - GyroZ_offset;
  
  // Currently the raw values are in degrees per seconds, we need to multiply by seconds (elapsedTime)
  gyroAngleX = gyroAngleX + GyroX * elapsedTime;
  gyroAngleY = gyroAngleY + GyroY * elapsedTime;
  yaw = yaw + GyroZ * elapsedTime;
  
  // Complementary filter - combine accelerometer and gyro angle values
  roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
  pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;
  
  // Update gyro angles with complementary filter
  gyroAngleX = roll;
  gyroAngleY = pitch;
  
  // Print the values in the format: pitch, roll, yaw
  Serial.print(pitch);
  Serial.print(", ");
  Serial.print(roll);
  Serial.print(", ");
  Serial.println(yaw);
  
  delay(20); // Adjust delay as needed for your application
}