+/**
+
+COMMAND SYNTAX:
+===============
+"[COMMAND];[VALUE]"
+
+COMMANDS AND VALUES:
+====================
+-1 NONE
+0 TURN HEAD (600 - 2400)
+1 SONAR PING (no value)
+2 SET LEFT MOTOR SPEED (-255 - 255) 
+3 SET RIGHT MOTOR SPEED (-255 - 255)
+4 SET SPEED FOR BOTH MOTORS (-255 - 255)
+
+*/
+
+
+
 #include <Servo.h> 
 #include <NewPing.h>
+#include <AFMotor.h>
 
 #define BAUD_RATE 115200
 #define SERVO_HEAD 10

#define SERVO_WAIT_TIME_MILLIS 30
#define SONAR_WAIT_TIME_MILLIS 30

#define SERVO_MIN_MS 600
#define SERVO_MAX_MS 2400

#define COMMAND_BUFFER_LENGTH 12

Servo servo;

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);


    case TURN_HEAD: {
      turnHead(value);  
      break;
    }
    
    case SET_MOTOR_LEFT: {
      setMotorSpeed(1, value);  
      break;
    } 
    
    case SET_MOTOR_RIGHT: {
      setMotorSpeed(2, value);  
      break;
    } 
    
    case SET_MOTORS: {
      setMotorSpeed(0, value);  
      break;
    } 

    default: {
      setMotorSpeed(0,0);
      turnHead(SERVO_CENTER);
    }
  } 
  
  return readSomething;
}

void setMotorSpeed(int motor, int value) {
  motor = constrain(motor, 0, 2);
  value = constrain(value, -255, 255);
  int absValue = abs(value);
  
  uint8_t direction = RELEASE;
  if( value > 0 ) {
    direction = FORWARD;
  }
  else if( value < 0 ) {
    direction = BACKWARD;  
  }
  
  if( motor == 0 ) {
    motorLeft.setSpeed(absValue);
    motorRight.setSpeed(absValue);  
    motorLeft.run(direction);
    motorRight.run(direction);    
  }
  else if( motor == 1 ) {
    motorLeft.setSpeed(absValue);
    motorLeft.run(direction);    
  }
  else {
    motorRight.setSpeed(absValue);
    motorRight.run(direction);  
  }
} 

void sonarPing() {
  unsigned int dist = getDistance();
