#define left 7
#define right 8
#define button_left A5
#define button_right A4
#define button_stop A3
#define velocity 6
#define echo 2
#define trigger 4


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

LiquidCrystal_I2C lcd(0x27,16,2);


void setup() {


  pinMode(velocity, OUTPUT);
  pinMode(left, OUTPUT);
  pinMode(right, OUTPUT);
  pinMode(button_left, INPUT_PULLUP);
  pinMode(button_right, INPUT_PULLUP);
  pinMode(button_stop, INPUT_PULLUP);
  pinMode(trigger, OUTPUT);
  pinMode(echo, INPUT);
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0,0);
  lcd.print("Sterowanie brama");
  lcd.setCursor(4,1);
  lcd.print("garazowa");
  delay(1000);
  lcd.clear();
  delay(100);

}

void loop()
{

Serial.begin(9600);
  buttons_function();
  scope(0,20);
  measure();
delay(500);

}



void motor_left()

{
 analogWrite(velocity, 190);
  digitalWrite(left, LOW);
  digitalWrite(right, HIGH);
  delay(100);
}


void motor_right()

{

 analogWrite(velocity, 190);
  digitalWrite(left, HIGH);
  digitalWrite(right, LOW);
  delay(100);

}

void motor_stop()

{

 analogWrite(velocity, 0);
  digitalWrite(left,  LOW);
  digitalWrite(right, LOW);
  delay(1000);
}

int measure() {
  long l_time, distance;

  digitalWrite(trigger, LOW);
  delayMicroseconds(2);
  digitalWrite(trigger, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigger, LOW);

  l_time = pulseIn(echo, HIGH);
  distance = l_time / 58;

  return distance;
}


void scope(int a, int b) {
  int howFar = measure();
  if (howFar > 20)  {
      lcd.clear();
    lcd.setCursor(0,0);
  lcd.setCursor(0,0);
  lcd.print("Sterowanie brama");
  lcd.setCursor(4,1);
  lcd.print("garazowa");
  delay(500);
  } else
  {
lcd.clear();
      lcd.setCursor(0,0);
  lcd.print("Alarm!");
  motor_stop();
  delay(500);
  }
}


void buttons_function()

{

   if(digitalRead(button_left)==LOW)
   {

    motor_left();

   }
    if(digitalRead(button_right)==LOW)
   {

    motor_right();


    }


      if(digitalRead(button_right)==LOW)
   {

    motor_right();


    }
         if(digitalRead(button_stop)==LOW)
   {

    motor_stop();


    }

}