Notes

#define trigPin 2 // Trigger Pin

#define echoPin1 5 // Echo Pin for sensor # 1
#define echoPin2 6 // Echo Pin for sensor # 2
#define echoPin3 7 // Echo Pin for sensor # 3
#define echoPin4 8 // Echo Pin for sensor # 4
#define echoPin5 9 // Echo Pin for sensor # 5
#define echoPin6 10 // Echo Pin for sensor # 6

#define SENSORS_NUMBER 6
#define MAXIMUM_RANGE 200 // Maximum range needed
#define MINIMUM_RANGE 0 // Minimum range needed

volatile boolean time_elapsed;
volatile byte i;

volatile byte vect[] = {-2, -2, -2, -2, -2, -2};

void read_distance(byte sensor_trig_pin, byte sensor_echo_pin, byte sensor_id);

void setup()
{
pinMode(trigPin, OUTPUT);

pinMode(echoPin1, INPUT);
pinMode(echoPin2, INPUT);
pinMode(echoPin3, INPUT);
pinMode(echoPin4, INPUT);
pinMode(echoPin5, INPUT);
pinMode(echoPin6, INPUT);

// open the serial port at 9600 bps:
Serial.begin(9600);

// initialize Timer1
cli(); // disable global interrupts

TCCR1A = 0; // set entire TCCR1A register to 0
TCCR1B = 0; // set entire TCCR1B register to 0

// set compare match register to desired timer count, corresponds to 1 second
OCR1A = 15624; // Timer 1 Output Compare Register A

// turn on CTC mode on Timer1:
TCCR1B |= (1 << WGM12);

// Set CS10 and CS12 bits for 1024 prescaler:
TCCR1B |= (1 << CS10);
TCCR1B |= (1 << CS12);

// enable timer compare interrupt:
TIMSK1 |= (1 << OCIE1A);

// enable Timer1 overflow interrupt:
//TIMSK1 = (1 << TOIE1);

// enable global interrupts:
sei();

time_elapsed = false;
}

ISR(TIMER1_COMPA_vect)
{
read_distance(trigPin, echoPin1, 0);
read_distance(trigPin, echoPin2, 1);
read_distance(trigPin, echoPin3, 2);
read_distance(trigPin, echoPin4, 3);
read_distance(trigPin, echoPin5, 4);
read_distance(trigPin, echoPin6, 5);

time_elapsed = true;
}

void loop()
{
if(time_elapsed){

for(i = 0; i < SENSORS_NUMBER; i++){
Serial.print(" # ");
Serial.print(i+1);
Serial.print(" :");
Serial.println(vect[i]);
}
time_elapsed = false;
}
}

void read_distance(byte sensor_trig_pin, byte sensor_echo_pin, byte sensor_id){

long duration; // Duration used to calculate distance
long distance;

digitalWrite(sensor_trig_pin, LOW);
delayMicroseconds(2);

digitalWrite(sensor_trig_pin, HIGH);
delayMicroseconds(10);

digitalWrite(sensor_trig_pin, LOW);
duration = pulseIn(sensor_echo_pin, HIGH);

//Calculate the distance (in cm) based on the speed of sound.
distance = duration/58.2;

if (!(distance >= MAXIMUM_RANGE || distance <= MINIMUM_RANGE)){
vect[sensor_id] = distance;
}
else{
vect[sensor_id] = -1;
}
}