#include <Wire.h>
#include <MadgwickAHRS.h>  

const int MPU = 0x68;      
Madgwick filter;         
float ax, ay, az, gx, gy, gz;
float roll, pitch, yaw;

void setup() {
  Serial.begin(19200);
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);        
  Wire.write(0x00);        
  Wire.endTransmission(true);
  
  filter.begin(100); 
}

void loop() {
  readMPU6050();
  
  filter.updateIMU(gx, gy, gz, ax, ay, az);
  
  roll = filter.getRoll();
  pitch = filter.getPitch();
  yaw = filter.getYaw();
  
  Serial.print("Roll: ");
  Serial.print(roll);
  Serial.print(" Pitch: ");
  Serial.print(pitch);
  Serial.print(" Yaw: ");
  Serial.println(yaw);
  
  delay(10);
}

void readMPU6050() {
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);      
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 14, true); 
  
  ax = (Wire.read() << 8 | Wire.read()) / 16384.0;
  ay = (Wire.read() << 8 | Wire.read()) / 16384.0;
  az = (Wire.read() << 8 | Wire.read()) / 16384.0;
  
  gx = (Wire.read() << 8 | Wire.read()) / 131.0;
  gy = (Wire.read() << 8 | Wire.read()) / 131.0;
  gz = (Wire.read() << 8 | Wire.read()) / 131.0;
}