#include <ESP32Servo.h>
#include <Wire.h>
#include <MPU6050.h>
MPU6050 mpu;
Servo baling1;
Servo baling2;
Servo baling3;
Servo baling4;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");
  Wire.begin(22, 21);
  baling1.attach(25);
  baling2.attach(33);
  baling3.attach(26);
  baling4.attach(17);
  mpu.initialize();
  if (mpu.testConnection()) {
    Serial.println("terhubung");
  }
  else {
    Serial.println("tidak terhubung");
  }
}

void loop() {
  balingputar();
  int16_t ax, ay, az;
  int16_t gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  if (ax > 0) {
    balingBawahLambat();
  }
  if (ax < 0) {
    balingAtasLambat();
  }
}

void balingputar() {
  baling1.write(180);
  baling2.write(180);
  baling3.write(180);
  baling4.write(180);
  delay(250);
  baling1.write(0);
  baling2.write(0);
  baling3.write(0);
  baling4.write(0);
  delay(250);
}

void balingKiriLambat() {
  for (int i=0; i<=180; i++) {
    baling2.write(i);
    baling3.write(i);
    delay(8);
  }
  for (int j=180; j>=0; j--) {
    baling2.write(j);
    baling3.write(j);
    delay(8);
  }
}

void balingKananLambat() {
  for (int i=0; i<=180; i++) {
    baling1.write(i);
    baling4.write(i);
    delay(8);
  }
  for (int j=180; j>=0; j--) {
    baling1.write(j);
    baling4.write(j);
    delay(8);
  }
}

void balingAtasLambat() {
  for (int i=0; i<=180; i++) {
    baling1.write(i);
    baling2.write(i);
    delay(8);
  }
  for (int j=180; j>=0; j--) {
    baling1.write(j);
    baling2.write(j);
    delay(8);
  }
}

void balingBawahLambat() {
  for (int i=0; i<=180; i++) {
    baling3.write(i);
    baling4.write(i);
    delay(8);
  }
  for (int j=180; j>=0; j--) {
    baling3.write(j);
    baling4.write(j);
    delay(8);
  }
}