    steps = max( steps, wDiff );
    DEBUG("Steps: "); DEBUGln(steps);
    
+   int delayus = round(((double)1000*g_moveDelay)/steps);
+   
+   DEBUG("Delayus: "); DEBUGln(delayus);
+   
    if( steps < 2 )
    {
       ret_val = moveArm( basPulse, shlPulse, elbPulse, wriPulse );
+      delayMicroseconds(delayus);
    }
    else
    {

   if ( ( ret_val = servoWritePulse( ELB, elbPulse ) ) != 0 ) return ret_val;
   if ( ( ret_val = servoWritePulse( WRI, wriPulse ) ) != 0 ) return ret_val;
   
   delay(g_moveDelay);
   
   return ret_val;
}

            else Serial.println(ret_val);
            break;
         case 'R':
            moveArm(degToPulse(BAS, BAS_PARK), degToPulse(SHL, SHL_PARK), degToPulse(ELB, ELB_PARK), degToPulse(WRI, WRI_PARK)); Serial.println("ARM PARKED!"); break;
            
         case 'l': //start of setting light PWM value
            i=0; buf[i] = Serial.read(); delay(1);
