 }
 
 // Write to servo using pulse length
-int servoWritePulse( int id, double pulseLength)
-{
-  if (pulseLength < min(SERVO_MIN[id], SERVO_MAX[id])) { DEBUGln(min(SERVO_MIN[id], SERVO_MAX[id])); return -1; }
-  if (pulseLength > max(SERVO_MIN[id], SERVO_MAX[id])) { DEBUGln(max(SERVO_MIN[id], SERVO_MAX[id])); return -2; }
+int servoWritePulse( int id, int pulseLength)
+{
+  if ( pulseLength < SERVO_MIN[id] ) { DEBUG("Pulse too small ("); DEBUG(pulseLength); DEBUG("<"); DEBUG(SERVO_MIN[id]); DEBUGln(")"); return -1; }
+  if ( pulseLength > SERVO_MAX[id] ) { DEBUG("Pulse too large ("); DEBUG(pulseLength); DEBUG(">"); DEBUG(SERVO_MAX[id]); DEBUGln(")"); return -2; }
   
   pwm.setPWM(id, 0, pulseLength);
   

// 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  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  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  43.125 // 0 = wrist closed
#define WRI_MAX_D  180 // 180 = wrist open, hand parallel to ulna
#define GRI_MIN_D  90 // 100 = gripper fully open
#define GRI_MAX_D  180 // 180 = gripper fully closed

#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

}

// Moves the arm slowly to a new position, depending on the specified delay
int slowMoveArm( double basAngle_d, double shlAngle_d, double elbAngle_d, double wriAngle_d )
{ 
   DEBUG("slowMoveArm("); DEBUG(basAngle_d); DEBUG(","); DEBUG(shlAngle_d); DEBUG(","); DEBUG(elbAngle_d); DEBUG(","); DEBUG(wriAngle_d); DEBUGln(")");
