#include "Ultrasonic.h"

Ultrasonic sensor1(13,A0);
Ultrasonic sensor2(13,A1);
Ultrasonic sensor3(13,A2);
Ultrasonic sensor4(13,A3);

float distance_m;

void setup() {
  Serial.begin(9600);
  pinMode(13, OUTPUT);
  pinMode(11, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(8, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(4, OUTPUT);
}
void loop() {
  float Sensor1 =  distance(sensor1,10,9);
  float Sensor2 =  distance(sensor2,12,11);
  float Sensor3 =  distance(sensor3,7,5);
  float Sensor4 =  distance(sensor4,8,6);
  Serial.println(' ');

  if(Sensor1 < 0.5 || Sensor2 < 0.5 || Sensor3 < 0.5 || Sensor4 < 0.5) {
    noTone(4);
    tone(4,150);
  }
  else if(Sensor1 < 1.5 || Sensor2 < 1.5 || Sensor3 < 1.5 || Sensor4 < 1.5) {
    noTone(4);
    tone(4,150);
    delay(600);

    noTone(4);
    delay(900);
  }
  else if(Sensor1 < 3 || Sensor2 < 3 || Sensor3 < 3 || Sensor4 < 3) {
    noTone(4);
    tone(4,150);
    delay(200);

    noTone(4);
    delay(900);
  }
  else if(Sensor1 > 3 || Sensor2 > 3 || Sensor3 > 3 || Sensor4 > 3) {
    noTone(4);
  }
}
float distance(Ultrasonic sensor,int Red,int Green) {
  int distance_cm = sensor.read(CM);
  distance_m = distance_cm * 0.01;

  Serial.print(distance_m);
  Serial.print(' ');
  Serial.print(' ');

  if(distance_m < 0.5) {
    digitalWrite(Red, HIGH);
    analogWrite(Green,0);
  }
  else if(distance_m < 1.5) {
    digitalWrite(Red, HIGH);
    analogWrite(Green,165);
  }
  else if(distance_m < 3) {
    digitalWrite(Green, HIGH);
    digitalWrite(Red, LOW);

  }
  else if(distance_m > 3) {
    digitalWrite(Green, LOW);
    digitalWrite(Red, LOW);
  }
  delay(100);
  return distance_m;
}