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

Adafruit_MPU6050 mpu;
sensors_event_t a;
byte servo_right_pin = 3;
byte servo_left_pin = 5;
float Kp = 1.0;

Servo servo_left;
Servo servo_right;

void setup() {

  Serial.begin(115200);

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

  servo_left.attach(servo_left_pin);
  servo_right.attach(servo_right_pin);
}

void loop() {

  mpu.getAccelerometerSensor()->getEvent(&a);

  float correctionX = a.acceleration.x * Kp;
  float correctionY = a.acceleration.y * Kp;
  
  
  int speedLeft  = constrain(90 + correctionX, 0, 180);
  int speedRight = constrain(90 - correctionY, 0, 180);
  
  servo_left.write(speedLeft);
  servo_right.write(speedRight);

}