/*
     Modified from ARDUINO HOCAM GIMBAL YAPIMI

*/
#include   <Servo.h>
#include  <I2Cdev.h>
#include   <MPU6050.h>
#include   <Wire.h>// MPU6050 using I2C protocol ,thats why we need to use these libraries.



MPU6050 ACC_GYR; // Creating MPU6050 object and giving a name.
Servo servo1;
Servo servo2;// Servo objects.
//Create 16 bit variables. the reason is data which is coming from sensor has 16bit information. 
int16_t accx, accy, accz;
int16_t gyrx, gyry, gyrz;


int deger, deger2, eskideger, eskideger2, x, y;  // create variable. Here x and y are used for my personal stabilization. You don't have to use them. 

void setup() {
  Serial.begin(9600);
  Serial.println("ARDUINO HOCAM GIMBALL YAPIMI");
  Serial.println("www.youtube.com/ArduinoHocam ");
  ACC_GYR.initialize(); // Sensor initialize
  Serial.println(ACC_GYR.testConnection() ? "BAĞLANDI" : "BAĞLANAMADI!!!"); //connection test

  servo1.attach(6);
  servo2.attach(9);//servo pins
  servo1.write(90);
  servo2.write(90);// 90 DERECE KONUMUNA GETIR.
  delay(50);
}

void loop() {
  ACC_GYR.getMotion6(&accx, &accy, &accz, &gyrx, &gyry , &gyrz); // dalatarı çekiyoruz sensörden

  deger = map(accx, -17000, 17000, 0, 180); // change x axis boundries
  deger2 = map(accy, -17000, 17000, 0, 180); //change yaxis boundries


  if (deger != eskideger) { //if old value is not equal to new value ; which means we have movement on the sensor...
    y = deger - 25 ;             //Here, this is personal. In order to stabilization. I just made some math...
    servo1.write(y);  //
    eskideger = deger;
    Serial.print("x ekseni:");
    Serial.println(deger);
  }
  if (deger2 != eskideger2) {
    x = 180 - deger2;
    servo2.write(x);
    eskideger2 = deger2;
    Serial.print("y ekseni:");
    Serial.println(deger2);
  }

  delay(50);
}