 /* FUNCTIONS */
 /*************/
 // Moves arm to specified servo angles
-int moveArm( double basAngle_d, double shlAngle_d, double elbAngle_d, double wriAngle_d )
+int moveArm( int basPulse, int shlPulse, int elbPulse, int wriPulse )
 {  
    int ret_val;
    
-   DEBUG("moveArm("); DEBUG(basAngle_d); DEBUG(","); DEBUG(shlAngle_d); DEBUG(","); DEBUG(elbAngle_d); DEBUG(","); DEBUG(wriAngle_d); DEBUGln(")");
+   DEBUG("moveArm("); DEBUG(basPulse); DEBUG(","); DEBUG(shlPulse); DEBUG(","); DEBUG(elbPulse); DEBUG(","); DEBUG(wriPulse); DEBUGln(")");
    
    //Drive servos
-   if ( ( ret_val = servoWrite( BAS, basAngle_d ) ) != 0 ) return ret_val;
-   if ( ( ret_val = servoWrite( SHL, shlAngle_d ) ) != 0 ) return ret_val;
-   if ( ( ret_val = servoWrite( ELB, elbAngle_d ) ) != 0 ) return ret_val;
-   if ( ( ret_val = servoWrite( WRI, wriAngle_d ) ) != 0 ) return ret_val;
+   if ( ( ret_val = servoWritePulse( BAS, basPulse ) ) != 0 ) return ret_val;
+   if ( ( ret_val = servoWritePulse( SHL, shlPulse ) ) != 0 ) return ret_val;
+   if ( ( ret_val = servoWritePulse( ELB, elbPulse ) ) != 0 ) return ret_val;
+   if ( ( ret_val = servoWritePulse( WRI, wriPulse ) ) != 0 ) return ret_val;
    
    delay(g_moveDelay);
    

   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 );
   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 );
   }
   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 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;
      
      for(int i=0; i<steps; i++)
      {
         if( ( ret_val = moveArm( bMove, sMove, eMove, wMove ) ) != 0 ) break;
         bMove += bStep; sMove += sStep; eMove += eStep; wMove += wStep;
      }
   }

// 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;  
