Problema Robot fermo con interrupt attivato

Salve, riporto di seguito una parte del codice del mio robot a 4 zampe:

#include <Servo.h>
#include "D:\Progetto sp4\software\sp4_legged_robot\funzioni.h"
#include "D:\Progetto sp4\software\sp4_legged_robot\DueTimer.h"

int microsec=0;
long distanza=0;
int stato=LOW;
char i=LOW;


void lunghezza()
{ 
     //fai rimanere il led acceso se rilevi un ostacolo che si trova sopra la distanza 36000
     if(distanza>=36000)      {stato=HIGH;}   //controllare questo
     else stato=LOW;   
     //azzera distanza rilevata   
     distanza=0;
}


void temporizzazione()
{   
      //genera un impulso di TRIGER di 10usec e spegni led  
      digitalWrite(TRIG_CENTRO, LOW);
      digitalWrite(LED, LOW); 
      digitalWrite(TRIG_CENTRO, HIGH);
      delayMicroseconds(10);
      digitalWrite(TRIG_CENTRO, LOW);
      //accendi Timer rilevazione eco
      Timer7.start();
}

void sonar()
{
      //rileva presenza dell' impulso dell' eco
      if(digitalRead(ECHO_CENTRO)==HIGH)      microsec++;
      //fai lampeggiare il led ad una frequenza direttamente proporzionale alla distanza dell' ostacolo
      if(microsec>=100)    digitalWrite(LED, stato);            
      else {digitalWrite(LED, HIGH);   distanza++;}   
      //se non rilevi nessun ostacolo azzera distanza rilevata e ferma il Timer rilevazione eco
      if(microsec>=36000)    {microsec=0;  Timer7.stop();}     
}


void setup() {
  //configurazione delle uscite PWM
  braccio_dx.attach(2);
  avanbraccio_dx.attach(3);
  braccio_sx.attach(4);
  avanbraccio_sx.attach(9);    
  
  gamba_dx.attach(6);
  piede_dx.attach(7);
  gamba_sx.attach(11);
  piede_sx.attach(10);
  
  //configurazione degli 8 pulsanti;
  pinMode(51, INPUT);
  pinMode(49, INPUT);
  pinMode(47, INPUT);
  pinMode(45, INPUT);
  pinMode(43, INPUT);
  pinMode(41, INPUT);
  pinMode(39, INPUT);
  pinMode(37, INPUT);
  
  //configurazione degli I/O per gestione dei tre sonar
  pinMode(LED, OUTPUT);
  pinMode(ECHO_SX, INPUT);
  pinMode(ECHO_DX, INPUT);
  pinMode(ECHO_CENTRO, INPUT);
  pinMode(TRIG_CENTRO, OUTPUT);
  pinMode(TRIG_SX, OUTPUT);
  pinMode(TRIG_DX, OUTPUT);
  
  //configurazione interrupt rilevazione sonar
  Timer7.attachInterrupt(sonar).start(1);                   //testa presenza del segnale di eco ogni 1usec.          
  Timer7.stop(); 
  Timer6.attachInterrupt(temporizzazione).start(50000);    //attiva timer della temporizzazione ogni 50msec.
  Timer6.stop();   //attivare
  //interrupt per il calcolo della distanza e decisione dell' azione da compiere
  Timer8.attachInterrupt(lunghezza).start(1000000);        //testa distanza ogni 1sec.
  Timer8.stop();  
}

 
void loop() {
  allinea_servi();    //chiama funzione di allineamento al centro servi per reggere sp4
  //while(1){}; 
  inverti_senso();    //imposta senso iniziale avanti 
  delay(5000);        //genera una temporizzazione di 5 sec prima di procedere con la camminata
  
  passi=0;            //cammina all' infinito
  while(1)
  {
    if(senso_avanti=2)     {senso_avanti=0; passi=0; inverti_senso();}      //riabilita la camminata in avanti dopo la rotazione di 2 passi e riabilita rilevazione ostacoli
    switch(pulsante)
    {
      case 3: svolta(DX);
      break;
      case 4: svolta(SX);
      break;  
      case 5: svolta(SX);
      break; 
      case 6: svolta(DX);
    }     
    if(svolta_==1)    {passi=2;}                    //se sono state abilitate le var di svolta fai 2 passi di rotazione su se stesso  
    camminata(passi);                                  //cammina in avanti 
  }   
}

Ho aggiunto nella parte anteriore 3 sonar per rilevazione ostacoli.
Mi succede che se metto a start() la subroutine sonar() il robot non si muove più,
ma allinea le zampe secondo la funzione allinea_servi() e rimane li fermo.
Se invecie metto la sub sonar() a stop() esso torna a muoversi.
Non capisco il perchè di questo comportamento anomalo.
Con la funzione sonar() messa a start() ed il codice in essa contenuta ottengo che
faccio lampeggiare un led con una frequenza direttamente proporzionale alla distanza
dell’ ostacolo.
Nessuno mi può aiutare a capire il perchè dell’ arresto del robot se attivo gli interrupt?

La misura dei sensori blocca il codice. Per quello non si muove piú il robot. Potrebbe migliorare se la libreria lo consente limiti il raggio di misura con un timeout corto.

Ciao Uwe

Cosa intendi per timeout corto ?
Spiegati meglio.
Dici che dovrei far partire la sub temporizzazione() con tempi superiori a 50msec oppure la sub sonar()
con tempi superiori a 1usec ?