#define trigPin1  3
#define echoPin1  2
#define trigPin2  4
#define echoPin2  5
#define trigPin3  7
#define echoPin3  8
#define ledPin    9


// SR04 rs = SR04(trigPin1, echoPin1);
// SR04 ls = SR04(trigPin2, echoPin2);
// SR04 fs = SR04(trigPin3, echoPin3);

int duration, distance, RightSensor,BackSensor,FrontSensor,LeftSensor;

void setup()
{
Serial.begin (9600);
pinMode(trigPin1, OUTPUT);
pinMode(echoPin1, INPUT);

pinMode(trigPin2, OUTPUT);
pinMode(echoPin2, INPUT);

pinMode(trigPin3, OUTPUT);
pinMode(echoPin3, INPUT);

pinMode(ledPin, OUTPUT);
}

void loop() {
  SonarSensor(trigPin1, echoPin1);
  RightSensor = distance;
  SonarSensor(trigPin2, echoPin2);
  LeftSensor = distance;
  SonarSensor(trigPin3, echoPin3);
  FrontSensor = distance;

  if (RightSensor < 100){
    digitalWrite(ledPin, HIGH);
  }
  else{
    digitalWrite(ledPin, LOW);
  }

  if (LeftSensor < 100){
    digitalWrite(ledPin, HIGH);
  }
  else{
    digitalWrite(ledPin, LOW);
  }

  Serial.print("\n");
  Serial.print("Right: ");
  Serial.print(RightSensor);
  Serial.print("\n");

  Serial.print("Left: ");
  Serial.print(LeftSensor);
  Serial.print("\n");

  Serial.print("Front: ");
  Serial.print(FrontSensor);
  Serial.print("\n------------------");

  delay(500);
}

void SonarSensor(int trigPin,int echoPin)
{
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
// Sets the trigPin HIGH (ACTIVE) for 10 microseconds
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
// Reads the echoPin, returns the sound wave travel time in microseconds
duration = pulseIn(echoPin, HIGH);
// Calculating the distance
distance = duration * 0.034 / 2; // Speed of sound wave divided by 2 (go and back)

}