// Forum: https://forum.arduino.cc/t/mpu6050-pitch-angle/1164593/
// Sketch by: mhalneami
// This Wokwi project: 

#include <Wire.h>

float RateRoll, RatePitch, RateYaw;
float Roll = 0.0, Pitch = 0.0, Yaw = 0.0;  // Current angles
float RateCalibrationRoll, RateCalibrationPitch, RateCalibrationYaw;
int RateCalibrationNumber;
unsigned long prevUpdateTime = 0;


void gyro_signals(void) {
  Wire.beginTransmission(0x68);
  Wire.write(0x1B);  // Register 0x1B is the Gyro Config register
  Wire.write(0x00);  // Set sensitivity to ±250°/s (default is ±131°/s)
  Wire.endTransmission();
  
  Wire.beginTransmission(0x68);
  Wire.write(0x43);
  Wire.endTransmission();
  Wire.requestFrom(0x68, 6);
  int16_t GyroX = Wire.read() << 8 | Wire.read();
  int16_t GyroY = Wire.read() << 8 | Wire.read();
  int16_t GyroZ = Wire.read() << 8 | Wire.read();
  RateRoll = (float)GyroX / 131.0;  // Adjust sensitivity from 65.5 to 131.0
  RatePitch = (float)GyroY / 131.0;
  RateYaw = (float)GyroZ / 131.0;
}

void calculate_angles(float dt) {
  Roll += RateRoll * dt;    
  Pitch += RatePitch * dt;  
  Yaw += RateYaw * dt;      
}

void setup() {
  Serial.begin(57600);
  pinMode(13, OUTPUT);
  digitalWrite(13, HIGH);
  Wire.setClock(400000);
  Wire.begin();
  delay(250);
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission();
  for (RateCalibrationNumber = 0;
       RateCalibrationNumber < 2000;
       RateCalibrationNumber++) {
    gyro_signals();
    RateCalibrationRoll += RateRoll;
    RateCalibrationPitch += RatePitch;
    RateCalibrationYaw += RateYaw;
    delay(1);
  }
  RateCalibrationRoll /= 2000;
  RateCalibrationPitch /= 2000;
  RateCalibrationYaw /= 2000;

}

void loop() {
  unsigned long currentTime = millis();
  float dt = (currentTime - prevUpdateTime) / 1000.0;  // Time in seconds since last update
  prevUpdateTime = currentTime;

  gyro_signals();
  RateRoll -= RateCalibrationRoll;
  RatePitch -= RateCalibrationPitch;
  RateYaw -= RateCalibrationYaw;

  calculate_angles(dt);


  Serial.print("Pitch Angle ");
  Serial.println(Pitch);
 
  delay(50);
}