#include <LiquidCrystal.h>

const int trigPin1 = 6; // Ultrasonic sensor 1 trigger pin
const int echoPin1 = 5; // Ultrasonic sensor 1 echo pin
const int trigPin2 = 4; // Ultrasonic sensor 2 trigger pin
const int echoPin2 = 3; // Ultrasonic sensor 2 echo pin
const int ledPin1 = 13;  // LED 1 pin
const int ledPin2 = 2;  // LED 2 pin

LiquidCrystal lcd(12, 11, 10, 9, 8, 7); // LCD connections: RS, E, D4, D5, D6, D7

void setup() {
  lcd.begin(16, 2); // Initialize the LCD with 16 columns and 2 rows
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(ledPin1, OUTPUT);
  pinMode(ledPin2, OUTPUT);
}

void loop() {
  long duration1, distance1, duration2, distance2;
  int speed;

  // Measure distance using ultrasonic sensor 1
  digitalWrite(trigPin1, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin1, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin1, LOW);
  duration1 = pulseIn(echoPin1, HIGH);
  distance1 = duration1 * 0.034 / 2;

  // Measure distance using ultrasonic sensor 2
  digitalWrite(trigPin2, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin2, LOW);
  duration2 = pulseIn(echoPin2, HIGH);
  distance2 = duration2 * 0.034 / 2;

  // Calculate speed based on the difference in distances
  speed = abs(distance1 - distance2);

  lcd.clear(); // Clear the LCD screen
  lcd.setCursor(0, 0);
  lcd.print("Speed: ");
  lcd.print(speed);
  lcd.print(" km/h");

  // Activate the appropriate LED based on speed
  if (speed <= 40) {
    digitalWrite(ledPin1, HIGH);
    digitalWrite(ledPin2, LOW);
  } else {
    digitalWrite(ledPin1, LOW);
    digitalWrite(ledPin2, HIGH);
    //tone(7, 220, 100);
    delay(200);
  }

  delay(100);
}