#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <AccelStepper.h>

#define MPU6050_ADDR 0x68

Adafruit_MPU6050 mpu;

const int stepPin1 = 6; // Pin STEP pada driver A4988
const int dirPin1 = 5;  // Pin DIR pada driver A4988
const int stepPin2 = 8; // Pin STEP pada driver A4988
const int dirPin2 = 7;  // Pin DIR pada driver A4988


const int stepsPerRevolution = 200; // Jumlah langkah per revolusi pada stepper motor
const float anglePerStep = 360.0 / stepsPerRevolution; // Sudut per langkah motor

// Konfigurasi PID
double kp = 30.0; // Proporsional
double ki = 0.0;  // Integral (tidak digunakan dalam contoh ini)
double kd = 40.0; // Derivative

double setpoint = 0.0; // Nilai setpoint (target) untuk sudut kemiringan (pitch)
double last_error = 0.0;

// Inisialisasi objek stepper motor
AccelStepper stepper1(AccelStepper::DRIVER, stepPin1, dirPin1);
AccelStepper stepper2(AccelStepper::DRIVER, stepPin2, dirPin2);


void setup() {
  Wire.begin();
  mpu.begin(MPU6050_ADDR);

  // Inisialisasi driver A4988
  pinMode(stepPin1, OUTPUT);
  pinMode(dirPin1, OUTPUT);
  pinMode(stepPin2, OUTPUT);
  pinMode(dirPin2, OUTPUT);
  
  
  // Set kecepatan maksimum dan percepatan stepper motor
  stepper1.setMaxSpeed(1000);
  stepper1.setAcceleration(100);
  stepper2.setMaxSpeed(1000);
  stepper2.setAcceleration(100);
  
}

void loop() {
  // Baca data dari MPU6050
  sensors_event_t a, g, event;
  mpu.getEvent(&a, &g, &event);

  // Hitung sudut kemiringan (pitch) dari data akselerometer
  double pitch = atan2(-event.acceleration.x, sqrt(event.acceleration.y * event.acceleration.y + event.acceleration.z * event.acceleration.z)) * RAD_TO_DEG;

  // Kendalikan motor berdasarkan nilai sudut kemiringan menggunakan kontrol PID
  double error = setpoint - pitch;
  double output = kp * error + kd * (error - last_error);

  // Batasi output agar tidak melebihi batas tertentu
  if (output > 500) output = 500;
  if (output < -500) output = -500;

  // Hitung jumlah langkah yang harus dijalankan oleh stepper motor
  int steps = output / anglePerStep;

  // Tentukan arah putaran stepper motor berdasarkan nilai output
  if (output > 0) {
    digitalWrite(dirPin1, HIGH); // Putaran searah jarum jam
  } else {
    digitalWrite(dirPin1, LOW); // Putaran berlawanan arah jarum jam
  }

  // Jalankan stepper motor dengan jumlah langkah yang telah dihitung
  stepper1.move(steps);
  stepper1.run();

  if (output > 0) {
    digitalWrite(dirPin2, HIGH); // Putaran searah jarum jam
  } else {
    digitalWrite(dirPin2, LOW); // Putaran berlawanan arah jarum jam
  }

  
  // Jalankan stepper motor dengan jumlah langkah yang telah dihitung
  stepper2.move(steps);
  stepper2.run();

  // Simpan error saat ini untuk perhitungan selanjutnya
  last_error = error;
}