#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Servo.h>

Servo myservo;

int belok = 90;

const int trig = 9;
const int echo = 10;
long durasi;
int jarak;

#define Screen_lebar 128
#define Screen_panjang 64
#define Reset -1
#define Screen_address 0x3C

Adafruit_SSD1306 display(Screen_lebar, Screen_panjang, &Wire, Reset);

void setup() {
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);
  myservo.attach(3);
  if(!display.begin(SSD1306_SWITCHCAPVCC, Screen_address)){
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }
   display.clearDisplay();
    display.setTextSize(3);
    display.setTextColor(SSD1306_WHITE);
    display.cp437(true);

    display.setCursor(0,0);
    display.println("KONTOL");

    display.display();

}

void loop() {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  durasi =pulseIn(echo,HIGH);
  jarak = durasi / 58.2;


  if(jarak <=180){
      display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    display.cp437(true);

    display.setCursor(0,0);
    display.println("hi");

    display.display();
   
    myservo.write(0);
     delayMicroseconds(10000);
     myservo.write(0);
     delayMicroseconds(10000);
     myservo.write(0);
     delayMicroseconds(10000);
     myservo.write(0);
     delayMicroseconds(10000);
     myservo.write(0);
     delayMicroseconds(10000);
     myservo.write(0);
     delayMicroseconds(10000);
     myservo.write(0);
     delayMicroseconds(10000);
      myservo.write(belok);
     
 
  }else {

    myservo.write(belok);
      display.clearDisplay();
    display.setTextSize(3);
    display.setTextColor(SSD1306_WHITE);
    display.cp437(true);

    display.setCursor(0,0);
    display.println("KONTOL");

    display.display();
  }

}