 // want these to be as small/large as possible without hitting the hard stop
 // for max range. You'll have to tweak them as necessary to match the servos you
 // have!
-#define BAS_MIN_D  0   // this is the 'minimum' angle in base servo range, 0 = -y direction
+#define BAS_MIN_D  90   // this is the 'minimum' angle in base servo range, 0 = -y direction
 #define BAS_MAX_D  180 // this is the 'maximum' angle in base servo range, 180 = +y direction
-#define SHL_MIN_D  0   // 0 = horizontal in +x direction
-#define SHL_MAX_D  180 // 180 = horizontal in -x direction
-#define ELB_MIN_D  0   // 0 = elbow closed
+#define SHL_MIN_D  16   // 0 = horizontal in +x direction
+#define SHL_MAX_D  165.762 // 180 = horizontal in -x direction
+#define ELB_MIN_D  11.5385   // 0 = elbow closed
 #define ELB_MAX_D  180 // 180 = elbow open, ulna parallel to humerus
-#define WRI_MIN_D  0   // 0 = wrist closed
+#define WRI_MIN_D  43.125 // 0 = wrist closed
 #define WRI_MAX_D  180 // 180 = wrist open, hand parallel to ulna
-#define GRI_MIN_D  100 // 100 = gripper fully open
+#define GRI_MIN_D  90 // 100 = gripper fully open
 #define GRI_MAX_D  180 // 180 = gripper fully closed
 
-#define BAS_MIN    146 // this is the pulse length count (out of 4096) that corresponds to BAS_MIN_D
-#define BAS_MAX    590 // this is the pulse length count (out of 4096) that corresponds to BAS_MAX_D
-#define SHL_MIN    186 // this is the pulse length count (out of 4096) that corresponds to SHL_MIN_D
-#define SHL_MAX    612 // this is the pulse length count (out of 4096) that corresponds to SHL_MAX_D //HS805BB 186-612
-#define ELB_MIN    590 // this is the pulse length count (out of 4096) that corresponds to ELB_MIN_D
-#define ELB_MAX    146 // this is the pulse length count (out of 4096) that corresponds to ELB_MAX_D
-#define WRI_MIN    146 // this is the pulse length count (out of 4096) that corresponds to WRI_MIN_D
-#define WRI_MAX    590 // this is the pulse length count (out of 4096) that corresponds to WRI_MAX_D
-#define GRI_MIN    382 // this is the pulse length count (out of 4096) that corresponds to GRI_MIN_D
+#define BAS_MIN    383 // this is the pulse length count (out of 4096) that corresponds to BAS_MIN_D
+#define BAS_MAX    160 // this is the pulse length count (out of 4096) that corresponds to BAS_MAX_D
+#define SHL_MIN    200 // this is the pulse length count (out of 4096) that corresponds to SHL_MIN_D
+#define SHL_MAX    540 // this is the pulse length count (out of 4096) that corresponds to SHL_MAX_D //HS805BB 186-612
+#define ELB_MIN    540 // this is the pulse length count (out of 4096) that corresponds to ELB_MIN_D
+#define ELB_MAX    175 // this is the pulse length count (out of 4096) that corresponds to ELB_MAX_D
+#define WRI_MIN    500 // this is the pulse length count (out of 4096) that corresponds to WRI_MIN_D
+#define WRI_MAX    135 // this is the pulse length count (out of 4096) that corresponds to WRI_MAX_D
+#define GRI_MIN    320 // this is the pulse length count (out of 4096) that corresponds to GRI_MIN_D
 #define GRI_MAX    590 // this is the pulse length count (out of 4096) that corresponds to GRI_MAX_D
+//WRI 375 = 90
+//ELB 370 = 90
+//SHL 368 = 90  
+//BAS 383 = 90
 
 #define BAS_PARK 90
 #define SHL_PARK 90

// Write angle to servo using microseconds for added precision
int servoWrite( int id, double angle_d )
{
//   DEBUG("servoWrite("); DEBUG(id); DEBUG(", "); DEBUG(angle_deg); DEBUGln(")");
   if (angle_d < SERVO_MIN_D[id]) { DEBUG(angle_d); DEBUG("<"); DEBUGln(SERVO_MIN_D[id]); return -1; }
   if (angle_d > SERVO_MAX_D[id]) { DEBUG(angle_d); DEBUG(">"); DEBUGln(SERVO_MAX_D[id]); return -2; }
   
   double pulseLength = map(angle_d, 0, 180, SERVO_MIN[id], SERVO_MAX[id]);
   
   pwm.setPWM(id, 0, pulseLength);
   
   g_angle_d[id] = angle_d;
   
   return 0;
}

// Calculates the cylindrical coordinates of the wrist from the servo angles

            buf[i] = '\0';
            number[0] = atof(buf);
            g_moveDelay = 50;
            Serial.println(servoWrite(BAS, number[0]));
            //Serial.println(slowMoveArm(number[0],g_angle_d[SHL],g_angle_d[ELB],g_angle_d[WRI]));
            break;
            
         case '<': // start of coordinate move packet
