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

Servo myservo;
Adafruit_MPU6050 mpu; 
LiquidCrystal_I2C lcd(0x27, 20, 4);
int servopin = 3;
float ax;

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

  myservo.attach(servopin); 
  Serial.println("Teste do Adafruit MPU6050!");
  
  // Try to initialize!
  if (!mpu.begin()) {
    Serial.println("MPU6050 falhou!!");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 encontrado!");

  lcd.init();
  lcd.backlight();
}

void loop(){
  // Coleta os dados dos sensores
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // Mapeia a aceleração no eixo X para o ângulo do servo
  ax = map(a.acceleration.x, -19.6133, 19.6133, 0, 180);
  
  // Movimenta o servo
  myservo.write(ax);
  
  // Atualiza a tela LCD 
  lcdzinho(a, g); 
}

void lcdzinho(sensors_event_t a, sensors_event_t g) {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Angulo do servo:"); 
  lcd.setCursor(0, 1);  
  lcd.print(ax);
  
  delay(2000);  
  lcd.clear();                        
  lcd.setCursor(0, 0);  
  lcd.print("Accel X: ");
  lcd.print(a.acceleration.x);
  lcd.setCursor(0, 1);  
  lcd.print("Accel Y: ");
  lcd.print(a.acceleration.y);
  lcd.setCursor(0, 2);  
  lcd.print("Accel Z: ");
  lcd.print(a.acceleration.z);
  lcd.setCursor(0, 3);     
  lcd.print("m/s^2");
  
  delay(2000);  
  lcd.clear();   
  lcd.setCursor(0, 0);     
  lcd.print("Gyro X: ");
  lcd.print(g.gyro.x);
  lcd.setCursor(0, 1);   
  lcd.print("Gyro Y: ");
  lcd.print(g.gyro.y);
  lcd.setCursor(0, 2);   
  lcd.print("Gyro Z: ");
  lcd.print(g.gyro.z);
  lcd.setCursor(0, 3);     
  lcd.print("rad/s");
  
  delay(2000);  
}