 /*********************************************************************************************************/
-/* File: AL5D.ino                                                                                        */
+/* File: al5d.ino                                                                                        */
 /* Author: Darren McMorran                                                                               */
 /* Date Started: 28/05/2015                                                                              */
 /*                                                                                                       */

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