#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Servo.h>

Adafruit_MPU6050 mpu;
Servo servo1, servo2, servo3, servo4;

const int MPU_ADDRESS = 0x68; 
const int servoPin1 = 9;
const int servoPin2 = 10;
const int servoPin3 = 11;
const int servoPin4 = 6;

const float angleThreshold = 5.0;
void setup() {
  Wire.begin();
  mpu.begin();
  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);

  servo1.attach(servoPin1);
  servo2.attach(servoPin2);
  servo3.attach(servoPin3);
  servo4.attach(servoPin4);
}

void loop() {
  // Read the accelerometer data
  sensors_event_t event;
  mpu.getAccelerometerSensor()->getEvent(&event);

  // Calculate the roll angle
  float roll = atan2(event.acceleration.y, event.acceleration.z) * 180 / PI;

  
  if (roll > angleThreshold) {
    
    servo1.write(90 + roll);
    servo2.write(90 - roll);
    servo3.write(90 - roll);
    servo4.write(90 + roll);
  } else if (roll < -angleThreshold) {
    
    servo1.write(90 - roll);
    servo2.write(90 + roll);
    servo3.write(90 + roll);
    servo4.write(90 - roll);
  } else {
    
    servo1.write(90);
    servo2.write(90);
    servo3.write(90);
    servo4.write(90);
  }

  delay(10); 
}