   Serial.println(m);
 }
 
+/*
+ * Handle Servo commands
+ * attach, detach, write, read, writeMicroseconds, attached
+ */
+void handleServo(char *pin, char *val, char *aux) {
+  if (debug) Serial.println("ss");
+  int p = atoi(pin);
+  Serial.println("got signal");
+  if (strcmp(val, "00") == 0) {
+    servo.detach();
+  } else if (strcmp(val, "01") == 0) {
+    servo.attach(p);
+    Serial.println("attached");
+  } else if (strcmp(val, "02") == 0) {
+    Serial.println("writing to servo");
+    Serial.println(atoi(aux));
+    servo.write(atoi(aux));
+  }
+}

  if (strcmp(cmd, "00") == 0) {
      sm(pin, val);
  } else if (strcmp(cmd, "01") == 0) {
      dw(pin, val);
  } else if (strcmp(cmd, "02") == 0) {
      dr(pin, val);
  } else if (strcmp(cmd, "99") == 0) {
      toggleDebug(val);
  }

  pin[2] = '\0';
  strncpy(val, messageBuffer + 4, 2);
  val[2] = '\0';
  strncpy(aux, messageBuffer + 6, 3);
  aux[2] = '\0';
  
  if (debug) {
    Serial.println(messageBuffer);
