+#include <Servo.h>
 
-char messageBuffer[6];
+char messageBuffer[11];
 int index = 0;
 char cmd[3];
 char pin[3];
 char val[3];
+char aux[4];
 bool debug = false;
+Servo servo;
 
 void setup() {
   Serial.begin(115200);

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

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