Problema con Servo y ultrasonico en arduino nano

Estoy intentando hacer unos “lentes para personas invidentes” con nano, utilizo un tipico sensor ultrasonico HC-SR04 para medir la distancia y a su vez un MPU6050 para medir el angulo de inclinacion de dicho sensor con respecto a la vertical para a manera de teorema de pitagoras detectar objetos a diferentes distancias y de diferentes magnitudes cerca de la persona; para avisarle al usuario la cercania de un objeto uso la funcion Tone y conecto un audifono pero necesito ser capas de mover todo esto con un servomotor y que se mueva lo mas lento posible de arriba hacia abajo, pero no e encontrado una manera que me permita hacerlo mientras pues los sensores leen, no puedo alterar ningún timer porque uso la función milis, la librería servo y la función tone, dejo el código sin meter nada de servo porque la verdad nada me a funcionado, al final no se usara el monitor serial sin embargo aqui sigo usando para poder ver las medidas exactas que estoy tomando

#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"
#include <Servo.h>

Servo servoMotor;
int salida = 3;
int i;
int i2;
int is;
// La dirección del MPU6050 puede ser 0x68 o 0x69, dependiendo 
// del estado de AD0. Si no se especifica, 0x68 estará implicito
MPU6050 sensor;

// Valores RAW (sin procesar) del acelerometro y giroscopio en los ejes x,y,z
int ax, ay, az;
int gx, gy, gz;

long tiempo_prev;
float dt;
float ang_x, ang_y;
float ang_x_prev, ang_y_prev;

int echopin = 9;
int trigpin = 8;
long lectura;
long tiempo;

float lim;
float Persona=15;
float c;
float distancia;
float yrad;
float cang;
float sang;
float h;

float freq;
void setup() {
  servoMotor.attach(5);
  servoMotor.write(0);
  Serial.begin(57600);    //Iniciando puerto serial
  Wire.begin();           //Iniciando I2C  
  pinMode(echopin, INPUT);
  pinMode (trigpin, OUTPUT);
    pinMode(salida, OUTPUT);
  sensor.initialize();    //Iniciando el sensor

  if (sensor.testConnection()) Serial.println("Sensor iniciado correctamente");
  else Serial.println("Error al iniciar el sensor");
}

void loop() {

  // Leer las aceleraciones y velocidades angulares
  sensor.getAcceleration(&ax, &ay, &az);
  sensor.getRotation(&gx, &gy, &gz);
  
  dt = (millis()-tiempo_prev)/1000.0;
  tiempo_prev=millis();
  
  //Calcular los ángulos con acelerometro
  float accel_ang_y=atan(-ax/sqrt(pow(ay,2) + pow(az,2)))*(180.0/3.14);
  
  //Calcular angulo de rotación con giroscopio y filtro complemento  
  ang_y = 0.98*(ang_y_prev+(gy/131)*dt) + 0.02*accel_ang_y;
  ang_y_prev=ang_y;
digitalWrite(trigpin,LOW);
delayMicroseconds(5);
digitalWrite(trigpin,HIGH);
delayMicroseconds(10);
tiempo = pulseIn(echopin,HIGH);
lectura = 0.017*tiempo;
 h=lectura;
 yrad=(ang_y*3.141592)/180;
  cang=cos(yrad);
  c=Persona/cang;
  sang=sin(yrad);
  distancia=h*sang;
  //Mostrar los angulos separadas por un [tab] 
  Serial.print("tRotacion en Y: ");
  Serial.println(ang_y);
    Serial.print("RAD Y: ");
  Serial.println(yrad);
    Serial.print("cosY: ");
  Serial.println(cang);
  Serial.print("h =");
Serial.print(lectura); 
Serial.println("CM");
  Serial.print("C =");
Serial.print(c); 
Serial.println("CM");
Serial.print("Distancia =");
Serial.print(distancia);
Serial.println("CM");
  delay(10);
  freq = map(distancia, 0, 210, 1050, 400);
  tone (salida, freq, 300); //De aquí borrar el "300" si se quiere un tono continuo
  delay(500); // Borrar esta línea completa si se quiere un tono continuo


}