 // Use accelerometer feedback to ensure the arm has reached the destination
 int checkPosition()
 {
-   float angle_d = getAngle(SHL);
+   double angle_d = getAngle(SHL);
    if ( abs( angle_d - g_angle_d[SHL] ) > THRESHOLD3 ) return 1;
    DEBUG("Check1: "); DEBUGln(angle_d - g_angle_d[SHL]);
    angle_d = getAngle(ELB) - angle_d;


// 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 )
{ 
   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 );
   steps = max( steps, eDiff );
   steps = max( steps, wDiff );
   steps = round(steps);

    for (int k=6; k<14; k++){      //discard the 6 highest and 6 lowest readings
      mean += reading[k];
    }
    return round((double)mean/8);   //average useful readings
}

// Changes the wrist angle until the gripper is level
double levelGripper()
{
   int ret_val, stall_count = 0;
   double h, h_old, servo_angle = actual[WRI], servo_angle_old = servo_angle;
   h = getFeedback(H_PIN[WRI]) - H_REF[WRI];
   h_old = h;
   DEBUG("servo_angle: "); DEBUG(servo_angle); DEBUG(", h: "); DEBUGln(h);
