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

// 
int angle1;
int angle2;
int angle3;
int angle4;
int angle5;



Adafruit_MPU6050 mpu;

Servo servo1; // 
Servo servo2; // 
Servo servo3; // 
Servo servo4; // 
Servo servo5; // 


void setup(void) {
  Serial.begin(115200);

  pinMode(8, INPUT);
  pinMode(4, INPUT);

  while (!mpu.begin()) { // 
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }
  Serial.println("MPU6050 ready!");

  //
  servo1.attach(11);
  servo2.attach(10);
  servo3.attach(9);
  servo4.attach(6);
  servo5.attach(5);

  //
  angle1 = 90;
  angle2 = 0;
  angle3 = 180;
  angle4 = 90;
  angle5 = 0;
}

sensors_event_t event;

void loop() {
  
  // 
  mpu.getAccelerometerSensor()->getEvent(&event);

  //
  if (digitalRead(8) == HIGH);
    {
    angle1 = min(angle1, 180);
    angle1 = max(angle1, 0);
    angle1 =(angle1 + (event.acceleration.x));
    servo1.write(angle1);
    delay(10);

    angle2 = min(angle2, 180);
    angle2 = max(angle2, 0);
    angle2 =(angle2 + (event.acceleration.z) - 1);
    servo2.write(angle2);
    delay(10);

    angle3 = min(angle3, 180);
    angle3 = max(angle3, 0);
    angle3 =(angle3 + (event.acceleration.y));
    servo3.write(angle3);
    delay(10);
    
    //
    mpu.getGyroSensor()->getEvent(&event);

    angle4 = min(angle4, 180);
    angle4 = max(angle4, 0);
    angle4 =(angle4 + ((event.gyro.z)*2));
    servo4.write(angle4);
    delay(10);
    }

  if (digitalRead(4) == HIGH) {
    servo5.write(180);

    } else {
     servo5.write(0);

    }
}