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

نسخ الكود

// استدعاء مكتبة I2C
#include <Wire.h>

// عنوان MPU 6050 في واجهة I2C
const int MPU = 0x68;

// متغيرات بيانات الجيروسكوب ومقياس التسارع
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
float roll, pitch, yaw;
float elapsedTime, currentTime, previousTime;

// إزاحات المعايرة
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();
  
  // تهيئة MPU6050
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);  // سجل الطاقة PWR_MGMT_1
  Wire.write(0);     // ضبطه على الصفر (لتشغيل MPU6050)
  Wire.endTransmission(true);
  
  // إعداد حساسية مقياس التسارع - النطاق الكامل الافتراضي (+/- 2g)
  Wire.beginTransmission(MPU);
  Wire.write(0x1C);
  Wire.write(0x00);
  Wire.endTransmission(true);
  
  // إعداد حساسية الجيروسكوب - النطاق الكامل الافتراضي (+/- 250 درجة/ثانية)
  Wire.beginTransmission(MPU);
  Wire.write(0x1B);
  Wire.write(0x00);
  Wire.endTransmission(true);
  
  delay(20);
  
  // المعايرة
  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++) {
    // قراءة بيانات مقياس التسارع
    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;
    
    // قراءة بيانات الجيروسكوب
    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;
    
    // مؤشر التقدم
    if (i % 200 == 0) {
      Serial.print(".");
    }
    
    delay(3);
  }
  
  Serial.println();
  
  // حساب متوسط الإزاحات
  AccX_offset = sumAccX / numReadings;
  AccY_offset = sumAccY / numReadings;
  AccZ_offset = (sumAccZ / numReadings) - 1.0; // يجب أن تكون Z = 1g عندما يكون المستشعر مستويًا
  GyroX_offset = sumGyroX / numReadings;
  GyroY_offset = sumGyroY / numReadings;
  GyroZ_offset = sumGyroZ / numReadings;
  
  // عرض قيم الإزاحات
  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() {
  // قراءة بيانات مقياس التسارع
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); // البدء من السجل 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;
  
  // حساب الزوايا من مقياس التسارع
  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);
  
  // قراءة بيانات الجيروسكوب
  previousTime = currentTime;
  currentTime = millis();
  elapsedTime = (currentTime - previousTime) / 1000; // التحويل إلى ثوانٍ
  
  Wire.beginTransmission(MPU);
  Wire.write(0x43); // أول سجل بيانات الجيروسكوب 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;
  
  // التكامل للسرعة الزاوية
  gyroAngleX = gyroAngleX + GyroX * elapsedTime;
  gyroAngleY = gyroAngleY + GyroY * elapsedTime;
  yaw = yaw + GyroZ * elapsedTime;
  
  // الفلتر التكاملي - دمج زوايا التسارع والجيروسكوب
  roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
  pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;
  
  // تحديث زوايا الجيروسكوب باستخدام الفلتر التكاملي
  gyroAngleX = roll;
  gyroAngleY = pitch;
  
  // طباعة القيم بالترتيب: pitch, roll, yaw
  Serial.print(pitch);
  Serial.print(", ");
  Serial.print(roll);
  Serial.print(", ");
  Serial.println(yaw);
  
  delay(20); // يمكن تعديل التأخير حسب الحاجة
}