// Define LCD library, lines, and coloumns
#include  <Wire.h>
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 20, 4);

// Define pin connections & motor's steps per revolution
int dirPin = 16;
int stepPin = 17;
const int stepsPerRevolution = 200;

// Define pin connections for Ultrasonic sensor
#define trigPin 26 // ESP32 pin GPIO26 connected to Ultrasonic Sensor's TRIG pin
#define echoPin 25 // ESP32 pin GPIO25 connected to Ultrasonic Sensor's ECHO pin
float duration, distance;

void setup() {
// Declare pins as Outputs (Motors)
	pinMode(stepPin, OUTPUT);
	pinMode(dirPin, OUTPUT);

// Declare pins as Outputs and Input (Ultrasonic)
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

// Initiation for LCD
  lcd.init();
  lcd.backlight();
	lcd.setCursor (1,0);
	lcd.print("TEST LOCKER SYSTEM");
	lcd.setCursor (9,1);
	lcd.print ("BY");
	lcd.setCursor (6,2);
	lcd.print ("DEV TEAM");
	lcd.setCursor (0,3);
	lcd.print ("--------------------");
	delay (5000);
	lcd.clear();
}


void loop() {
  // generate 10-microsecond pulse to TRIG pin
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  // measure duration of pulse from ECHO pin
  duration = pulseIn(echoPin, HIGH);
  // calculate the distance
  distance = 0.017 * duration;


	// Print Equation to LCD
	lcd.setCursor (0,0);
	lcd.print ("Jarak :");
			lcd.setCursor (8,0);
		lcd.print (distance);
	lcd.setCursor (0,2);
	lcd.print ("Status :");

if (distance >=69 ){
	// Set motor direction clockwise
	digitalWrite(dirPin, HIGH);

	// Spin motor slowly
	for(int x = 0; x < stepsPerRevolution; x++)
	{
		digitalWrite(stepPin, HIGH);
		delayMicroseconds(2000);
		digitalWrite(stepPin, LOW);
		delayMicroseconds(2000);
	}
	delay(1000); // Wait a second
	
	// Set motor direction counterclockwise
	digitalWrite(dirPin, LOW);

	// Spin motor quickly
	for(int x = 0; x < stepsPerRevolution; x++)
	{
		digitalWrite(stepPin, HIGH);
		delayMicroseconds(1000);
		digitalWrite(stepPin, LOW);
		delayMicroseconds(1000);
	}
	delay(1000); // Wait a second
}
}