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

#include <Wire.h>

const int MPU = 0x68; // I2C address
float AccX, AccY, AccZ, GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
float roll, pitch, yaw;
float AccX_offset = 0, GyroX_offset = 0; // سيتم ملؤها بعد المعايرة

void setup() {
  Serial.begin(9600);
  Wire.begin();
  
  Wire.beginTransmission(MPU);
  Wire.write(0x6B); // سجل الطاقة
  Wire.write(0);    // تفعيل المستشعر
  Wire.endTransmission(true);
  
  Serial.println("Calibrating... Keep it steady!");
  delay(1000);
}

void loop() {
  // 1. قراءة بيانات مقياس التسارع
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true);
  
  AccX = (Wire.read() << 8 | Wire.read()) / 16384.0;
  AccY = (Wire.read() << 8 | Wire.read()) / 16384.0;
  AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0;

  // 2. حساب الزوايا من التسارع
  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);

  // 3. دمج البيانات باستخدام الفلتر التكاملي
  roll = 0.96 * accAngleX + 0.04 * AccX; // مثال مبسط للفلترة
  
  Serial.print("Pitch: "); Serial.print(accAngleY);
  Serial.print(" | Roll: "); Serial.println(accAngleX);
  
  delay(20);
}