 
 // Moves the arm slowly to a new position, depending on the specified delay
 // If the last input angle is negative, the function will determine the wrist angle required to achieve horizontal position
-int slowMoveArm( float basAngle_d, float shlAngle_d, float elbAngle_d, float wriAngle_d )
+int slowMoveArm( double basAngle_d, double shlAngle_d, double elbAngle_d, double wriAngle_d )
 { 
    DEBUG("slowMoveArm("); DEBUG(basAngle_d); DEBUG(","); DEBUG(shlAngle_d); DEBUG(","); DEBUG(elbAngle_d); DEBUG(","); DEBUG(wriAngle_d); DEBUGln(")");
    
    int ret_val;
-   float bDiff = abs( basAngle_d - g_angle_d[BAS] );
-   float sDiff = abs( shlAngle_d - g_angle_d[SHL] );
-   float eDiff = abs( elbAngle_d - g_angle_d[ELB] );
-   float wDiff = abs( wriAngle_d - g_angle_d[WRI] );
-   
-   float steps = max( bDiff, sDiff );
+   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);

#define RIGHT          HIGH

// Servo IDs used for addressing servos in arrays
#define BAS 0
#define SHL 1
#define ELB 2
#define WRI 3
#define GRI 4

// Depending on your servo make, the pulse width min and max may vary, you 
// want these to be as small/large as possible without hitting the hard stop


// Arm positioning routine utilizing inverse kinematics in cylindrical coordinates
// (r = radius, h = height, b = azimuth = base servo angle, handAngle = angle of hand from horizontal)
int moveArmCoord( double radius_mm, double height_mm, double basAngle_d, double handAngle_d )
{
   DEBUG("moveArmCoord("); DEBUG(radius_mm); DEBUG(","); DEBUG(height_mm); DEBUG(","); DEBUG(basAngle_d); DEBUGln(")");
   /* Lengths from shoulder to wrist in cylindrical coordinates */
   double s2w_h = height_mm - BASE_HEIGHT - HAND*sin( radians( handAngle_d) );
   double s2w_r = radius_mm - HAND*cos( radians( handAngle_d) );
   /* Shoulder to wrist distance ( AKA s2w ) */
   double s2w = ( s2w_h * s2w_h ) + ( s2w_r * s2w_r ); 
   double s2w_sqrt = sqrt( s2w ); // Length of line from shoulder to wrist (S-W)
   /* s2w angle to ground */
   double a1 = atan2( s2w_h, s2w_r ); // Angle between S-W line and ground
   /* s2w angle to humerus */
   double a2 = acos((( HUM_SQ - ULN_SQ ) + s2w ) / ( 2 * HUMERUS * s2w_sqrt )); // Angle between S-W line and humerus
   /* shoulder angle */
   double shlAngle_r = a1 + a2;
   /* elbow angle */
   double elbAngle_r = acos(( HUM_SQ + ULN_SQ - s2w ) / ( 2 * HUMERUS * ULNA )); // Angle between humerus and ulna
   /* wrist angle */
   double wriAngle_d = 360 - degrees(shlAngle_r + elbAngle_r);
   
   return ( s2w_sqrt > HUMERUS + ULNA ) ? -3 : slowMoveArm( basAngle_d, degrees(shlAngle_r), degrees(elbAngle_r), wriAngle_d ); // Return -3 if no solution exists (out of range)
}

// Moves the arm slowly to a new position, depending on the specified delay using cylindrical coordinates
// (r = radius, h = height, b = azimuth = base servo angle)
int slowMoveArmCoord( double radius_mm, double height_mm, double basAngle_d, double handAngle_d )
{
   DEBUG("slowMoveArmCoord("); DEBUG(radius_mm); DEBUG(","); DEBUG(height_mm); DEBUG(","); DEBUG(basAngle_d); DEBUG(","); DEBUG(handAngle_d); DEBUGln(")");
   
