Error de lectura de HC-SR04 al activar servos

Hola.
He fabricado un robot con cartón basándome en los proyectos "Robot Otto" y "uBipedino" de la web.
El caso es que una vez creado el código encuentro que las funciones de movimiento corren sin problemas y el sensor de distancia HC-SR04 mide correctamente si desactivo las funciones de movimiento.
Cuando todo el código esta activo, los resultados de medición de distancia o bien son unos pocos centimetros, o bien alterna una medida correcta con otra de escasos centimetros.
Me he quedado sin recursos para intentar solucionar el problema.
Agradezco cualquier ayuda.

/*
 *
 */

#include <Servo.h>
#include <NewPing.h>

#define ServoActiu 12

//definicio de pins sensor distancia
#define EchoPin 4
#define TriggerPin 5
#define Max_distance 200

NewPing sonar(TriggerPin, EchoPin, Max_distance);

// creem objectes Servo per cada servo
Servo CamaE;
Servo CamaD;
Servo PeuE;
Servo PeuD;

// variables per guardar la posicio centrada dels servos
int CEcentre = 85;
int CDcentre = 85;
int PEcentre = 85;
int PDcentre = 85;

// variables per guardar l'angle dels servos
int iAngle = 30; //inclinacio
int gAngle = 25; //gir
int bAngle = 35; //balanceig

int velocitat = 50; //factor de velocitat del moviment
int distanciaCm = 0;
  
void setup() 
{
  Serial.begin(9600);
  // assigna pins a sensor distancia
  pinMode(TriggerPin, OUTPUT);
  pinMode(EchoPin, INPUT);
  pinMode(ServoActiu, OUTPUT);

  ActivaServos();  
  CentraServos();  //centra els servos
  DesactivaServos();  
  delay(1000);

}

void loop() 
{
  delay(2000);
  distanciaCm = sonar.ping_cm();
  
  if (distanciaCm>15)
  {
    Serial.println(distanciaCm);
    Endavant(1,velocitat);
  } 
  else
  {
    Serial.println(distanciaCm);
    GiraDreta(2,velocitat);
  }
    
}

void CentraServos()
{
  CamaE.write(CEcentre);
  CamaD.write(CDcentre);
  PeuE.write(PEcentre);
  PeuD.write(PDcentre);
  delay(100);
}


void ActivaServos()
{
  // assigna el pin x a l'objecte Servo
  CamaE.attach(10);
  CamaD.attach(7);
  PeuE.attach(8);
  PeuD.attach(9);
}

void DesactivaServos()
{
  CamaE.detach();
  CamaD.detach();
  PeuE.detach();
  PeuD.detach();
}

//Funcions Basiques Servo

void AlcaPeuE (byte ang, byte v)
{  
  for (int i=0; i<=ang; i+=5)
  {
    PeuE.write(PEcentre-i);
    PeuD.write(PDcentre-i);
    delay(v);
  }
}

void BaixaPeuE (byte ang, byte v)
{
  for (int i=ang; i>0; i-=5)
  {
    PeuE.write(PEcentre-i);
    PeuD.write(PDcentre-i);
    delay(v);
  }
}

void AlcaPeuD (byte ang, byte v)
{  
  for (int i=0; i<=ang; i+=5)
  {
    PeuE.write(PEcentre+i);
    PeuD.write(PDcentre+i);
    delay(v);
  }
}

void BaixaPeuD (byte ang, byte v)
{
  for (int i=ang; i>0; i-=5)
  {
    PeuE.write(PEcentre+i);
    PeuD.write(PDcentre+i);
    delay(v);
  }
}

void AvantCamaE (byte ang, byte v)
{
  for (int i=0; i<=ang; i+=5)
  {
    CamaE.write(CEcentre-i);
    CamaD.write(CDcentre-i);
    delay(v);
  }
}

void CentraCamaE (byte ang, byte v)
{
  for (int i=ang; i>0; i-=5)
  {
    CamaE.write(CEcentre-i);
    CamaD.write(CDcentre-i);
    delay(v);
  }
}

void AvantCamaD (byte ang, byte v)
{
  for (byte i=0; i<=ang; i=i+5)
  {
    CamaE.write(CEcentre+i);
    CamaD.write(CDcentre+i);
    delay(v);
  }
}

void CentraCamaD (byte ang, byte v)
{
  for (byte i=ang; i>0; i=i-5)
  {
    CamaE.write(CEcentre+i);
    CamaD.write(CDcentre+i);
    delay(v);
  }
}

void DinsCamaDreta(byte ang, byte v)
{
  for (int i=0; i<=ang; i+=5){
    CamaD.write(CDcentre-i);
    delay(v);
  }
}

void CentraDinsCamaDreta(byte ang, byte v)
{
  for (int i=ang; i>0; i-=5){
    CamaD.write(CDcentre-i);
    delay(v);
  }
}

void ForaCamaDreta(byte ang, byte v)
{
  for (int i=0; i<=ang; i+=5){
    CamaD.write(CDcentre+i);
    delay(v);
  }
}

void CentraForaCamaDreta(byte ang, byte v)
{
  for (int i=ang; i>0; i-=5){
    CamaD.write(CDcentre+i);
    delay(v);
  }
}

void DinsCamaEsquerra(byte ang, byte v)
{
  for (byte i=0; i<=ang; i=i+5){
    CamaE.write(CEcentre+i);
    delay(v);
  }
}

void CentraDinsCamaEsquerra(byte ang, byte v)
{
  for (byte i=ang; i>0; i=i-5){
    CamaE.write(CEcentre+i);
    delay(v);
  }
}

void ForaCamaEsquerra(byte ang, byte v)
{
  for (byte i=0; i<=ang; i=i+5){
    CamaE.write(CEcentre-i);
    delay(v);
  }
}

void CentraForaCamaEsquerra(byte ang, byte v)
{
  for (byte i=ang; i>0; i=i-5){
    CamaE.write(CEcentre-i);
    delay(v);
  }
}

// Funcions moviment

void Endavant(byte Passos, byte Vel)
{
  Serial.println("Inici endavant");
  ActivaServos();
  AlcaPeuD(iAngle,Vel);
  for (byte j=0; j<Passos; ++j)
  {
    AvantCamaD(bAngle,Vel);
    BaixaPeuD(iAngle,Vel);
    AlcaPeuE(iAngle,Vel);
    CentraCamaD(bAngle,Vel);
    AvantCamaE(bAngle,Vel);
    BaixaPeuE(iAngle,Vel);
    AlcaPeuD(iAngle,Vel);
    CentraCamaE(bAngle,Vel);  
  }
  BaixaPeuD(iAngle,Vel);
  DesactivaServos(); 
  Serial.println("Fi endavant");
}

void GiraDreta(byte Passos, byte Vel)
{
  Serial.println("Inici gira");
  /*
  ActivaServos();
  AlcaPeuD(iAngle,Vel);
  for (byte j=0; j<Passos; ++j)
  {
    ForaCamaDreta(gAngle,Vel);
    BaixaPeuD(iAngle,Vel);
    AlcaPeuE(iAngle,Vel);
    delay(20);
    CentraForaCamaDreta(gAngle,Vel);
    DinsCamaDreta(gAngle,Vel);
    BaixaPeuE(iAngle,Vel);
    AlcaPeuD(iAngle,Vel);
    delay(20);
    CentraDinsCamaDreta(gAngle,Vel);
  }
  BaixaPeuD(iAngle,Vel);
  DesactivaServos();
  */
  Serial.println("Fi gira");
}

void GiraEsquerra(byte Passos, byte Vel)
{
  ActivaServos();
  AlcaPeuE(iAngle,Vel);
  for (byte j=0; j<Passos; ++j)
  {
    ForaCamaEsquerra(gAngle,Vel);
    BaixaPeuE(iAngle,Vel);
    AlcaPeuD(iAngle,Vel);
    delay(20);
    CentraForaCamaEsquerra(gAngle,Vel);
    DinsCamaEsquerra(gAngle,Vel);
    BaixaPeuD(iAngle,Vel);
    AlcaPeuE(iAngle,Vel);
    delay(20);
    CentraDinsCamaEsquerra(gAngle,Vel);
  }
  BaixaPeuE(iAngle,Vel);
  DesactivaServos();
}

Por favor, edita el post, no pongas enlaces externos. Si el código es muy largo y no te deja ponerlo con etiquetas code, añadelo como attachment.

Ok. Editado.
No puedo adjuntar archivos, soy nuevo :hugs: