diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino
index 059b464..1464a46 100644
--- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino
+++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino
@@ -194,8 +194,10 @@ int runCommand() {
 #elif defined(USE_SERVOS2)
   case CONFIG_SERVO:
     myServos[arg1].initServo(arg1, arg2);
-    myServoPins[nServos] = arg1;
-    nServos++;
+    if (!haveServo(arg1)) {
+      myServoPins[nServos] = arg1;
+      nServos++;
+    }
     Serial.println("OK");
     break;
   case SERVO_WRITE:
diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos2.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos2.ino
index d8cf594..5cd0afb 100644
--- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos2.ino
+++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos2.ino
@@ -18,7 +18,7 @@
 // Constructor
 SweepServo2::SweepServo2()
 {
-  this->currentPositionDegrees = 0;
+  this->currentPositionDegrees = 90;
   this->targetPositionDegrees = 90; 
   this->lastSweepCommand = 0;
 }
@@ -29,7 +29,7 @@ void SweepServo2::initServo(
     int stepDelayMs)
 {
   this->stepDelayMs = stepDelayMs;
-  this->currentPositionDegrees = 0;
+  this->currentPositionDegrees = 90;
   this->targetPositionDegrees = 90;
   this->lastSweepCommand = millis();
   this->servo.attach(servoPin);
@@ -74,11 +74,21 @@ void SweepServo2::setTargetPosition(int position)
   this->targetPositionDegrees = position;
 }
 
+// Get the current servo position
 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;
+}
+
 // Accessor for servo object
 Servo SweepServo2::getServo()
 {