    servoWrite( ELB, ELB_PARK );
    servoWrite( WRI, WRI_PARK );
    servoWrite( GRI, GRI_OPEN );
-   DEBUG("Robot parked: {"); DEBUG(g_angle_d[BAS]); DEBUG(","); DEBUG(g_angle_d[SHL]); DEBUG(","); DEBUG(g_angle_d[ELB]); DEBUG(","); DEBUG(g_angle_d[WRI]); DEBUGln(",delay}");
+   //DEBUG("Robot parked: ("); DEBUG(g_pulse[BAS]); DEBUG(","); DEBUG(g_pulse[SHL]); DEBUG(","); DEBUG(g_pulse[ELB]); DEBUG(","); DEBUG(g_pulse[WRI]); DEBUG(","); DEBUG(g_pulse[GRI]); DEBUGln(")");
+   DEBUG("Robot parked: {"); DEBUG(pulseToDeg( BAS, g_pulse[BAS] )); DEBUG(","); DEBUG(pulseToDeg( SHL, g_pulse[SHL] )); DEBUG(","); DEBUG(pulseToDeg( ELB, g_pulse[ELB] )); DEBUG(","); DEBUG(pulseToDeg( WRI, g_pulse[WRI] )); DEBUGln(",delay}");
    calcPosition();
-   DEBUG("Robot parked: <"); DEBUG(g_radius_mm); DEBUG(","); DEBUG(g_height_mm); DEBUG(","); DEBUG(g_angle_d[BAS]); DEBUG(","); DEBUG(g_hand_d); DEBUGln(",delay>");
+   DEBUG("Robot parked: <"); DEBUG(g_radius_mm); DEBUG(","); DEBUG(g_height_mm); DEBUG(","); DEBUG(pulseToDeg( BAS, g_pulse[BAS] )); DEBUG(","); DEBUG(g_hand_d); DEBUGln(",delay>");
    DEBUGln("Enter r to return robot to park position\n");
    //slide(LEFT,MAX_STEPS);
     

// Write to servo using pulse length
int servoWritePulse( int id, int pulseLength)
{
  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);
  
  DEBUG("pwm.setPWM("); DEBUG(id); DEBUG(", 0, "); DEBUG(pulseLength); DEBUGln(")");
  
  return 0;  

   DEBUG("slowMoveArm("); DEBUG(basAngle_d); DEBUG(","); DEBUG(shlAngle_d); DEBUG(","); DEBUG(elbAngle_d); DEBUG(","); DEBUG(wriAngle_d); DEBUGln(")");
   
   int ret_val;
   
   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 );
   DEBUG("Steps: "); DEBUGln(steps);
   
   if( steps < 2 )
   {
      ret_val = moveArm( basPulse, shlPulse, elbPulse, wriPulse );
   }
   else
   {
      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_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( round(bMove), round(sMove), round(eMove), round(wMove) ) ) != 0 ) break;
         bMove += bStep; sMove += sStep; eMove += eStep; wMove += wStep;
      }
   }
