    DEBUG("slowMoveArm("); DEBUG(basAngle_d); DEBUG(","); DEBUG(shlAngle_d); DEBUG(","); DEBUG(elbAngle_d); DEBUG(","); DEBUG(wriAngle_d); DEBUGln(")");
    
    int ret_val;
-   double bDiff = abs( basAngle_d - g_angle_d[BAS] );
-   double sDiff = abs( shlAngle_d - g_angle_d[SHL] );
-   double eDiff = abs( elbAngle_d - g_angle_d[ELB] );
-   double wDiff = abs( wriAngle_d - g_angle_d[WRI] );
-   
-   double steps = max( bDiff, sDiff );
+   
+   int basPulse = degToPulse( BAS, basAngle_d );
+   int shlPulse = degToPulse( SHL, shlAngle_d );
+   int elbPulse = degToPulse( ELB, elbAngle_d );
+   int wriPulse = degToPulse( WRI, wriAngle_d );
+   
+   int bDiff = abs( basPulse - g_pulse[BAS] );
+   int sDiff = abs( shlPulse - g_pulse[SHL] );
+   int eDiff = abs( elbPulse - g_pulse[ELB] );
+   int wDiff = abs( wriPulse - g_pulse[WRI] );
+   
+   int steps = max( bDiff, sDiff );
    steps = max( steps, eDiff );
    steps = max( steps, wDiff );
-   steps = round(steps);
    DEBUG("Steps: "); DEBUGln(steps);
    
    if( steps < 2 )
    {
-      ret_val = moveArm( basAngle_d, shlAngle_d, elbAngle_d, wriAngle_d );
+      ret_val = moveArm( basPulse, shlPulse, elbPulse, wriPulse );
    }
    else
    {
-      double bStep = (basAngle_d - g_angle_d[BAS])/steps;
-      double sStep = (shlAngle_d - g_angle_d[SHL])/steps;
-      double eStep = (elbAngle_d - g_angle_d[ELB])/steps;
-      double wStep = (wriAngle_d - g_angle_d[WRI])/steps;
+      double bStep = (double)(basPulse - g_pulse[BAS])/steps;
+      double sStep = (double)(shlPulse - g_pulse[SHL])/steps;
+      double eStep = (double)(elbPulse - g_pulse[ELB])/steps;
+      double wStep = (double)(wriPulse - g_pulse[WRI])/steps;
       
-      double bMove = g_angle_d[BAS] + bStep;
-      double sMove = g_angle_d[SHL] + sStep;
-      double eMove = g_angle_d[ELB] + eStep;
-      double wMove = g_angle_d[WRI] + wStep;
+      double bMove = g_pulse[BAS] + bStep;
+      double sMove = g_pulse[SHL] + sStep;
+      double eMove = g_pulse[ELB] + eStep;
+      double wMove = g_pulse[WRI] + wStep;
       
       for(int i=0; i<steps; i++)
       {
-         if( ( ret_val = moveArm( bMove, sMove, eMove, wMove ) ) != 0 ) break;
+         if( ( ret_val = moveArm( round(bMove), round(sMove), round(eMove), round(wMove) ) ) != 0 ) break;
          bMove += bStep; sMove += sStep; eMove += eStep; wMove += wStep;
       }
    }

int     ret_val, g_moveDelay = 50; // g_moveDelay is defined at global scope and is used to control speed of arm
char    buf[MAX_BUFFER+1];
double   number[4], current[4], actual[4], g_angle_d[4];
double   g_radius_mm, g_height_mm, g_hand_d, g_sliderPos;
double   g_prevRadius_mm, g_prevHeight_mm, g_prevBase_d, g_prevHand_d;


// Write to servo using pulse length
int servoWritePulse( int id, int pulseLength)
{
  if ( pulseLength == g_pulse[id] ) return 0;
  if ( pulseLength < SERVO_MIN[id] ) { DEBUG("Pulse too small ("); DEBUG(pulseLength); DEBUG("<"); DEBUG(SERVO_MIN[id]); DEBUGln(")"); return -1; }
  if ( pulseLength > SERVO_MAX[id] ) { DEBUG("Pulse too large ("); DEBUG(pulseLength); DEBUG(">"); DEBUG(SERVO_MAX[id]); DEBUGln(")"); return -2; }
  
  pwm.setPWM(id, 0, pulseLength);
  
  g_pulse[id] = pulseLength;
  
  DEBUG("pwm.setPWM("); DEBUG(id); DEBUG(", 0, "); DEBUG(pulseLength); DEBUGln(")");
  
  return 0;  
