diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino
index 466dd19..5de3726 100644
--- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino
+++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino
@@ -265,12 +265,15 @@ void setup() {
 #endif
 
 /* Attach servos if used */
-#ifdef USE_SERVOS
-  int i;
-  for (i = 0; i < N_SERVOS; i++) {
-    servos[i].initServo(servoPins[i], servoInitPosition[i]);
-  }
-#endif
+  #ifdef USE_SERVOS
+    int i;
+    for (i = 0; i < N_SERVOS; i++) {
+      servos[i].initServo(
+          servoPins[i],
+          stepDelay[i],
+          servoInitPosition[i]);
+    }
+  #endif
 }
 
 /* Enter the main loop.  Read and parse input from the serial port
diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h
index f0f404e..d2b6d21 100644
--- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h
+++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h
@@ -2,29 +2,38 @@
 #define SERVOS_H
 
 
-#define N_SERVOS 1
+#define N_SERVOS 2
 
-// This delay in milliseconds determines determines the pause 
+// This delay in milliseconds determines the pause 
 // between each one degree step the servo travels.  Increasing 
 // this number will make the servo sweep more slowly.  
-// Decreasing this number will make the servo sweep more quickly.  
-#define SWEEP_COMMAND_INTERVAL 0 // ms
+// Decreasing this number will make the servo sweep more quickly.
+// Zero is the default number and will make the servos spin at
+// full speed.  150 ms makes them spin very slowly.
+int stepDelay [N_SERVOS] = { 0, 0 }; // ms
 
-byte servoPins [N_SERVOS] = { 3 };
-byte servoInitPosition [N_SERVOS] = { 90 }; // degrees
+// Pins
+byte servoPins [N_SERVOS] = { 3, 4 };
+
+// Initial Position
+byte servoInitPosition [N_SERVOS] = { 90, 90 }; // [0, 180] degrees
 
 
 class SweepServo
 {
   public:
     SweepServo();
-    void initServo(int servoPin, int initPosition);
+    void initServo(
+        int servoPin,
+        int stepDelayMs,
+        int initPosition);
     void doSweep();
     void setTargetPosition(int position);
     Servo getServo();
 
   private:
     Servo servo;
+    int stepDelayMs;
     int currentPositionDegrees;
     int targetPositionDegrees;
     long lastSweepCommand;
diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.ino
index fc68f60..5ba344c 100644
--- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.ino
+++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.ino
@@ -22,9 +22,13 @@ SweepServo::SweepServo()
 
 
 // Init
-void SweepServo::initServo(int servoPin, int initPosition)
+void SweepServo::initServo(
+    int servoPin,
+    int stepDelayMs,
+    int initPosition)
 {
   this->servo.attach(servoPin);
+  this->stepDelayMs = stepDelayMs;
   this->currentPositionDegrees = initPosition;
   this->targetPositionDegrees = initPosition;
   this->lastSweepCommand = millis();
@@ -39,7 +43,7 @@ void SweepServo::doSweep()
   int delta = millis() - this->lastSweepCommand;
 
   // Check if time for a step
-  if (delta > SWEEP_COMMAND_INTERVAL) {
+  if (delta > this->stepDelayMs) {
     // Check step direction
     if (this->targetPositionDegrees > this->currentPositionDegrees) {
       this->currentPositionDegrees++;