#define LedRed  12
#define LedGreen  13

const int pingPin_1_4 = 2; // Trigger Pin of Ultrasonic Sensor
const int echoPin1 = 8; // Echo Pin of Ultrasonic Sensor
const int echoPin2 = 9; // Echo Pin of Ultrasonic Sensor

const int pingPin_2_3 = 5; // Trigger Pin of Ultrasonic Sensor
const int echoPin3 = 10; // Echo Pin of Ultrasonic Sensor
const int echoPin4 = 11;

void setup() {
      pinMode(2,OUTPUT);
      pinMode(3,OUTPUT);
      pinMode(4,OUTPUT);
      pinMode(5,OUTPUT);
      pinMode(6,OUTPUT);

     pinMode(echoPin1, INPUT);
     pinMode(echoPin2, INPUT);

     pinMode(echoPin3, INPUT);
     pinMode(echoPin4, INPUT);
     
     pinMode(LedRed , OUTPUT);
     pinMode(LedGreen, OUTPUT);

    digitalWrite(LedRed, HIGH);
    digitalWrite(LedGreen , HIGH);

     Serial.begin(9600); // Starting Serial Terminal
     delay(250);
     digitalWrite(LedRed, LOW);
     digitalWrite(LedGreen , LOW);
     Serial.println("starting ...");
}

void loop() {
   long duration, inches, cm, duration2, cm2, duration3, cm3, duration4, cm4;

for(int i = 0; i < 10; i++){
    digitalWrite(pingPin_1_4, LOW);
    delayMicroseconds(2);
    digitalWrite(pingPin_1_4, HIGH);
    delayMicroseconds(10);
    digitalWrite(pingPin_1_4, LOW);

    duration = pulseIn(echoPin1, HIGH);
    cm       = microsecondsToCentimeters(duration);
    
    Serial.print("cm 1 = ");
    Serial.print(cm);
    Serial.println();

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

    duration4 = pulseIn(echoPin4, HIGH);
    cm4       = microsecondsToCentimeters(duration4);

    Serial.print("cm 4 =");
    Serial.print(cm4);
    Serial.println();

    if(cm > 200 && cm4 > 200){   
          digitalWrite(LedRed, LOW);
          digitalWrite(LedGreen, HIGH);
          //noTone(speaker);
      }
      else{
          digitalWrite(LedRed ,HIGH);
          digitalWrite(LedGreen, LOW);
          //tone(speaker, 1000);
      }

  digitalWrite(2,1); //enables the 1rd set of signals
  digitalWrite(7,1);
  digitalWrite(4,0);
  digitalWrite(6,0);
  delay(1000);
}

  digitalWrite(3,1); //enables the yellow lights
  digitalWrite(7,1);
  digitalWrite(2,0); 
  delay(3000);

for(int i = 0; i < 10; i++){
  
    digitalWrite(pingPin_2_3, LOW);
    delayMicroseconds(2);
    digitalWrite(pingPin_2_3, HIGH);
    delayMicroseconds(10);
    digitalWrite(pingPin_2_3, LOW);
    
    duration2 = pulseIn(echoPin2, HIGH);
    cm2       = microsecondsToCentimeters(duration2);
    
    Serial.print("cm 2 = ");
    Serial.print(cm2);
    Serial.println();

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

    duration3 = pulseIn(echoPin3, HIGH);
    cm3       = microsecondsToCentimeters(duration3);

    Serial.print("cm 3 = ");
    Serial.print(cm3);
    Serial.println();

    if(cm2 > 200 && cm3 > 200){   
          digitalWrite(LedRed, LOW);
          digitalWrite(LedGreen, HIGH);
          //noTone(speaker);
      }
      else{
          digitalWrite(LedRed ,HIGH);
          digitalWrite(LedGreen, LOW);
          //tone(speaker, 1000); // You can adjust the frequency of the tone
      }
 
  digitalWrite(4,1); //enables the 2nd set of signals
  digitalWrite(5,1);
  digitalWrite(2,0);
  digitalWrite(3,0);  
  digitalWrite(7,0);
  delay(1000);

}
 
  digitalWrite(6,1); //enables the yellow lights
  digitalWrite(4,1);
  digitalWrite(5,0);  
  delay(3000); 
}

long microsecondsToCentimeters(long microseconds) {
   return (microseconds / 29 / 2);
}