#include <Adafruit_MPU6050.h>

#include <Adafruit_Sensor.h>

#include <Wire.h>

Adafruit_MPU6050 mpu;

float yaw = 0.0;

unsigned long lastTime = 0;

float gyroBiasZ = 0.0;

void setup() {

  Serial.begin(115200);

  if (!mpu.begin()) {

    Serial.println("Failed to find MPU6050 chip");

    while (1) {

      delay(10);

    }

  }

  Serial.println("MPU6050 Found!");

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);

  mpu.setGyroRange(MPU6050_RANGE_500_DEG);

  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  calibrateGyro();

  lastTime = millis();

  delay(100);

}

void calibrateGyro() {

  const int numReadings = 100;

  gyroBiasZ = 0.0;

  for (int i = 0; i < numReadings; i++) {

    sensors_event_t a, g, temp;

    mpu.getEvent(&a, &g, &temp);

    gyroBiasZ += g.gyro.z;

    delay(10);

  }

  gyroBiasZ /= numReadings;

}

void loop() {

  sensors_event_t a, g, temp;

  mpu.getEvent(&a, &g, &temp);

  float ax = a.acceleration.x;

  float ay = a.acceleration.y;

  float az = a.acceleration.z;

  float pitch = atan2(ay, sqrt(ax * ax + az * az)) * 180.0 / PI;

  float roll = atan2(-ax, az) * 180.0 / PI;

  unsigned long currentTime = millis();

  float deltaTime = (currentTime - lastTime) / 1000.0;

  float gyroZ = g.gyro.z - gyroBiasZ; // Subtract bias

  yaw += gyroZ * deltaTime;

  lastTime = currentTime;

  if (pitch < -17) {

     if (pitch < -17 && roll < -14) {

       Serial.println(7);

    
    }

    else if (pitch < -17 && roll > 14) {


       Serial.println(5);

      
    }

    else {

  Serial.println(3);

  
    }


  }
 else if (pitch > 17) {
    

    if (pitch > 17 && roll < -14) {

    

     Serial.println(8);

  
    }
 else if (pitch > 17 && roll > 14) {

    

   Serial.println(6);

  
    }
 else {

       Serial.println(4);

    }

  }

 else if (roll > 17) {

    Serial.println(1);
  }

else if (roll < -17) {
  
    Serial.println(2);
  }

  else {
  
    Serial.println(15);
  }

  delay(250);

}