 /* FUNCTIONS */
 /*************/
 // Moves arm to specified servo angles
-int moveArm( float basAngle_d, float shlAngle_d, float elbAngle_d, float wriAngle_d )
+int moveArm( double basAngle_d, double shlAngle_d, double elbAngle_d, double wriAngle_d )
 {  
    int ret_val;
    

#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

// Use accelerometer feedback to ensure the arm has reached the destination
int checkPosition()
{
   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;
