  * Handle Servo commands
  * attach, detach, write, read, writeMicroseconds, attached
  */
-void handleServo(char *pin, char *val, char *aux) {
-  int p = getPin(pin);
-  if (p == -1) return;
-  
+void handleServo(int p, char *val, char *aux) {
   // 00(0) Detach
   if (atoi(val) == 0) {
     servo.detach();

  }

  // if (debug) Serial.println(request);
  int cmdid = atoi(cmd);

  switch(cmdid) {
    case 0:  setMode(pin,val);         break;
    case 1:  dWrite(pin,val);          break;
    case 2:  dRead(pin);               break;
    case 3:  aWrite(pin,val);          break;
    case 4:  aRead(pin);               break;
    case 97: handlePing(pin,val,aux);  break;
    case 98: handleServo(pin,val,aux); break;
    case 99: toggleDebug(val);         break;
    default:                           break;
  }
}


/*
 * Set pin mode
 */
void setMode(int p, char *val) {
  if (atoi(val) == 0) {
    pinMode(p, OUTPUT);
  } else {
