#include <Wire.h>
#include <MPU6050.h>
#include <Servo.h>
#include <LiquidCrystal_I2C.h>

// Inisialisasi objek MPU6050, Servo, dan LCD
MPU6050 mpu;
Servo myServo;
LiquidCrystal_I2C lcd(0x27, 16, 2); // Alamat I2C LCD bisa berbeda (0x27 atau 0x3F)

// Variabel untuk menyimpan nilai akselerasi dan suhu
int16_t ax, ay, az;
int16_t tempRaw;
float temperature;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  
  // Inisialisasi MPU6050
  mpu.initialize();

  // Inisialisasi Servo
  myServo.attach(9); // Sambungkan servo ke pin 9

  // Inisialisasi LCD
  lcd.init();
  lcd.backlight();
  
  // Tampilkan pesan awal pada LCD
  lcd.setCursor(0, 0);
  lcd.print("  MPU6050 Test ");
  lcd.setCursor(0, 1);
  lcd.print(" By :  Notdamar ");
  delay(2000);
  lcd.clear();
}

void loop() {
  // Membaca nilai akselerasi dari MPU6050
  mpu.getAcceleration(&ax, &ay, &az);
  
  // Membaca nilai suhu dari MPU6050 (dalam bentuk mentah)
  tempRaw = mpu.getTemperature();
  
  // Konversi nilai mentah suhu ke Celcius (suhu dalam format raw dikonversi ke Celsius)
  temperature = (tempRaw / 340.0) + 36.53;
  
  // Menampilkan nilai akselerasi di Serial Monitor
  Serial.print("Ax: "); Serial.print(ax);
  Serial.print(" Ay: "); Serial.print(ay);
  Serial.print(" Az: "); Serial.println(az);
  
  // Menampilkan suhu di Serial Monitor
  Serial.print("Temperature: ");
  Serial.print(temperature);
  Serial.println(" C");

  // Menghitung sudut berdasarkan nilai akselerasi
  int angleX = map(ax, -17000, 17000, 0, 180);
  
  // Menggerakkan servo berdasarkan sudut
  myServo.write(angleX); // Menggerakkan servo sesuai dengan sumbu X
  
  // Menampilkan nilai akselerasi dan suhu di LCD
  lcd.setCursor(0, 0);
  lcd.print("Temp: ");
  lcd.print(temperature);
  lcd.print(" C   ");
  
  lcd.setCursor(0, 1);
  lcd.print("Ax: ");
  lcd.print(ax);
  
  delay(500); // Tunggu sebentar sebelum pembacaan berikutnya
}