#include <Wire.h>
const int MPU = 0x68;
float AccX, AccY, AccZ;
float GlobalAccZ;
float GlobalVelZ=0;
float GyroX, GyroY;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY;
float roll, pitch;
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY;
float elapsedTime, currentTime, previousTime, lastReset;
int c = 0;
int count = 0;
void setup() {
  Serial.begin(9600);
  pinMode(10, OUTPUT);
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission(true);
  calculate_IMU_error();
  delay(20);
}
void loop() {
  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)-1.056;
  accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) +1.0702331;
  accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) - 0.3581120;
  previousTime = currentTime;
  currentTime = millis();
  elapsedTime = (currentTime - previousTime) / 1000;
  Wire.beginTransmission(MPU);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true);
  GyroX = (Wire.read() << 8 | Wire.read()) / 131.0;
  GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
  GyroX = GyroX + 1.0034732;
  GyroY = GyroY - 0.0790840;
  gyroAngleX = gyroAngleX + GyroX * elapsedTime;
  gyroAngleY = gyroAngleY + GyroY * elapsedTime;
  roll = 0.96 * accAngleX + 0.04 * gyroAngleX;
  pitch = 0.96 * accAngleY + 0.04 * gyroAngleY;
  GlobalAccZ = -sin(pitch*(PI/180))*AccX+cos(pitch* (PI/180))*sin(roll*(PI/180))* AccY +cos(pitch*(PI/180))* cos(roll*(PI/180))*AccZ;
  if (GlobalAccZ>0.1 || GlobalAccZ<0.1){
    GlobalVelZ = GlobalVelZ + GlobalAccZ * elapsedTime;
  }
  if (currentTime-lastReset > 5000) {
    GlobalVelZ = 0;
    lastReset = currentTime;
  }
  Serial.print("Roll:");
  Serial.print(roll);
  Serial.print(",");
  Serial.print("Pitch:");
  Serial.print(pitch);
  Serial.print(",");
  Serial.print("Acceleration_of_X:");
  Serial.print(AccX);
  Serial.print(",");
  Serial.print("Acceleration_of_Y:");
  Serial.print(AccY);
  Serial.print(",");
  Serial.print("Acceleration_of_Z:");
  Serial.println(AccZ);
  Serial.print(",");
  Serial.print("Global_Acceleration_of_Z:");
  Serial.println(GlobalAccZ);
  Serial.print(",");
  Serial.print("Global_Velocity_of_Z:");
  Serial.println(GlobalVelZ);
  Serial.println();
  if (GlobalVelZ < -0.1){
    delay(20);
    count++;
    if(count>2){
      digitalWrite(10, 1);
      delay(50);
      count=0;
    }
  }
  else{
    count =0;
    digitalWrite(10,0);
  }
}
void calculate_IMU_error() {
  while (c < 200) {
    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 ;
    AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ), 2))) * 180 / PI));
    AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * 180 / PI));
    c++;
  }
  AccErrorX = AccErrorX / 200;
  AccErrorY = AccErrorY / 200;
  c = 0;
  while (c < 200) {
    Wire.beginTransmission(MPU);
    Wire.write(0x43);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    GyroX = Wire.read() << 8 | Wire.read();
    GyroY = Wire.read() << 8 | Wire.read();
    GyroErrorX = GyroErrorX + (GyroX / 131.0);
    GyroErrorY = GyroErrorY + (GyroY / 131.0);
    c++;
  }
  GyroErrorX = GyroErrorX / 200;
  GyroErrorY = GyroErrorY / 200;
  Serial.println();
  Serial.print("AccErrorX: ");
  Serial.println(AccErrorX,7);
  Serial.print("AccErrorY: ");
  Serial.println(AccErrorY,7);
  Serial.print("GyroErrorX: ");
  Serial.println(GyroErrorX,7);
  Serial.print("GyroErrorY: ");
  Serial.println(GyroErrorY,7);
}

asfadgsfhdgjfhkgjvhlkhjreytsrydjtukfyyutysyudyu

ystrudytfıuygıyyyurtyertaesytrdytfughj

yrsdtudfyıfguogyıuıyutyrtdruyfıughıjş

yretrudytıfuyoıguptudytfıuygoı6estdryutfıurystdruytfıu

rydtufyıtugoyrydtufyıguhoıouıyutyresdtrufyıguhıj

etrsyetdruytfıugytyrtstydufıgıhlıyutystdufuıgıh

wayesurydıtufutyrtsyetdrufuıgıhjşıyutrdytfıygı

yarstuydıufutytsduyfıguhııuysrdutfuıgı