#include <Wire.h>
#include <MPU6050.h>
#include <Stepper.h>

const int stepsPerRevolution = 200; // Devrim başına adım sayısı
const float degreesPerStep = 360.0 / stepsPerRevolution; // Her adımın açısal değeri

// Motor pin numaraları
const int dirPin = 2;
const int stepPin = 3;

// MPU6050 sensör nesnesini başlat
MPU6050 mpu;

// Stepper motor nesnesini başlat
Stepper myStepper(stepsPerRevolution, dirPin, stepPin);

// Önceki Z rotasyon değeri
float prevRotationZ = 0.0;

// Referans açısı
float referenceAngle = 0.0;

void setup() {
  Wire.begin();
  Serial.begin(9600);

  // MPU6050 sensörünü başlat
  mpu.initialize();

  // İleri veya geri dönüşü belirlemek için eşik değerlerini ayarla
  mpu.setZAccelOffset(0);
}

void loop() {
  // Z-ekseni rotasyon değerini oku
  float rotationZ = calculateRotationZ();

  // Önceki Z rotasyon değeri ile mevcut değer arasındaki farkı hesapla
  float deltaRotationZ = rotationZ - prevRotationZ;

  // Her adım için bir açısal değer hesapla
  float degreesToMove = deltaRotationZ * degreesPerStep;

  // Motoru döndür
  myStepper.step(static_cast<int>(degreesToMove));

  // Önceki Z rotasyon değerini güncelle
  prevRotationZ = rotationZ;

  // Referans açısını ekleyerek +180 ile -180 arasında sınırla
  float boundedRotationZ =rotationZ;

  // Elde edilen Z rotasyon değerini Serial Monitor'a yazdır
  Serial.println("Z ekseninde dönme açısı: " + String(boundedRotationZ/1.292) + " derece");

  // Döngünün makul bir hızda çalışmasına izin vermek için gecikme
  delay(500);
}

float calculateRotationZ() {
  // Z eksenindeki rotasyonu yaklaşık olarak hesapla
  float gyroZ = mpu.getRotationZ();
  
  // Açı değerini atan2 fonksiyonu ile hesapla ve dereceye çevir
  float rotationZ = atan2(gyroZ, 16384.0) * 330 / PI;

  return rotationZ;
}