   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);
-   DEBUGln("Serial connection started\n");  
+   Serial.println("Serial connection started\n");  
    
    pinMode(STEP_PIN, OUTPUT);
    pinMode(DIR_PIN, OUTPUT);

   
   for(int i=0; i<steps; i++)
   {
     long int delay_us = round(1000*(DELAY_MAX - (DELAY_MAX-delay_min)*sin((double)i*PI/(steps-1))));
     Serial.println(delay_us);
     if ( ( ret_val = moveArmCoord( rMove, hMove, bMove, dMove, delay_us ) )  != 0 ) return ret_val;
     rMove += rStep; hMove += hStep; bMove += bStep; dMove += dStep;
   }

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