    if ( steps == 0 ) return 0;
    
    long int delay_us = round((double)delay_/steps);
-   Serial.print("Delay_us: "); Serial.println(delay_us);
+   //Serial.print("Delay_us: "); Serial.println(delay_us);
    
    if( steps < 2 )
    {
       ret_val = moveArm( basPulse, shlPulse, elbPulse, wriPulse );
-      
-      if (delay_us > 1000) delay(delay_us/1000);
-      else delayMicroseconds(delay_us);
+      delayFunc(delay_us);
    }
    else
    {

         
         if (scaleDelay)
         {
           int delay_ms = DELAY_MAX - round((DELAY_MAX-delay_)*sin((double)i*PI/(steps-1)));
           delay(delay_ms);
           Serial.println(delay_ms);
         }
         else
         {
           if (delay_us > 1000) delay(delay_us/1000);
           else delayMicroseconds(delay_us);
         }
         
      }
   }


  return ((double)pulseLength - OFFSET[id])/GRADIENT[id];
}

void delayFunc ( long int delay_ )
{
  long int start = micros();
  int delay_ms = delay_/1000;
  int delay_us = delay_%1000;
  if ( delay_ms > 0 ) delay(delay_ms);
  if ( delay_us > 0 ) delayMicroseconds(delay_us);
  long int fin = micros();
  /*Serial.print(delay_ms);
  Serial.print("+");
  Serial.print(delay_us);
  Serial.print("=");
  Serial.println(fin-start);*/
}

/*********/
/* SETUP */
/*********/
void setup()
{
   Serial.begin(57600);
   Serial.println("Serial connection started\n");  
   
   pinMode(STEP_PIN, OUTPUT);
   pinMode(DIR_PIN, OUTPUT);
