    /* elbow angle */
    double elbAngle_r = acos(( HUM_SQ + ULN_SQ - s2w ) / ( 2 * HUMERUS * ULNA )); // Angle between humerus and ulna
    /* wrist angle */
-   double wriAngle_d = 360 - degrees(shlAngle_r + elbAngle_r);
+   double wriAngle_d = 360 + handAngle_d - degrees(shlAngle_r + elbAngle_r);
    
    return ( s2w_sqrt > HUMERUS + ULNA ) ? -3 : slowMoveArm( basAngle_d, degrees(shlAngle_r), degrees(elbAngle_r), wriAngle_d ); // Return -3 if no solution exists (out of range)
 }

         case 'p':
            servoWrite(GRI,GRI_MICROPLATE); Serial.println("Gripped Microplate"); break;
         case 'r':
            g_moveDelay = 20;
            if( ( ret_val = slowMoveArm(BAS_PARK,SHL_PARK,ELB_PARK,WRI_PARK) ) == 0 ) Serial.println("Arm returned to park position");
            else Serial.println(ret_val);
            break;

/*********************************/
/* 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
