////////////Step Ultrasonics & LED////////////
#define trigPin1 A0 //Ultrasonic 1 Firststep
#define echoPin1 A1 //Ultrasonic 1 Firststep
#define trigPin2 A2 //Ultrasonic 2 Secondstep
#define echoPin2 A3 //Ultrasonic 2 Secondstep
#define trigPin3 A4 //Ultrasonic 3 Thirdstep
#define echoPin3 A5 //Ultrasonic 3 Thirdstep
int ledS1 = 26;
int ledS2 = 27;
int ledS3 = 28;
long duration, distance, uSensorS1, uSensorS2, uSensorS3;


void setup() {
  Serial.begin (9600);
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(trigPin3, OUTPUT);
  pinMode(echoPin3, INPUT);
  pinMode(ledS1, OUTPUT);
  pinMode(ledS2, OUTPUT);
  pinMode(ledS3, OUTPUT);
  digitalWrite(ledS1, LOW);
  digitalWrite(ledS2, LOW);
  digitalWrite(ledS3, LOW);
}

void loop() {
  stepSensor(trigPin1, echoPin1);
  uSensorS1 = distance;
  stepSensor(trigPin2, echoPin2);
  uSensorS2 = distance;
  stepSensor(trigPin3, echoPin3);
  uSensorS3 = distance;
  /////////////////////////////////////////////////////////////
  digitalWrite(ledS1, LOW);
  digitalWrite(ledS2, LOW);
  digitalWrite(ledS3, LOW);

  ////////////First Step Ultrasonics////////////
  if (uSensorS1 <= 200) {
    digitalWrite(ledS1, HIGH);
    delay(500);
  }
  else {
    digitalWrite(ledS1, LOW);
  }
  ////////////Second Step Ultrasonics////////////
  if (uSensorS2 <= 200) {
    digitalWrite(ledS2, HIGH);
    delay(500);
  }
  else {
    digitalWrite(ledS2, LOW);
  }
  ////////////Third Step Ultrasonics////////////
  if (uSensorS3 <= 200) {
    digitalWrite(ledS3, HIGH);
    delay(500);
  }
  else {
    digitalWrite(ledS3, LOW);
  }


  Serial.print("S1:"); Serial.println(uSensorS1); delayMicroseconds(10);
  Serial.print("S2:"); Serial.println(uSensorS2); delayMicroseconds(10);
  Serial.print("S3:"); Serial.println(uSensorS3); delayMicroseconds(10);
}

/////////////////////////////////////////////////////////////
void stepSensor(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = (duration / 2) / 29.1;
}