 }
 
 // Read from accelerometers and determine angle from horizontal
-float getAngle(int id)
-{
-   float r = getFeedback(R_PIN[id]) - R_REF[id];
-   float h = getFeedback(H_PIN[id]) - H_REF[id];
-   float angle_d = degrees( id == ELB ? atan2(-h,-r) : atan2(-h,r) );
+double getAngle(int id)
+{
+   double r = getFeedback(R_PIN[id]) - R_REF[id];
+   double h = getFeedback(H_PIN[id]) - H_REF[id];
+   double angle_d = degrees( id == ELB ? atan2(-h,-r) : atan2(-h,r) );
    DEBUG("r: "); DEBUG(r); DEBUG(", h: "); DEBUG(h); DEBUG(", angle_d: "); DEBUGln(angle_d);
    return angle_d;
 }
 
 // Write angle to servo using microseconds for added precision
-int servoWrite( int id, float angle_d )
+int servoWrite( int id, double angle_d )
 {
 //   DEBUG("servoWrite("); DEBUG(id); DEBUG(", "); DEBUG(angle_deg); DEBUGln(")");
    if (angle_d < SERVO_MIN_D[id]) { DEBUG(angle_d); DEBUG("<"); DEBUGln(SERVO_MIN_D[id]); return -1; }
    if (angle_d > SERVO_MAX_D[id]) { DEBUG(angle_d); DEBUG(">"); DEBUGln(SERVO_MAX_D[id]); return -2; }
    
-   float pulseLength = map(angle_d, 0, 180, SERVO_MIN[id], SERVO_MAX[id]);
+   double pulseLength = map(angle_d, 0, 180, SERVO_MIN[id], SERVO_MAX[id]);
    
    pwm.setPWM(id, 0, pulseLength);
    

int     i,j,count=0;
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];
float   number[4], current[4], actual[4], g_angle_d[4];
float   g_radius_mm, g_height_mm, g_hand_d, g_sliderPos;
float   g_prevRadius_mm, g_prevHeight_mm, g_prevBase_d, g_prevHand_d;

const float gradient[] = {BAS_GRADIENT, SHL_GRADIENT, ELB_GRADIENT, WRI_GRADIENT, GRI_GRADIENT};
const float offset[] = {BAS_OFFSET, SHL_OFFSET, ELB_OFFSET, WRI_OFFSET, GRI_OFFSET};
const float R_PIN[] = {BAS_R_PIN, SHL_R_PIN, ELB_R_PIN, WRI_R_PIN};
const float H_PIN[] = {BAS_H_PIN, SHL_H_PIN, ELB_H_PIN, WRI_H_PIN};
const float R_REF[] = {BAS_R_REF, SHL_R_REF, ELB_R_REF, WRI_R_REF};
const float H_REF[] = {BAS_H_REF, SHL_H_REF, ELB_H_REF, WRI_H_REF};

const float SERVO_MIN_D[] = {BAS_MIN_D, SHL_MIN_D, ELB_MIN_D, WRI_MIN_D, GRI_MIN_D};
const float SERVO_MAX_D[] = {BAS_MAX_D, SHL_MAX_D, ELB_MAX_D, WRI_MAX_D, GRI_MAX_D};
const float SERVO_MIN[] = {BAS_MIN, SHL_MIN, ELB_MIN, WRI_MIN, GRI_MIN};
const float SERVO_MAX[] = {BAS_MAX, SHL_MAX, ELB_MAX, WRI_MAX, GRI_MAX};

/********************/
/* PRE CALCULATIONS */
/********************/
float HUM_SQ = HUMERUS*HUMERUS;
float ULN_SQ = ULNA*ULNA;

/*****************/
/* RETURN VALUES */


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