 
 Servo servo[4];  // servo control objects
 int angles[4] = { 90, 65, 90, 60 };   // initial angles
+int servoPosPin[4] = { servo0PosPin, servo1PosPin, servo2PosPin, servo3PosPin };
 
 void setup()
 {

  duration = pulseIn(sonarEchoPin, HIGH);
  return 0.146 * duration - 23.843;  // returns in mm
}

  Serial.print(" set to ");
  Serial.print( angles[servo_num] );
  Serial.println(" degrees.");

  Serial.print("Measured angle (raw): ");
  Serial.println( analogReadAverage( servoPosPin[servo_num], 10 ) );

  Serial.println();
  Serial.print("Sonar reading: ");
