 int moveArmCoord( double radius_mm, double height_mm, double basAngle_d, double handAngle_d )
 {
    if ( radius_mm < 0 ) return -4;
-   DEBUG("moveArmCoord("); DEBUG(radius_mm); DEBUG(","); DEBUG(height_mm); DEBUG(","); DEBUG(basAngle_d); DEBUGln(")");
+   DEBUG("moveArmCoord("); DEBUG(radius_mm); DEBUG(","); DEBUG(height_mm); DEBUG(","); DEBUG(basAngle_d); DEBUG(","); DEBUG(handAngle_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) );

/*********************************/
/* General system debugging code */
/*********************************/
#define DEBUG // Uncomment line to add debugging code

#ifdef DEBUG
#  define DEBUG(x) (Serial.print(x))
#  define DEBUGln(x) (Serial.println(x))
#else

{   
   double s = pulseToDeg( SHL, g_pulse[SHL] ), e = pulseToDeg( ELB, g_pulse[ELB] ) - 180, w = pulseToDeg( WRI, g_pulse[WRI] ) - 180;
   
   g_hand_d = s + e + w;
   
   g_radius_mm = HUMERUS*cos( radians(s) ) + ULNA*cos( radians(s + e) ) + HAND*cos( radians(g_hand_d) );
   
   g_height_mm = BASE_HEIGHT + HUMERUS*sin( radians(s) ) + ULNA*sin( radians(s + e) ) + HAND*sin( radians(g_hand_d) );
   
   DEBUG("g_radius_mm: "); DEBUG(g_radius_mm); DEBUG(", g_height_mm: "); DEBUGln(g_height_mm);
}
