#include <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>
#include <Ultrasonic.h>

#define Lebar 120
#define Tinggi 64


int jarakMaju, jarakKiri, jarakKanan, jarakMundur;

Adafruit_SSD1306 oled(Lebar, Tinggi, &Wire, -1);


Ultrasonic sensorMaju(7, 8);      
Ultrasonic sensorKanan(5, 6);     
Ultrasonic sensorKiri(10, 9);     

void setup() 
{
  Serial.begin(9600);
  oled.begin(SSD1306_SWITCHCAPVCC, 0x3c);
  oled.clearDisplay();
  oled.setTextSize(0); 
  oled.setTextColor(WHITE);
  oled.setRotation(2);
}

void loop() 
{
  
  jarakMaju = sensorMaju.read(CM);
  jarakKanan = sensorKanan.read(CM);
  jarakKiri = sensorKiri.read(CM);

  oled.clearDisplay();
  
     if ((jarakMaju <=50) || (jarakKiri <=50) && (jarakKanan <=50))
      {
        oled.setCursor(50, 20);
        oled.print("MAJU");

        oled.setCursor(0, 55);
        oled.print("Kir:");
        oled.setCursor(23, 55);
        oled.print(jarakKiri);
  
        oled.setCursor(42, 55);
        oled.print("DPN:");
        oled.setCursor(63, 55);
        oled.print(jarakMaju);

        oled.setCursor(80, 55);
        oled.print("Kan:");
        oled.setCursor(100, 55);
        oled.print(jarakKanan);
      }
     else if ((jarakKiri <=50) || (jarakMaju <=50) && (jarakKanan <=30))
      {
         oled.setCursor(50, 20);
         oled.print("KIRI");

         oled.setCursor(0, 55);
         oled.print("Kir:");
         oled.setCursor(23, 55);
         oled.print(jarakKiri);

         oled.setCursor(42, 55);
         oled.print("DPN:");
         oled.setCursor(63, 55);
         oled.print(jarakMaju);

         oled.setCursor(80, 55);
         oled.print("Kan:");
         oled.setCursor(100, 55);
         oled.print(jarakKanan);
      } 
      else if ((jarakKanan <=50) || (jarakKiri <=30) && (jarakMaju <=50)) 
      {
         oled.setCursor(50, 20);
         oled.print("Kanan");

         oled.setCursor(0, 55);
         oled.print("Kir:");
         oled.setCursor(23, 55);
         oled.print(jarakKiri);

         oled.setCursor(42, 55);
         oled.print("DPN:");
         oled.setCursor(63, 55);
         oled.print(jarakMaju);

         oled.setCursor(80, 55);
         oled.print("Kan:");
         oled.setCursor(100, 55);
         oled.print(jarakKanan);
       } 
  
      else if ((jarakMundur) || (jarakMaju <=50) && (jarakKiri <=50) && (jarakKanan <=50))
       {
         oled.setCursor(50, 20);
         oled.print("KIRI");
       }
      else 
       {
         oled.setCursor(0, 40);
         oled.print("Kan");
         oled.print(jarakKanan);
    
         oled.setCursor(0, 40);
         oled.print("DPN");
         oled.print(jarakMaju);

         oled.setCursor(0, 40);
         oled.print("Kir");
         oled.print(jarakKiri);

         oled.setCursor(0, 40);
         oled.print("MDR");
         oled.print(jarakKiri);
         oled.print(jarakMaju);
         oled.print(jarakKanan);
    
       }
        oled.display();
        delay(500);
}