 int checkPosition()
 {
    float angle_d = getAngle(SHL);
-   if ( abs( angle_d - current[SHL] ) > THRESHOLD3 ) return 1;
-   DEBUG("Check1: "); DEBUGln(angle_d - current[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;
-   DEBUG("Check2: "); DEBUGln(angle_d - current[ELB] + 180);
-   return ( abs( angle_d - current[ELB] + 180 ) > THRESHOLD3 );
+   DEBUG("Check2: "); DEBUGln(angle_d - g_angle_d[ELB] + 180);
+   return ( abs( angle_d - g_angle_d[ELB] + 180 ) > THRESHOLD3 );
 }
 
 // Read from accelerometers and determine angle from horizontal

               buf[i] = Serial.read(); delay(1);
            }
            buf[i] = '\0';
            move_delay = atof( buf ); // move_delay
            ret_val = slowMoveArmCoord( number[0], number[1], number[2] );
            if( ret_val == 0 )
            {
               Serial.print(number[0]); Serial.print(","); Serial.print(number[1]); Serial.print(","); Serial.println(number[2]);
            }
            else
            {
               Serial.println(ret_val);
               if ( ret_val == -4 ) // If the arm did not reach the destination, return to previous position
               {
                  slowMoveArmCoord( old_r, old_h, old_b );
               }
            }
            break;
            
         case '{': // start of servo angle move packet
            for(j=0; j<4; j++) // capture first 4 arguments: bas_angle_d, shl_angle_d, elb_angle_d & wri_angle_d
            {
               i=0;
               buf[i] = Serial.read(); delay(1);

/************/
/* INCLUDES */
/************/
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

/*********************************/
/* General system debugging code */
