diff --git a/README.md b/README.md
index 3a4c4dc..1c2dabd 100644
--- a/README.md
+++ b/README.md
@@ -22,7 +22,7 @@ Overview
 --------
 This branch (indigo-devel) is intended for ROS Indigo and above, and uses the Catkin buildsystem. It may also be compatible with ROS Hydro.
 
-This ROS stack includes an Arduino library (called ROSArduinoBridge) and a collection of ROS packages for controlling an Arduino-based robot using standard ROS messages and services.  The stack does **not** depend on ROS Serial.
+This ROS metapackage includes an Arduino library (called ROSArduinoBridge) and a collection of ROS packages for controlling an Arduino-based robot using standard ROS messages and services.  The stack does **not** depend on ROS Serial.
 
 Features of the stack include:
 
@@ -43,7 +43,9 @@ the PC. The base controller requires the use of a motor controller and encoders
 * Pololu VNH5019 dual motor controller shield (http://www.pololu.com/catalog/product/2502) or Pololu MC33926 dual motor shield (http://www.pololu.com/catalog/product/2503).
 
 * Robogaia Mega Encoder shield
-(http://www.robogaia.com/two-axis-encoder-counter-mega-shield-version-2.html) or on-board wheel encoder counters.
+(http://www.robogaia.com/two-axis-encoder-counter-mega-shield-version-2.html)
+
+* Instead of the Encoder shield, wheel encoders can be [connected directly](#using-the-on-board-wheel-encoder-counters-arduino-uno-only) if using an Arduino Uno
 
 **NOTE:** The Robogaia Mega Encoder shield can only be used with an Arduino Mega. The on-board wheel encoder counters are currently only supported by Arduino Uno.
 
@@ -91,7 +93,7 @@ http://www.robogaia.com/uploads/6/8/0/9/6809982/__megaencodercounter-1.3.tar.gz
 These libraries should be installed in your standard Arduino
 sketchbook/libraries directory.
 
-Finally, it is assumed you are using version 1.0 or greater of the
+Finally, it is assumed you are using version 1.6.6 or greater of the
 Arduino IDE.
 
 Preparing your Serial Port under Linux
diff --git a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/MegaRobogaiaPololu.ino b/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/MegaRobogaiaPololu.ino
deleted file mode 100644
index 0f66429..0000000
--- a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/MegaRobogaiaPololu.ino
+++ /dev/null
@@ -1,341 +0,0 @@
-/*********************************************************************
- *  ROSArduinoBridge
- 
-    A set of simple serial commands to control a differential drive
-    robot and receive back sensor and odometry data. Default 
-    configuration assumes use of an Arduino Mega + Pololu motor
-    controller shield + Robogaia Mega Encoder shield.  Edit the
-    readEncoder() and setMotorSpeed() wrapper functions if using 
-    different motor controller or encoder method.
-
-    Created for the Pi Robot Project: http://www.pirobot.org
-
-    Inspired and modeled after the ArbotiX driver by Michael Ferguson
-    
-    Software License Agreement (BSD License)
-
-    Copyright (c) 2012, Patrick Goebel.
-    All rights reserved.
-
-    Redistribution and use in source and binary forms, with or without
-    modification, are permitted provided that the following conditions
-    are met:
-
-     * Redistributions of source code must retain the above copyright
-       notice, this list of conditions and the following disclaimer.
-     * Redistributions in binary form must reproduce the above
-       copyright notice, this list of conditions and the following
-       disclaimer in the documentation and/or other materials provided
-       with the distribution.
-
-    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-    COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-#define USE_BASE      // Enable the base controller code
-//#undef USE_BASE     // Disable the base controller code
-
-//#define USE_SERVOS  // Enable use of PWM servos as defined in servos.h
-#undef USE_SERVOS     // Disable use of PWM servos
-
-/* Serial port baud rate */
-#define BAUDRATE     57600
-
-/* Maximum PWM signal */
-#define MAX_PWM        255
-
-#if defined(ARDUINO) && ARDUINO >= 100
-#include "Arduino.h"
-#else
-#include "WProgram.h"
-#endif
-
-/* Include definition of serial commands */
-#include "commands.h"
-
-/* Sensor functions */
-#include "sensors.h"
-
-/* Include servo support if required */
-#ifdef USE_SERVOS
-   #include <Servo.h>
-   #include "servos.h"
-#endif
-
-#ifdef USE_BASE
-  /* The Pololu motor driver shield */
-  #include "DualVNH5019MotorShield.h"
-
-  /* The Robogaia Mega Encoder shield */
-  #include "MegaEncoderCounter.h"
-
-  /* Create the motor driver object */
-  DualVNH5019MotorShield drive;
-
-  /* Create the encoder shield object */
-  MegaEncoderCounter encoders(4); // Initializes the Mega Encoder Counter in the 4X Count mode
-
-  /* PID parameters and functions */
-  #include "diff_controller.h"
-
-  /* Run the PID loop at 30 times per second */
-  #define PID_RATE           30     // Hz
-
-  /* Convert the rate into an interval */
-  const int PID_INTERVAL = 1000 / PID_RATE;
-  
-  /* Track the next time we make a PID calculation */
-  unsigned long nextPID = PID_INTERVAL;
-
-  /* Stop the robot if it hasn't received a movement command
-   in this number of milliseconds */
-  #define AUTO_STOP_INTERVAL 2000
-  long lastMotorCommand = AUTO_STOP_INTERVAL;
-
-  /* Wrap the encoder reading function */
-  long readEncoder(int i) {
-    if (i == LEFT) return encoders.YAxisGetCount();
-    else return encoders.XAxisGetCount();
-  }
-
-  /* Wrap the encoder reset function */
-  void resetEncoder(int i) {
-    if (i == LEFT) return encoders.YAxisReset();
-    else return encoders.XAxisReset();
-  }
-
-  /* Wrap the encoder reset function */
-  void resetEncoders() {
-    resetEncoder(LEFT);
-    resetEncoder(RIGHT);
-  }
-  
-  /* Wrap the motor driver initialization */
-  void initMotorController() {
-    drive.init();
-  }
-
-  /* Wrap the drive motor set speed function */
-  void setMotorSpeed(int i, int spd) {
-    if (i == LEFT) drive.setM1Speed(spd);
-    else drive.setM2Speed(spd);
-  }
-
-  // A convenience function for setting both motor speeds
-  void setMotorSpeeds(int leftSpeed, int rightSpeed) {
-    setMotorSpeed(LEFT, leftSpeed);
-    setMotorSpeed(RIGHT, rightSpeed);
-  }
-#endif
-
-/* Variable initialization */
-
-// A pair of varibles to help parse serial commands (thanks Fergs)
-int arg = 0;
-int index = 0;
-
-// Variable to hold an input character
-char chr;
-
-// Variable to hold the current single-character command
-char cmd;
-
-// Character arrays to hold the first and second arguments
-char argv1[16];
-char argv2[16];
-
-// The arguments converted to integers
-long arg1;
-long arg2;
-
-/* Clear the current command parameters */
-void resetCommand() {
-  cmd = NULL;
-  memset(argv1, 0, sizeof(argv1));
-  memset(argv2, 0, sizeof(argv2));
-  arg1 = 0;
-  arg2 = 0;
-  arg = 0;
-  index = 0;
-}
-
-/* Run a command.  Commands are defined in commands.h */
-int runCommand() {
-  int i = 0;
-  char *p = argv1;
-  char *str;
-  int pid_args[4];
-  arg1 = atoi(argv1);
-  arg2 = atoi(argv2);
-  
-  switch(cmd) {
-  case GET_BAUDRATE:
-    Serial.println(BAUDRATE);
-    break;
-  case ANALOG_READ:
-    Serial.println(analogRead(arg1));
-    break;
-  case DIGITAL_READ:
-    Serial.println(digitalRead(arg1));
-    break;
-  case ANALOG_WRITE:
-    analogWrite(arg1, arg2);
-    Serial.println("OK"); 
-    break;
-  case DIGITAL_WRITE:
-    if (arg2 == 0) digitalWrite(arg1, LOW);
-    else if (arg2 == 1) digitalWrite(arg1, HIGH);
-    Serial.println("OK"); 
-    break;
-  case PIN_MODE:
-    if (arg2 == 0) pinMode(arg1, INPUT);
-    else if (arg2 == 1) pinMode(arg1, OUTPUT);
-    Serial.println("OK");
-    break;
-  case PING:
-    Serial.println(Ping(arg1));
-    break;
-#ifdef USE_SERVOS
-  case SERVO_WRITE:
-    servos[arg1].write(arg2);
-    Serial.println("OK");
-    break;
-  case SERVO_READ:
-    Serial.println(servos[arg1].read());
-    break;
-#endif
-    
-#ifdef USE_BASE
-  case READ_ENCODERS:
-    Serial.print(readEncoder(LEFT));
-    Serial.print(" ");
-    Serial.println(readEncoder(RIGHT));
-    break;
-   case RESET_ENCODERS:
-    resetEncoders();
-    Serial.println("OK");
-    break;
-  case MOTOR_SPEEDS:
-    /* Reset the auto stop timer */
-    lastMotorCommand = millis();
-    if (arg1 == 0 && arg2 == 0) {
-      setMotorSpeeds(0, 0);
-      moving = 0;
-    }
-    else moving = 1;
-    leftPID.TargetTicksPerFrame = arg1;
-    rightPID.TargetTicksPerFrame = arg2;
-    Serial.println("OK"); 
-    break;
-  case UPDATE_PID:
-    while ((str = strtok_r(p, ":", &p)) != '\0') {
-       pid_args[i] = atoi(str);
-       i++;
-    }
-    Kp = pid_args[0];
-    Kd = pid_args[1];
-    Ki = pid_args[2];
-    Ko = pid_args[3];
-    Serial.println("OK");
-    break;
-#endif
-  default:
-    Serial.println("Invalid Command");
-    break;
-  }
-}
-
-/* Setup function--runs once at startup. */
-void setup() {
-  Serial.begin(BAUDRATE);
-
-// Initialize the motor controller if used */
-#ifdef USE_BASE
-  initMotorController();
-#endif
-
-/* Attach servos if used */
-#ifdef USE_SERVOS
-  int i;
-  for (i = 0; i < N_SERVOS; i++) {
-    servos[i].attach(servoPins[i]);
-  }
-#endif
-}
-
-/* Enter the main loop.  Read and parse input from the serial port
-   and run any valid commands. Run a PID calculation at the target
-   interval and check for auto-stop conditions.
-*/
-void loop() {
-  while (Serial.available() > 0) {
-    
-    // Read the next character
-    chr = Serial.read();
-
-    // Terminate a command with a CR
-    if (chr == 13) {
-      if (arg == 1) argv1[index] = NULL;
-      else if (arg == 2) argv2[index] = NULL;
-      runCommand();
-      resetCommand();
-    }
-    // Use spaces to delimit parts of the command
-    else if (chr == ' ') {
-      // Step through the arguments
-      if (arg == 0) arg = 1;
-      else if (arg == 1)  {
-        argv1[index] = NULL;
-        arg = 2;
-        index = 0;
-      }
-      continue;
-    }
-    else {
-      if (arg == 0) {
-        // The first arg is the single-letter command
-        cmd = chr;
-      }
-      else if (arg == 1) {
-        // Subsequent arguments can be more than one character
-        argv1[index] = chr;
-        index++;
-      }
-      else if (arg == 2) {
-        argv2[index] = chr;
-        index++;
-      }
-    }
-  }
-  
-// If we are using base control, run a PID calculation at the appropriate intervals
-#ifdef USE_BASE
-  if (millis() > nextPID) {
-    updatePID();
-    nextPID += PID_INTERVAL;
-  }
-  
-  // Check to see if we have exceeded the auto-stop interval
-  if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {;
-    setMotorSpeeds(0, 0);
-    moving = 0;
-  }
-
-#endif
-}
-
-
-
-
-
-
diff --git a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/commands.h b/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/commands.h
deleted file mode 100644
index 18ff0ab..0000000
--- a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/commands.h
+++ /dev/null
@@ -1,22 +0,0 @@
-/* Define single-letter commands that will be sent by the PC over the
-   serial link.
-*/
-
-#define ANALOG_READ    'a'
-#define GET_BAUDRATE   'b'
-#define PIN_MODE       'c'
-#define DIGITAL_READ   'd'
-#define READ_ENCODERS  'e'
-#define MOTOR_SPEEDS   'm'
-#define PING           'p'
-#define RESET_ENCODERS 'r'
-#define SERVO_WRITE    's'
-#define SERVO_READ     't'
-#define UPDATE_PID     'u'
-#define DIGITAL_WRITE  'w'
-#define ANALOG_WRITE   'x'
-#define LEFT            0
-#define RIGHT           1
-
-
-
diff --git a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/diff_controller.h b/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/diff_controller.h
deleted file mode 100644
index 0284681..0000000
--- a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/diff_controller.h
+++ /dev/null
@@ -1,71 +0,0 @@
-/* Functions and type-defs for PID control.
-
-   Taken mostly from Mike Ferguson's ArbotiX code which lives at:
-   
-   http://vanadium-ros-pkg.googlecode.com/svn/trunk/arbotix/
-*/
-
-/* PID setpoint info For a Motor */
-typedef struct {
-  double TargetTicksPerFrame;    // target speed in ticks per frame
-  long Encoder;                  // encoder count
-  long PrevEnc;                  // last encoder count
-  int PrevErr;                   // last error
-  int Ierror;                    // integrated error
-  int output;                    // last motor setting
-}
-SetPointInfo;
-
-SetPointInfo leftPID, rightPID;
-
-/* PID Parameters */
-int Kp = 20;
-int Kd = 12;
-int Ki = 0;
-int Ko = 50;
-
-unsigned char moving = 0; // is the base in motion?
-
-/* PID routine to compute the next motor commands */
-void doPID(SetPointInfo * p) {
-  long Perror;
-  long output;
-
-  Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc);
-
-  // Derivative error is the delta Perror
-  output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko;
-  p->PrevErr = Perror;
-  p->PrevEnc = p->Encoder;
-
-  output += p->output;
-  // Accumulate Integral error *or* Limit output.
-  // Stop accumulating when output saturates
-  if (output >= MAX_PWM)
-    output = MAX_PWM;
-  else if (output <= -MAX_PWM)
-    output = -MAX_PWM;
-  else
-    p->Ierror += Perror;
-
-  p->output = output;
-}
-
-/* Read the encoder values and call the PID routine */
-void updatePID() {
-  /* Read the encoders */
-  leftPID.Encoder = readEncoder(0);
-  rightPID.Encoder = readEncoder(1);
-  
-  /* If we're not moving there is nothing more to do */
-  if (!moving)
-    return;
-
-  /* Compute PID update for each motor */
-  doPID(&rightPID);
-  doPID(&leftPID);
-
-  /* Set the motor speeds accordingly */
-  setMotorSpeeds(leftPID.output, rightPID.output);
-}
-
diff --git a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/sensors.h b/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/sensors.h
deleted file mode 100644
index 75caeb7..0000000
--- a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/sensors.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/* Functions for various sensor types */
-
-float microsecondsToCm(long microseconds)
-{
-  // The speed of sound is 340 m/s or 29 microseconds per cm.
-  // The ping travels out and back, so to find the distance of the
-  // object we take half of the distance travelled.
-  return microseconds / 29 / 2;
-}
-
-long Ping(int pin) {
-  long duration, range;
-
-  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
-  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
-  pinMode(pin, OUTPUT);
-  digitalWrite(pin, LOW);
-  delayMicroseconds(2);
-  digitalWrite(pin, HIGH);
-  delayMicroseconds(5);
-  digitalWrite(pin, LOW);
-
-  // The same pin is used to read the signal from the PING))): a HIGH
-  // pulse whose duration is the time (in microseconds) from the sending
-  // of the ping to the reception of its echo off of an object.
-  pinMode(pin, INPUT);
-  duration = pulseIn(pin, HIGH);
-
-  // convert the time into meters
-  range = microsecondsToCm(duration);
-  
-  return(range);
-}
-
diff --git a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/servos.h b/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/servos.h
deleted file mode 100644
index 6100723..0000000
--- a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/servos.h
+++ /dev/null
@@ -1,16 +0,0 @@
-/* Define the attachment of any servos here.
-   The example shows two servos attached on pins 3 and 5.
-*/
-
-#define N_SERVOS 2
-
-Servo servos [N_SERVOS];
-byte servoPins [N_SERVOS] = {3, 5};
-
-
-
-
-
-
-
-
diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino
index 1464a46..dbe1db4 100644
--- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino
+++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino
@@ -50,13 +50,22 @@
 /* Define the motor controller and encoder library you are using */
 #ifdef USE_BASE
    /* The Pololu VNH5019 dual motor driver shield */
-   #define POLOLU_VNH5019
+   //#define POLOLU_VNH5019
 
    /* The Pololu MC33926 dual motor driver shield */
    //#define POLOLU_MC33926
 
+   /* The Ardunino Motor Shield R3 */
+   //#define ARDUINO_MOTOR_SHIELD_R3
+
+   /* For testing only */
+   //#define NO_MOTOR_CONTROLLER
+
    /* The RoboGaia encoder shield */
-   #define ROBOGAIA
+   //#define ROBOGAIA
+
+   /* The RoboGaia 3-axis encoder shield */
+   //#define ROBOGAIA_3_AXIS
    
    /* Encoders directly attached to Arduino board */
    //#define ARDUINO_ENC_COUNTER
@@ -94,6 +103,11 @@
   /* Motor driver function definitions */
   #include "motor_driver.h"
 
+  #ifdef ROBOGAIA_3_AXIS
+    // The sensor communicates using SPI, so include the library:
+    #include <SPI.h>
+  #endif
+
   /* Encoder driver function definitions */
   #include "encoder_driver.h"
 
@@ -193,15 +207,18 @@ int runCommand() {
     break;
 #elif defined(USE_SERVOS2)
   case CONFIG_SERVO:
-    myServos[arg1].initServo(arg1, arg2);
     if (!haveServo(arg1)) {
+      myServos[arg1].initServo(arg1, arg2);
       myServoPins[nServos] = arg1;
+      myServos[arg1].enable();
       nServos++;
     }
     Serial.println("OK");
     break;
   case SERVO_WRITE:
-    myServos[arg1].setTargetPosition(arg2);
+    if (myServos[arg1].isEnabled()) {
+      myServos[arg1].setTargetPosition(arg2);
+    }
     Serial.println("OK");
     break;
   case SERVO_READ:
@@ -213,10 +230,19 @@ int runCommand() {
     break;
   case DETACH_SERVO:
     myServos[arg1].getServo().detach();
+    myServos[arg1].disable();
     Serial.println("OK");
     break;
   case ATTACH_SERVO:
-    myServos[arg1].getServo().attach(arg1);
+    if (!haveServo(arg1)) {
+      myServos[arg1].initServo(arg1, 0);
+      myServoPins[nServos] = arg1;
+      nServos++;
+    }
+    else {
+      myServos[arg1].getServo().attach(arg1);  
+    }
+    myServos[arg1].enable();
     Serial.println("OK");
     break;
 
@@ -270,27 +296,8 @@ void setup() {
 
   // Initialize the motor controller if used */
   #ifdef USE_BASE
-    #ifdef ARDUINO_ENC_COUNTER
-      //set as inputs
-      DDRD &= ~(1<<LEFT_ENC_PIN_A);
-      DDRD &= ~(1<<LEFT_ENC_PIN_B);
-      DDRC &= ~(1<<RIGHT_ENC_PIN_A);
-      DDRC &= ~(1<<RIGHT_ENC_PIN_B);
-    
-      // Enable pull up resistors
-      PORTD |= (1<<LEFT_ENC_PIN_A);
-      PORTD |= (1<<LEFT_ENC_PIN_B);
-      PORTC |= (1<<RIGHT_ENC_PIN_A);
-      PORTC |= (1<<RIGHT_ENC_PIN_B);
-    
-      // Tell pin change mask to listen to left encoder pins
-      PCMSK2 |= (1 << LEFT_ENC_PIN_A)|(1 << LEFT_ENC_PIN_B);
-      // Tell pin change mask to listen to right encoder pins
-      PCMSK1 |= (1 << RIGHT_ENC_PIN_A)|(1 << RIGHT_ENC_PIN_B);
-    
-      // Enable PCINT1 and PCINT2 interrupt in the general interrupt mask
-      PCICR |= (1 << PCIE1) | (1 << PCIE2);
-    #endif
+    /* Initialize the encoder interface */
+    initEncoders();
     initMotorController();
     resetPID();
   #endif
diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.h
index 8d35ab1..45e8aba 100644
--- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.h
+++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.h
@@ -11,10 +11,21 @@
   
   //below can be changed, but should be PORTC pins
   #define RIGHT_ENC_PIN_A PC4  //pin A4
-  #define RIGHT_ENC_PIN_B PC5   //pin A5
+  #define RIGHT_ENC_PIN_B PC5  //pin A5
 #endif
-   
+
+#ifdef ROBOGAIA_3_AXIS
+  #define chipSelectPin1 10
+  #define chipSelectPin2 9
+  #define chipSelectPin3 8
+
+#define CNTR B00100000
+#define CLR B00000000
+#endif
+
+void initEncoders();
 long readEncoder(int i);
 void resetEncoder(int i);
 void resetEncoders();
 
+
diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino
index ef2b8b1..df4fb55 100644
--- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino
+++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino
@@ -22,11 +22,149 @@
     else return encoders.XAxisGetCount();
   }
 
+  /* Wrap the encoder initializeing to be run in the main setup() function */
+  void initEncoders() {
+    /* Nothing to do here */
+  }
+
   /* Wrap the encoder reset function */
   void resetEncoder(int i) {
     if (i == LEFT) return encoders.YAxisReset();
     else return encoders.XAxisReset();
   }
+#elif defined(ROBOGAIA_3_AXIS)
+  /* Robogaia 3-axis encoder shield */
+
+  //*****************************************************  
+  void initEncoders()
+  //*****************************************************   
+  {
+      pinMode(chipSelectPin1, OUTPUT);
+      pinMode(chipSelectPin2, OUTPUT);
+      pinMode(chipSelectPin3, OUTPUT);
+  
+      digitalWrite(chipSelectPin1, HIGH);
+      digitalWrite(chipSelectPin2, HIGH);
+      digitalWrite(chipSelectPin3, HIGH);
+ 
+      LS7366_Init();
+      
+      delay(100); 
+  }
+  
+  long readEncoder(int encoder)
+  //*****************************************************
+  {
+      unsigned int count1Value, count2Value, count3Value, count4Value;
+      long result;
+
+      /* The 3-axis encoder uses a 1-based index */
+      if (encoder == 2) encoder = 3;
+      if (encoder == 1) encoder = 2;
+      if (encoder == 0) encoder = 1;
+      
+      selectEncoder(encoder);
+      
+      SPI.transfer(0x60); // Request count
+      count1Value = SPI.transfer(0x00); // Read highest order byte
+      count2Value = SPI.transfer(0x00);
+      count3Value = SPI.transfer(0x00);
+      count4Value = SPI.transfer(0x00); // Read lowest order byte
+      
+      deselectEncoder(encoder);
+     
+      result= ((long)count1Value<<24) + ((long)count2Value<<16) + ((long)count3Value<<8) + (long)count4Value;
+      
+      return result;
+  }//end func
+  
+  //*************************************************
+  void selectEncoder(int encoder)
+  //*************************************************
+  {
+    switch(encoder)
+    {
+       case 1:
+          digitalWrite(chipSelectPin1,LOW);
+          break;
+       case 2:
+         digitalWrite(chipSelectPin2,LOW);
+         break;
+       case 3:
+         digitalWrite(chipSelectPin3,LOW);
+         break;    
+    }//end switch
+    
+  }//end func
+  
+  //*************************************************
+  void deselectEncoder(int encoder)
+  //*************************************************
+  {
+    switch(encoder)
+    {
+       case 1:
+          digitalWrite(chipSelectPin1,HIGH);
+          break;
+       case 2:
+         digitalWrite(chipSelectPin2,HIGH);
+         break;
+       case 3:
+         digitalWrite(chipSelectPin3,HIGH);
+         break;    
+    }//end switch
+    
+  }//end func
+
+  //*************************************************
+  void resetEncoder(int encoder)
+  //*************************************************
+  {
+    /* The 3-axis encoder uses a 1-based index */
+    if (encoder == 2) encoder = 3;
+    if (encoder == 1) encoder = 2;
+    if (encoder == 0) encoder = 1;
+    
+    selectEncoder(encoder);
+    SPI.transfer(CLR | CNTR);
+    deselectEncoder(encoder);
+  }
+
+  /* Wrap the encoder reset function */
+  void resetEncoders() {
+    resetEncoder(LEFT);
+    resetEncoder(RIGHT);
+  }
+  
+  // LS7366 Initialization and configuration
+  //*************************************************
+  void LS7366_Init(void)
+  //*************************************************
+  {
+     // SPI initialization
+     SPI.begin();
+     SPI.setClockDivider(SPI_CLOCK_DIV16);      // SPI at 1Mhz (on 16Mhz clock)
+     delay(10);
+     
+     digitalWrite(chipSelectPin1,LOW);
+     SPI.transfer(0x88); 
+     SPI.transfer(0x03);
+     digitalWrite(chipSelectPin1,HIGH); 
+     
+     
+     digitalWrite(chipSelectPin2,LOW);
+     SPI.transfer(0x88); 
+     SPI.transfer(0x03);
+     digitalWrite(chipSelectPin2,HIGH); 
+     
+     
+     digitalWrite(chipSelectPin3,LOW);
+     SPI.transfer(0x88); 
+     SPI.transfer(0x03);
+     digitalWrite(chipSelectPin3,HIGH); 
+     
+  }//end func
+  
 #elif defined(ARDUINO_ENC_COUNTER)
   volatile long left_enc_pos = 0L;
   volatile long right_enc_pos = 0L;
@@ -51,6 +189,29 @@
   
   	right_enc_pos += ENC_STATES[(enc_last & 0x0f)];
   }
+
+  /* Wrap the encoder initializeing to be run in the main setup() function */
+  void initEncoders() {
+    //set as inputs
+    DDRD &= ~(1<<LEFT_ENC_PIN_A);
+    DDRD &= ~(1<<LEFT_ENC_PIN_B);
+    DDRC &= ~(1<<RIGHT_ENC_PIN_A);
+    DDRC &= ~(1<<RIGHT_ENC_PIN_B);
+  
+    // Enable pull up resistors
+    PORTD |= (1<<LEFT_ENC_PIN_A);
+    PORTD |= (1<<LEFT_ENC_PIN_B);
+    PORTC |= (1<<RIGHT_ENC_PIN_A);
+    PORTC |= (1<<RIGHT_ENC_PIN_B);
+  
+    // Tell pin change mask to listen to left encoder pins
+    PCMSK2 |= (1 << LEFT_ENC_PIN_A)|(1 << LEFT_ENC_PIN_B);
+    // Tell pin change mask to listen to right encoder pins
+    PCMSK1 |= (1 << RIGHT_ENC_PIN_A)|(1 << RIGHT_ENC_PIN_B);
+  
+    // Enable PCINT1 and PCINT2 interrupt in the general interrupt mask
+    PCICR |= (1 << PCIE1) | (1 << PCIE2);
+  }
   
   /* Wrap the encoder reading function */
   long readEncoder(int i) {
@@ -68,15 +229,16 @@
       return;
     }
   }
+
+  /* Wrap the encoder reset function */
+  void resetEncoders() {
+    resetEncoder(LEFT);
+    resetEncoder(RIGHT);
+  }
 #else
   #error A encoder driver must be selected!
 #endif
 
-/* Wrap the encoder reset function */
-void resetEncoders() {
-  resetEncoder(LEFT);
-  resetEncoder(RIGHT);
-}
-
 #endif
 
+
diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h
index d2b6d21..5957834 100644
--- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h
+++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h
@@ -42,3 +42,4 @@ class SweepServo
 SweepServo servos [N_SERVOS];
 
 #endif
+
diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.ino
index 5ba344c..7f92a6f 100644
--- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.ino
+++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.ino
@@ -76,3 +76,4 @@ Servo SweepServo::getServo()
 
 
 #endif
+
diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos2.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos2.h
index e6e0dc3..3bbd64d 100644
--- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos2.h
+++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos2.h
@@ -17,25 +17,29 @@ class SweepServo2
     
     void initServo(
         int servoPin,
-        int stepDelayMs);
+        unsigned long stepDelayMs);
 
      void setServoDelay(
         int servoPin,
-        int stepDelayMs);
+        unsigned long stepDelayMs);
         
     void moveServo();
     void setTargetPosition(int position);
     void setServoDelay(int delay);
     int getCurrentPosition();
-
+    void enable();
+    void disable();
+    bool isEnabled();
+    
     Servo getServo();
 
   private:
     Servo servo;
-    int stepDelayMs;
+    unsigned long stepDelayMs;
     int currentPositionDegrees;
     int targetPositionDegrees;
-    long lastSweepCommand;
+    unsigned long lastSweepCommand;
+    bool enabled;
 };
 
 SweepServo2 myServos [MAX_N_SERVOS];
@@ -43,3 +47,4 @@ SweepServo2 myServos [MAX_N_SERVOS];
 int myServoPins [MAX_N_SERVOS];
 
 #endif
+
diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos2.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos2.ino
index 5cd0afb..7eb4161 100644
--- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos2.ino
+++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos2.ino
@@ -26,7 +26,7 @@ SweepServo2::SweepServo2()
 // Init
 void SweepServo2::initServo(
     int servoPin,
-    int stepDelayMs)
+    unsigned long stepDelayMs)
 {
   this->stepDelayMs = stepDelayMs;
   this->currentPositionDegrees = 90;
@@ -38,7 +38,7 @@ void SweepServo2::initServo(
 // Init
 void SweepServo2::setServoDelay(
     int servoPin,
-    int stepDelayMs)
+    unsigned long stepDelayMs)
 {
   this->stepDelayMs = stepDelayMs;
 }
@@ -48,7 +48,7 @@ void SweepServo2::moveServo()
 {
 
   // Get ellapsed time
-  int delta = millis() - this->lastSweepCommand;
+  unsigned long delta = millis() - this->lastSweepCommand;
 
   // Check if time for a step
   if (delta > this->stepDelayMs) {
@@ -80,13 +80,16 @@ int SweepServo2::getCurrentPosition()
   return this->currentPositionDegrees;
 }
 
-// Check whether we have already configured this servo
-bool haveServo(int pin) {
-  int i;
-  for (i = 0; i < nServos; i++) {
-    if (myServoPins[i] == pin) return true;
-  }
-  return false;
+void SweepServo2::enable() {
+    this->enabled = true;
+}
+
+void SweepServo2::disable() {
+    this->enabled = false;
+}
+
+bool SweepServo2::isEnabled() {
+    return this->enabled;
 }
 
 // Accessor for servo object
@@ -96,4 +99,14 @@ Servo SweepServo2::getServo()
 }
 
 
+// Check whether we have already configured this servo
+bool haveServo(int pin) {
+  int i;
+  for (i = 0; i < nServos; i++) {
+    if (myServoPins[i] == pin) return true;
+  }
+  return false;
+}
+
 #endif
+
diff --git a/ros_arduino_msgs/CMakeLists.txt b/ros_arduino_msgs/CMakeLists.txt
index 0f66e15..a2f8211 100644
--- a/ros_arduino_msgs/CMakeLists.txt
+++ b/ros_arduino_msgs/CMakeLists.txt
@@ -14,19 +14,23 @@ add_message_files(FILES
 add_service_files(FILES
                   AnalogWrite.srv
                   AnalogSensorWrite.srv
-                  AnalogFloatSensorWrite.srv              
+                  AnalogFloatSensorWrite.srv
+                  AnalogPinMode.srv           
                   AnalogRead.srv
                   AnalogSensorRead.srv
                   AnalogFloatSensorRead.srv
+                  DigitalPinMode.srv
                   DigitalRead.srv
                   DigitalSensorRead.srv
                   DigitalSetDirection.srv
-                  DigitalSensorSetDirection.srv
+                  DigitalSensorPinMode.srv
                   DigitalWrite.srv
                   DigitalSensorWrite.srv
                   Enable.srv
                   Relax.srv
                   AnalogSensorRead.srv
+                  ServoAttach.srv
+                  ServoDetach.srv
                   ServoRead.srv
                   ServoWrite.srv
                   SetSpeed.srv
diff --git a/ros_arduino_msgs/srv/AnalogPinMode.srv b/ros_arduino_msgs/srv/AnalogPinMode.srv
new file mode 100755
index 0000000..01a2e12
--- /dev/null
+++ b/ros_arduino_msgs/srv/AnalogPinMode.srv
@@ -0,0 +1,3 @@
+uint8 pin
+bool direction
+---
diff --git a/ros_arduino_msgs/srv/DigitalSetDirection.srv b/ros_arduino_msgs/srv/DigitalSetDirection.srv
new file mode 100755
index 0000000..01a2e12
--- /dev/null
+++ b/ros_arduino_msgs/srv/DigitalSetDirection.srv
@@ -0,0 +1,3 @@
+uint8 pin
+bool direction
+---
diff --git a/ros_arduino_msgs/srv/ServoAttach.srv b/ros_arduino_msgs/srv/ServoAttach.srv
new file mode 100755
index 0000000..9233d46
--- /dev/null
+++ b/ros_arduino_msgs/srv/ServoAttach.srv
@@ -0,0 +1,3 @@
+uint8 id
+---
+
diff --git a/ros_arduino_msgs/srv/ServoDetach.srv b/ros_arduino_msgs/srv/ServoDetach.srv
new file mode 100755
index 0000000..9233d46
--- /dev/null
+++ b/ros_arduino_msgs/srv/ServoDetach.srv
@@ -0,0 +1,3 @@
+uint8 id
+---
+
diff --git a/ros_arduino_msgs/srv/ServoRead.srv b/ros_arduino_msgs/srv/ServoRead.srv
index fd68ffa..9d4e6fd 100755
--- a/ros_arduino_msgs/srv/ServoRead.srv
+++ b/ros_arduino_msgs/srv/ServoRead.srv
@@ -1,3 +1,3 @@
 uint8 id
 ---
-float32 value
+uint8 position
diff --git a/ros_arduino_msgs/srv/ServoWrite.srv b/ros_arduino_msgs/srv/ServoWrite.srv
index 5a9170b..1c2473a 100755
--- a/ros_arduino_msgs/srv/ServoWrite.srv
+++ b/ros_arduino_msgs/srv/ServoWrite.srv
@@ -1,3 +1,3 @@
 uint8 id
-float32 value
+uint8 position
 ---
diff --git a/ros_arduino_msgs/srv/SetServoSpeed.srv b/ros_arduino_msgs/srv/SetServoSpeed.srv
index 5a9170b..667284e 100755
--- a/ros_arduino_msgs/srv/SetServoSpeed.srv
+++ b/ros_arduino_msgs/srv/SetServoSpeed.srv
@@ -1,3 +1,3 @@
 uint8 id
-float32 value
+float32 speed
 ---
diff --git a/ros_arduino_msgs/srv/SetSpeed.srv b/ros_arduino_msgs/srv/SetSpeed.srv
index effb167..ef7ce7d 100755
--- a/ros_arduino_msgs/srv/SetSpeed.srv
+++ b/ros_arduino_msgs/srv/SetSpeed.srv
@@ -1,2 +1,2 @@
-float32 value
+float32 speed
 ---
diff --git a/ros_arduino_python/examples/sweep_servo.py b/ros_arduino_python/examples/sweep_servo.py
index 72424f4..eb75df4 100755
--- a/ros_arduino_python/examples/sweep_servo.py
+++ b/ros_arduino_python/examples/sweep_servo.py
@@ -35,7 +35,7 @@ class SweepServo():
         self.node_name = "sweep_servo"
         
         # Initialize the node
-        rospy.init_node(self.node_name, anonymous=True)
+        rospy.init_node(self.node_name)
                 
         # Set a shutdown function to clean up when termniating the node
         rospy.on_shutdown(self.shutdown)
@@ -43,11 +43,10 @@ class SweepServo():
         # Name of the joint we want to control
         joint_name = rospy.get_param('~joint', 'head_pan_joint')
             
-        if joint_name == '':
+        if joint_name is None or joint_name == '':
             rospy.logino("Joint name for servo must be specified in parameter file.")
             os._exit(1)
             
-        servo_pin = rospy.get_param('/arduino/joints/' + str(joint_name) + '/pin')
         max_position = radians(rospy.get_param('/arduino/joints/' + str(joint_name) + '/max_position'))
         min_position = radians(rospy.get_param('/arduino/joints/' + str(joint_name) + '/min_position'))
         
@@ -55,11 +54,11 @@ class SweepServo():
         target_min = min_position
                 
         # How fast should we sweep the servo
-        speed = rospy.get_param('~speed', 1.0) # rad/s
-        
-        # Time between between sweeps
-        delay = rospy.get_param('~delay', 2)  # seconds
+        servo_speed = rospy.get_param('~servo_speed', 1.0) # rad/s
         
+        # Time delay between between sweeps
+        delay = rospy.get_param('~delay', 0)  # seconds
+                
         # Create a publisher for setting the joint position
         joint_pub = rospy.Publisher('/' + joint_name + '/command', Float64, queue_size=5)
         
@@ -88,7 +87,7 @@ class SweepServo():
         set_speed = rospy.ServiceProxy('/' + joint_name + '/set_speed', SetSpeed)
         
         # Set the initial servo speed
-        set_speed(speed)
+        set_speed(servo_speed)
 
         rospy.loginfo('Sweeping servo...')
         
@@ -121,5 +120,4 @@ if __name__ == '__main__':
         SweepServo()
     except: 
         rospy.loginfo('Unexpected error: ' +  str(sys.exc_info()[0]))
-        raise
-    
\ No newline at end of file
+        raise
\ No newline at end of file
diff --git a/ros_arduino_python/launch/sweep_servo.launch b/ros_arduino_python/launch/sweep_servo.launch
index 3ba78d0..adcab6f 100755
--- a/ros_arduino_python/launch/sweep_servo.launch
+++ b/ros_arduino_python/launch/sweep_servo.launch
@@ -1,7 +1,9 @@
 <launch>
    <arg name="joint" default="head_pan_joint" />
    
-   <node pkg="ros_arduino_python" name="$(anon sweep_servo)" type="sweep_servo.py" output="screen" clear_params="true">
+   <node pkg="rbx3_demo" name="sweep_servo" type="sweep_servo.py" output="screen" clear_params="true">
       <param name="joint" value="$(arg joint)" />
+      <param name="servo_speed" value="1.0" />
+      <param name="delay" value="0.0" />
    </node>
 </launch>