 
 // if you need PWM, just use the PWM outputs on the Arduino
 // and instead of digitalWrite, you should use the analogWrite command
+
+#include <Servo.h>
+
+Servo servo;
+int pos = 0;
+
+// Servo degree increment
+#define SERVO_INC 3
 
 // Bit masks for commands
 #define FULL_STOP 0xc0
 #define AXIS_Y 0x20
 #define DIRECTION_POS 0x10
 #define SPEED_MASK 0x0f
+#define SERVO_CTRL 0x80
+#define SERVO_MASK 0x3f
 
 // Motor pins
 int motor_y[] = {3, 5};

    Serial.readBytes(&command, 1);
    Serial.println(command, BIN);

    // Determine whether to full stop motors or not
    if (command & FULL_STOP) {
      Serial.println("Full stop");
      digitalWrite(motor_x[0], LOW);
      digitalWrite(motor_x[1], LOW);
      digitalWrite(motor_y[0], LOW);
      digitalWrite(motor_y[1], LOW);
    } else {
      uint8_t axis = 0;
      if (command & AXIS_Y) {


void setup() {
  Serial.begin(9600);
  
  // Use pin 7 for the servo
  servo.attach(7);

  // Setup motors
  int i;
