 const float R_REF[] = {BAS_R_REF, SHL_R_REF, ELB_R_REF, WRI_R_REF};
 const float H_REF[] = {BAS_H_REF, SHL_H_REF, ELB_H_REF, WRI_H_REF};
 
+const float SERVO_MIN_D[] = {BAS_MIN_D, SHL_MIN_D, ELB_MIN_D, WRI_MIN_D, GRI_MIN_D};
+const float SERVO_MAX_D[] = {BAS_MAX_D, SHL_MAX_D, ELB_MAX_D, WRI_MAX_D, GRI_MAX_D};
+const float SERVO_MIN[] = {BAS_MIN, SHL_MIN, ELB_MIN, WRI_MIN, GRI_MIN};
+const float SERVO_MAX[] = {BAS_MAX, SHL_MAX, ELB_MAX, WRI_MAX, GRI_MAX};
+
 /********************/
 /* PRE CALCULATIONS */
 /********************/
-float hum_sq = HUMERUS*HUMERUS;
-float uln_sq = ULNA*ULNA;
+float HUM_SQ = HUMERUS*HUMERUS;
+float ULN_SQ = ULNA*ULNA;
 
 /*****************/
 /* RETURN VALUES */

}

// Arm positioning routine utilizing inverse kinematics in cylindrical coordinates
// (r = radius, h = height, b = azimuth = base servo angle
int moveArmCoord( float radius_mm, float height_mm, float bas_angle_d )
{
   DEBUG("moveArmCoord("); DEBUG(radius_mm); DEBUG(","); DEBUG(height_mm); DEBUG(","); DEBUG(bas_angle_d); DEBUGln(")");
   /* Wrist position */
   float wri_h = height_mm - BASE_HEIGHT;
   float wri_r = radius_mm;
   /* Shoulder to wrist distance ( AKA sw ) */
   float s_w = ( wri_h * wri_h ) + ( wri_r * wri_r ); 
   float s_w_sqrt = sqrt( s_w ); // Length of line from shoulder to wrist (S-W)
   /* s_w angle to ground */
   float a1 = atan2( wri_h, wri_r ); // Angle between S-W line and ground
   /* s_w angle to humerus */
   float a2 = acos((( hum_sq - uln_sq ) + s_w ) / ( 2 * HUMERUS * s_w_sqrt )); // Angle between S-W line and humerus
   /* shoulder angle */
   float shl_angle_r = a1 + a2;
   float shl_angle_d = degrees( shl_angle_r );
   /* elbow angle */
   float elb_angle_r = acos(( hum_sq + uln_sq - s_w ) / ( 2 * HUMERUS * ULNA )); // Angle between humerus and ulna
   float elb_angle_d = degrees( elb_angle_r ); // Shifted so that neutral position is 180 degrees
   
   return ( s_w_sqrt > HUMERUS + ULNA ) ? -3 : slowMoveArm( bas_angle_d, shl_angle_d, elb_angle_d, -1 ); // Return -3 if no solution exists (out of range)
}

// Moves the arm slowly to a new position, depending on the specified delay using cylindrical coordinates
// (r = radius, h = height, b = azimuth = base servo angle)
int slowMoveArmCoord( float radius_mm, float height_mm, float bas_angle_d )
{
   DEBUG("slowMoveArmCoord("); DEBUG(radius_mm); DEBUG(","); DEBUG(height_mm); DEBUG(","); DEBUG(bas_angle_d); DEBUGln(")");
   
   calcPosition();
   
   old_r = current_r; old_h = current_h; old_b = current[BAS]; // Use with feedback to allow reversing after a collision
   
   int ret_val;
   float diff_r = abs( radius_mm - current_r );
   float diff_h = abs( height_mm - current_h );
   float diff_b = abs( bas_angle_d - current[BAS] );
   
   int steps = max( diff_r, diff_h );
   steps = max( steps, diff_b );
   DEBUG("Steps: "); DEBUGln(steps);
   
   float r_step = ( radius_mm - current_r )/steps;
   float h_step = ( height_mm - current_h )/steps;
   float b_step = ( bas_angle_d - current[BAS] )/steps;
   
   float r_move = current_r + r_step;
   float h_move = current_h + h_step;
   float b_move = current[BAS] + b_step;
   
   for(int i=0; i<steps; i++)
   {
     if ( ( ret_val = moveArmCoord( r_move, h_move, b_move ) )  != 0 ) return ret_val;
     r_move += r_step; h_move += h_step; b_move += b_step;
   }
   
   delay(100); // Wait for arm to stop moving
   
   levelGripper();
   
   delay(100); // Wait for arm to stop moving
   
   if ( checkPosition() != 0 ) return -4; // Stall detection

   return 0;
}

// Adjust the servo angle to compensate for servo error
float adjust( int id, float angle_d )
{
   return gradient[id]*angle_d + offset[id];
}

// Sends signal to stepper motor to move slider in a given direction

      delay(1);
      digitalWrite(STEP_PIN, LOW);
      delay(1);
      dir ? g_sliderPos++ : g_sliderPos--;
   }
   DEBUGln();
}
