//Rangkaian stabilizer/ gimabal 3 axis

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

Servo myservo1;
Servo myservo2;
Servo myservo3;
Adafruit_MPU6050 mysensor;

void setup() {
  Serial.begin(115200);
  myservo1.attach(9);
  myservo2.attach(10);
  myservo3.attach(11);
  Wire.begin();
  mysensor.begin();
  myservo1.write(0);
  myservo2.write(0);
  myservo3.write(0);

  mysensor.setAccelerometerRange(MPU6050_RANGE_8_G);
  mysensor.setGyroRange(MPU6050_RANGE_250_DEG);
  mysensor.setFilterBandwidth(MPU6050_BAND_21_HZ);
  delay(100);
  
}

void loop() {
  sensors_event_t a, g, temp;
  mysensor.getEvent(&a, &g, &temp);

  float accx = a.acceleration.x;
  accx = map(accx,  -10, 10, 0, 180);
  myservo1.write(accx);  
  Serial.print("Rotasi Servo X: ");
  Serial.print(accx);
  

  float accy = a.acceleration.y;
  accy = map(accy,  -10, 10, 0, 180);
  myservo2.write(accy);  
  Serial.print(", Y: ");
  Serial.print(accy);

  float accz = a.acceleration.z;
  accz = map(accz,  -10, 10, 0, 180);
  myservo3.write(accz);
  Serial.print(", z: ");
  Serial.print(accz);
  Serial.println("r/s");
  delay(500);
 
  
}