 }
 
 // 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);
   

}

// Moves the arm slowly to a new position, depending on the specified delay
// If the last input angle is negative, the function will determine the wrist angle required to achieve horizontal position
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(")");

double   g_radius_mm, g_height_mm, g_hand_d, g_sliderPos;
double   g_prevRadius_mm, g_prevHeight_mm, g_prevBase_d, g_prevHand_d;

const double GRADIENT[] = {BAS_GRADIENT, SHL_GRADIENT, ELB_GRADIENT, WRI_GRADIENT};
const double OFFSET[] = {BAS_OFFSET, SHL_OFFSET, ELB_OFFSET, WRI_OFFSET};
const double R_PIN[] = {BAS_R_PIN, SHL_R_PIN, ELB_R_PIN, WRI_R_PIN};
const double H_PIN[] = {BAS_H_PIN, SHL_H_PIN, ELB_H_PIN, WRI_H_PIN};
const double R_REF[] = {BAS_R_REF, SHL_R_REF, ELB_R_REF, WRI_R_REF};
const double H_REF[] = {BAS_H_REF, SHL_H_REF, ELB_H_REF, WRI_H_REF};

const double SERVO_MIN[] = {BAS_MIN, SHL_MIN, ELB_MIN, WRI_MIN, GRI_MIN};
const double SERVO_MAX[] = {BAS_MAX, SHL_MAX, ELB_MAX, WRI_MAX, GRI_MAX};

